From 76801f72ac1dbe24a2a8bc76a878fd4a7261c00a Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Wed, 12 Mar 2014 16:39:58 +0100 Subject: [PATCH] first commit --- README.md | 137 ++ rqt_svo/CMakeLists.txt | 22 + rqt_svo/README.md | 10 + rqt_svo/package.xml | 25 + rqt_svo/plugin.xml | 12 + rqt_svo/resource/widget.ui | 101 + rqt_svo/scripts/rqt_svo | 10 + rqt_svo/setup.py | 12 + rqt_svo/src/rqt_svo/.gitignore | 1 + rqt_svo/src/rqt_svo/__init__.py | 0 rqt_svo/src/rqt_svo/svo.py | 66 + rqt_svo/src/rqt_svo/svo_widget.py | 126 ++ svo/CMakeLists.txt | 156 ++ svo/CMakeModules/FindG2O.cmake | 113 ++ svo/Doxyfile | 1782 +++++++++++++++++ svo/doc/.gitignore | 2 + svo/include/svo/bundle_adjustment.h | 103 + svo/include/svo/config.h | 159 ++ svo/include/svo/depth_filter.h | 163 ++ svo/include/svo/feature.h | 62 + svo/include/svo/feature_alignment.h | 63 + svo/include/svo/feature_detection.h | 76 + svo/include/svo/frame.h | 130 ++ svo/include/svo/frame_handler_base.h | 116 ++ svo/include/svo/frame_handler_mono.h | 75 + svo/include/svo/global.h | 95 + svo/include/svo/initialization.h | 83 + svo/include/svo/map.h | 134 ++ svo/include/svo/matcher.h | 105 + svo/include/svo/point.h | 83 + svo/include/svo/pose_optimizer.h | 77 + svo/include/svo/reprojection.h | 98 + svo/include/svo/sparse_img_align.h | 78 + svo/package.xml | 24 + svo/src/bundle_adjustment.cpp | 435 ++++ svo/src/config.cpp | 95 + svo/src/depth_filter.cpp | 362 ++++ svo/src/feature_alignment.cpp | 601 ++++++ svo/src/feature_detection.cpp | 77 + svo/src/frame.cpp | 197 ++ svo/src/frame_handler_base.cpp | 200 ++ svo/src/frame_handler_mono.cpp | 254 +++ svo/src/initialization.cpp | 205 ++ svo/src/map.cpp | 357 ++++ svo/src/matcher.cpp | 343 ++++ svo/src/point.cpp | 170 ++ svo/src/pose_optimizer.cpp | 164 ++ svo/src/reprojection.cpp | 214 ++ svo/src/sparse_img_align.cpp | 296 +++ svo/test/README.md | 11 + svo/test/benchmark.csv | 307 +++ svo/test/data/.gitignore | 2 + svo/test/results/.gitignore | 2 + svo/test/test_depth_filter.cpp | 200 ++ svo/test/test_feature_alignment.cpp | 122 ++ svo/test/test_feature_detection.cpp | 80 + svo/test/test_matcher.cpp | 195 ++ svo/test/test_pipeline.cpp | 94 + svo/test/test_pose_optimizer.cpp | 119 ++ svo/test/test_sparse_img_align.cpp | 165 ++ svo_analysis/benchmark.py | 67 + svo_analysis/evaluate.py | 47 + svo_analysis/experiments/asl2_acc.yaml | 2 + svo_analysis/experiments/asl2_fast.yaml | 2 + svo_analysis/results/.gitignore | 2 + svo_analysis/scripts/__init__.py | 2 + svo_analysis/scripts/analyse_dataset.py | 42 + svo_analysis/scripts/analyse_logs.py | 132 ++ svo_analysis/scripts/analyse_timing.py | 97 + svo_analysis/scripts/analyse_trajectory.py | 187 ++ svo_analysis/scripts/associate.py | 67 + svo_analysis/scripts/evaluate_rpe.py | 355 ++++ .../scripts/filter_groundtruth_smooth.py | 60 + svo_analysis/scripts/hand_eye_calib.py | 287 +++ svo_analysis/scripts/rotation_utils.py | 143 ++ svo_analysis/scripts/trajectory_comparison.py | 166 ++ svo_analysis/scripts/transformations.py | 1705 ++++++++++++++++ svo_msgs/CMakeLists.txt | 35 + svo_msgs/msg/DenseInput.msg | 6 + svo_msgs/msg/Feature.msg | 5 + svo_msgs/msg/Info.msg | 6 + svo_msgs/msg/NbvTrajectory.msg | 2 + svo_msgs/package.xml | 19 + svo_ros/CMakeLists.txt | 62 + svo_ros/include/svo_ros/dataset_img.h | 62 + svo_ros/include/svo_ros/dataset_imu.h | 70 + svo_ros/include/svo_ros/visualizer.h | 75 + svo_ros/launch/live.launch | 25 + svo_ros/launch/test_rig3.launch | 16 + svo_ros/package.xml | 42 + svo_ros/param/vo_accurate.yaml | 4 + svo_ros/param/vo_fast.yaml | 3 + svo_ros/rqt.perspective | 293 +++ svo_ros/rviz_config.rviz | 153 ++ svo_ros/src/benchmark_node.cpp | 155 ++ svo_ros/src/remote_node.cpp | 169 ++ svo_ros/src/visualizer.cpp | 268 +++ svo_ros/src/vo_node.cpp | 181 ++ 98 files changed, 14580 insertions(+) create mode 100644 README.md create mode 100644 rqt_svo/CMakeLists.txt create mode 100644 rqt_svo/README.md create mode 100644 rqt_svo/package.xml create mode 100644 rqt_svo/plugin.xml create mode 100644 rqt_svo/resource/widget.ui create mode 100755 rqt_svo/scripts/rqt_svo create mode 100644 rqt_svo/setup.py create mode 100644 rqt_svo/src/rqt_svo/.gitignore create mode 100644 rqt_svo/src/rqt_svo/__init__.py create mode 100644 rqt_svo/src/rqt_svo/svo.py create mode 100644 rqt_svo/src/rqt_svo/svo_widget.py create mode 100644 svo/CMakeLists.txt create mode 100644 svo/CMakeModules/FindG2O.cmake create mode 100644 svo/Doxyfile create mode 100644 svo/doc/.gitignore create mode 100644 svo/include/svo/bundle_adjustment.h create mode 100644 svo/include/svo/config.h create mode 100644 svo/include/svo/depth_filter.h create mode 100644 svo/include/svo/feature.h create mode 100644 svo/include/svo/feature_alignment.h create mode 100644 svo/include/svo/feature_detection.h create mode 100644 svo/include/svo/frame.h create mode 100644 svo/include/svo/frame_handler_base.h create mode 100644 svo/include/svo/frame_handler_mono.h create mode 100644 svo/include/svo/global.h create mode 100644 svo/include/svo/initialization.h create mode 100644 svo/include/svo/map.h create mode 100644 svo/include/svo/matcher.h create mode 100644 svo/include/svo/point.h create mode 100644 svo/include/svo/pose_optimizer.h create mode 100644 svo/include/svo/reprojection.h create mode 100644 svo/include/svo/sparse_img_align.h create mode 100644 svo/package.xml create mode 100644 svo/src/bundle_adjustment.cpp create mode 100644 svo/src/config.cpp create mode 100644 svo/src/depth_filter.cpp create mode 100644 svo/src/feature_alignment.cpp create mode 100644 svo/src/feature_detection.cpp create mode 100644 svo/src/frame.cpp create mode 100644 svo/src/frame_handler_base.cpp create mode 100644 svo/src/frame_handler_mono.cpp create mode 100644 svo/src/initialization.cpp create mode 100644 svo/src/map.cpp create mode 100644 svo/src/matcher.cpp create mode 100644 svo/src/point.cpp create mode 100644 svo/src/pose_optimizer.cpp create mode 100644 svo/src/reprojection.cpp create mode 100644 svo/src/sparse_img_align.cpp create mode 100644 svo/test/README.md create mode 100644 svo/test/benchmark.csv create mode 100644 svo/test/data/.gitignore create mode 100644 svo/test/results/.gitignore create mode 100644 svo/test/test_depth_filter.cpp create mode 100644 svo/test/test_feature_alignment.cpp create mode 100644 svo/test/test_feature_detection.cpp create mode 100644 svo/test/test_matcher.cpp create mode 100644 svo/test/test_pipeline.cpp create mode 100644 svo/test/test_pose_optimizer.cpp create mode 100644 svo/test/test_sparse_img_align.cpp create mode 100755 svo_analysis/benchmark.py create mode 100755 svo_analysis/evaluate.py create mode 100644 svo_analysis/experiments/asl2_acc.yaml create mode 100644 svo_analysis/experiments/asl2_fast.yaml create mode 100644 svo_analysis/results/.gitignore create mode 100644 svo_analysis/scripts/__init__.py create mode 100644 svo_analysis/scripts/analyse_dataset.py create mode 100755 svo_analysis/scripts/analyse_logs.py create mode 100755 svo_analysis/scripts/analyse_timing.py create mode 100644 svo_analysis/scripts/analyse_trajectory.py create mode 100644 svo_analysis/scripts/associate.py create mode 100755 svo_analysis/scripts/evaluate_rpe.py create mode 100644 svo_analysis/scripts/filter_groundtruth_smooth.py create mode 100644 svo_analysis/scripts/hand_eye_calib.py create mode 100644 svo_analysis/scripts/rotation_utils.py create mode 100644 svo_analysis/scripts/trajectory_comparison.py create mode 100644 svo_analysis/scripts/transformations.py create mode 100644 svo_msgs/CMakeLists.txt create mode 100644 svo_msgs/msg/DenseInput.msg create mode 100644 svo_msgs/msg/Feature.msg create mode 100644 svo_msgs/msg/Info.msg create mode 100644 svo_msgs/msg/NbvTrajectory.msg create mode 100644 svo_msgs/package.xml create mode 100644 svo_ros/CMakeLists.txt create mode 100644 svo_ros/include/svo_ros/dataset_img.h create mode 100644 svo_ros/include/svo_ros/dataset_imu.h create mode 100644 svo_ros/include/svo_ros/visualizer.h create mode 100644 svo_ros/launch/live.launch create mode 100644 svo_ros/launch/test_rig3.launch create mode 100644 svo_ros/package.xml create mode 100644 svo_ros/param/vo_accurate.yaml create mode 100644 svo_ros/param/vo_fast.yaml create mode 100644 svo_ros/rqt.perspective create mode 100644 svo_ros/rviz_config.rviz create mode 100644 svo_ros/src/benchmark_node.cpp create mode 100644 svo_ros/src/remote_node.cpp create mode 100644 svo_ros/src/visualizer.cpp create mode 100644 svo_ros/src/vo_node.cpp diff --git a/README.md b/README.md new file mode 100644 index 0000000..c3bc469 --- /dev/null +++ b/README.md @@ -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. diff --git a/rqt_svo/CMakeLists.txt b/rqt_svo/CMakeLists.txt new file mode 100644 index 0000000..b199493 --- /dev/null +++ b/rqt_svo/CMakeLists.txt @@ -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} +) \ No newline at end of file diff --git a/rqt_svo/README.md b/rqt_svo/README.md new file mode 100644 index 0000000..574a497 --- /dev/null +++ b/rqt_svo/README.md @@ -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 \ No newline at end of file diff --git a/rqt_svo/package.xml b/rqt_svo/package.xml new file mode 100644 index 0000000..862112a --- /dev/null +++ b/rqt_svo/package.xml @@ -0,0 +1,25 @@ + + + rqt_svo + 0.0.0 + The rqt_svo package + + cforster + + + TODO + + catkin + rospy + rqt_gui + rqt_gui_py + rospy + rqt_gui + rqt_gui_py + + + + + + + \ No newline at end of file diff --git a/rqt_svo/plugin.xml b/rqt_svo/plugin.xml new file mode 100644 index 0000000..fd6b605 --- /dev/null +++ b/rqt_svo/plugin.xml @@ -0,0 +1,12 @@ + + + + Plugin for SVO - Semi-Direct Visual Odometry + + + + system-help + Great user interface to provide real value. + + + \ No newline at end of file diff --git a/rqt_svo/resource/widget.ui b/rqt_svo/resource/widget.ui new file mode 100644 index 0000000..7808e37 --- /dev/null +++ b/rqt_svo/resource/widget.ui @@ -0,0 +1,101 @@ + + + SvoWidget + + + + 0 + 0 + 467 + 140 + + + + SVO + + + + + + + + SVO Namespace: + + + + + + + Topic to send Twist message to + + + + + + + + + + + + + + Current angular velocity + + + Status Message + + + + + + + + + + + 100 + + + 100 + + + %p% + + + + + + + + + + + Start + + + + + + + Reset velocities to zero + + + Reset + + + + + + + Quit + + + + + + + + + + diff --git a/rqt_svo/scripts/rqt_svo b/rqt_svo/scripts/rqt_svo new file mode 100755 index 0000000..753dcb5 --- /dev/null +++ b/rqt_svo/scripts/rqt_svo @@ -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 \ No newline at end of file diff --git a/rqt_svo/setup.py b/rqt_svo/setup.py new file mode 100644 index 0000000..e07a5df --- /dev/null +++ b/rqt_svo/setup.py @@ -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) \ No newline at end of file diff --git a/rqt_svo/src/rqt_svo/.gitignore b/rqt_svo/src/rqt_svo/.gitignore new file mode 100644 index 0000000..7e99e36 --- /dev/null +++ b/rqt_svo/src/rqt_svo/.gitignore @@ -0,0 +1 @@ +*.pyc \ No newline at end of file diff --git a/rqt_svo/src/rqt_svo/__init__.py b/rqt_svo/src/rqt_svo/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/rqt_svo/src/rqt_svo/svo.py b/rqt_svo/src/rqt_svo/svo.py new file mode 100644 index 0000000..88acaf7 --- /dev/null +++ b/rqt_svo/src/rqt_svo/svo.py @@ -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 diff --git a/rqt_svo/src/rqt_svo/svo_widget.py b/rqt_svo/src/rqt_svo/svo_widget.py new file mode 100644 index 0000000..fd8303b --- /dev/null +++ b/rqt_svo/src/rqt_svo/svo_widget.py @@ -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)) + + diff --git a/svo/CMakeLists.txt b/svo/CMakeLists.txt new file mode 100644 index 0000000..7612a94 --- /dev/null +++ b/svo/CMakeLists.txt @@ -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) diff --git a/svo/CMakeModules/FindG2O.cmake b/svo/CMakeModules/FindG2O.cmake new file mode 100644 index 0000000..655600f --- /dev/null +++ b/svo/CMakeModules/FindG2O.cmake @@ -0,0 +1,113 @@ +# Find the header files + +FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h + ${G2O_ROOT}/include + $ENV{G2O_ROOT}/include + $ENV{G2O_ROOT} + /usr/local/include + /usr/include + /opt/local/include + /sw/local/include + /sw/include + NO_DEFAULT_PATH + ) + +# Macro to unify finding both the debug and release versions of the +# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY +# macro. + +MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) + + FIND_LIBRARY("${MYLIBRARY}_DEBUG" + NAMES "g2o_${MYLIBRARYNAME}_d" + PATHS + ${G2O_ROOT}/lib/Debug + ${G2O_ROOT}/lib + $ENV{G2O_ROOT}/lib/Debug + $ENV{G2O_ROOT}/lib + NO_DEFAULT_PATH + ) + + FIND_LIBRARY("${MYLIBRARY}_DEBUG" + NAMES "g2o_${MYLIBRARYNAME}_d" + PATHS + ~/Library/Frameworks + /Library/Frameworks + /usr/local/lib + /usr/local/lib64 + /usr/lib + /usr/lib64 + /opt/local/lib + /sw/local/lib + /sw/lib + ) + + FIND_LIBRARY(${MYLIBRARY} + NAMES "g2o_${MYLIBRARYNAME}" + PATHS + ${G2O_ROOT}/lib/Release + ${G2O_ROOT}/lib + $ENV{G2O_ROOT}/lib/Release + $ENV{G2O_ROOT}/lib + NO_DEFAULT_PATH + ) + + FIND_LIBRARY(${MYLIBRARY} + NAMES "g2o_${MYLIBRARYNAME}" + PATHS + ~/Library/Frameworks + /Library/Frameworks + /usr/local/lib + /usr/local/lib64 + /usr/lib + /usr/lib64 + /opt/local/lib + /sw/local/lib + /sw/lib + ) + + IF(NOT ${MYLIBRARY}_DEBUG) + IF(MYLIBRARY) + SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) + ENDIF(MYLIBRARY) + ENDIF( NOT ${MYLIBRARY}_DEBUG) + +ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) + +# Find the core elements +FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) +FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) + +# Find the CLI library +FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) + +# Find the pluggable solvers +FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) +FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) +FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) +FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) +FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) +FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) +FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) +FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) + +# Find the predefined types +FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) +FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) +FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) +FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) +FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) +FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) +FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) + +# G2O solvers declared found if we found at least one solver +SET(G2O_SOLVERS_FOUND "NO") +IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) + SET(G2O_SOLVERS_FOUND "YES") +ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) + +# G2O itself declared found if we found the core libraries and at least one solver +SET(G2O_FOUND "NO") +IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) + SET(G2O_FOUND "YES") +ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) diff --git a/svo/Doxyfile b/svo/Doxyfile new file mode 100644 index 0000000..fb60d29 --- /dev/null +++ b/svo/Doxyfile @@ -0,0 +1,1782 @@ +# Doxyfile 1.7.6.1 + +# This file describes the settings to be used by the documentation system +# doxygen (www.doxygen.org) for a project. +# +# All text after a hash (#) is considered a comment and will be ignored. +# The format is: +# TAG = value [value, ...] +# For lists items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (" "). + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- + +# This tag specifies the encoding used for all characters in the config file +# that follow. The default is UTF-8 which is also the encoding used for all +# text before the first occurrence of this tag. Doxygen uses libiconv (or the +# iconv built into libc) for the transcoding. See +# http://www.gnu.org/software/libiconv for the list of possible encodings. + +DOXYFILE_ENCODING = UTF-8 + +# The PROJECT_NAME tag is a single word (or sequence of words) that should +# identify the project. Note that if you do not use Doxywizard you need +# to put quotes around the project name if it contains spaces. + +PROJECT_NAME = "svo" + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. +# This could be handy for archiving the generated documentation or +# if some version control system is used. + +PROJECT_NUMBER = + +# Using the PROJECT_BRIEF tag one can provide an optional one line description +# for a project that appears at the top of each page and should give viewer +# a quick idea about the purpose of the project. Keep the description short. + +PROJECT_BRIEF = "Semi-Direct Visual Odometry" + +# With the PROJECT_LOGO tag one can specify an logo or icon that is +# included in the documentation. The maximum height of the logo should not +# exceed 55 pixels and the maximum width should not exceed 200 pixels. +# Doxygen will copy the logo to the output directory. + +PROJECT_LOGO = + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) +# base path where the generated documentation will be put. +# If a relative path is entered, it will be relative to the location +# where doxygen was started. If left blank the current directory will be used. + +OUTPUT_DIRECTORY = "doc" + +# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create +# 4096 sub-directories (in 2 levels) under the output directory of each output +# format and will distribute the generated files over these directories. +# Enabling this option can be useful when feeding doxygen a huge amount of +# source files, where putting all generated files in the same directory would +# otherwise cause performance problems for the file system. + +CREATE_SUBDIRS = NO + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# The default language is English, other supported languages are: +# Afrikaans, Arabic, Brazilian, Catalan, Chinese, Chinese-Traditional, +# Croatian, Czech, Danish, Dutch, Esperanto, Farsi, Finnish, French, German, +# Greek, Hungarian, Italian, Japanese, Japanese-en (Japanese with English +# messages), Korean, Korean-en, Lithuanian, Norwegian, Macedonian, Persian, +# Polish, Portuguese, Romanian, Russian, Serbian, Serbian-Cyrillic, Slovak, +# Slovene, Spanish, Swedish, Ukrainian, and Vietnamese. + +OUTPUT_LANGUAGE = English + +# If the BRIEF_MEMBER_DESC tag is set to YES (the default) Doxygen will +# include brief member descriptions after the members that are listed in +# the file and class documentation (similar to JavaDoc). +# Set to NO to disable this. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES (the default) Doxygen will prepend +# the brief description of a member or function before the detailed description. +# Note: if both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. + +REPEAT_BRIEF = YES + +# This tag implements a quasi-intelligent brief description abbreviator +# that is used to form the text in various listings. Each string +# in this list, if found as the leading text of the brief description, will be +# stripped from the text and the result after processing the whole list, is +# used as the annotated text. Otherwise, the brief description is used as-is. +# If left blank, the following values are used ("$name" is automatically +# replaced with the name of the entity): "The $name class" "The $name widget" +# "The $name file" "is" "provides" "specifies" "contains" +# "represents" "a" "an" "the" + +ABBREVIATE_BRIEF = + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# Doxygen will generate a detailed section even if there is only a brief +# description. + +ALWAYS_DETAILED_SEC = NO + +# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# inherited members of a class in the documentation of that class as if those +# members were ordinary class members. Constructors, destructors and assignment +# operators of the base classes will not be shown. + +INLINE_INHERITED_MEMB = NO + +# If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full +# path before files name in the file list and in the header files. If set +# to NO the shortest path that makes the file name unique will be used. + +FULL_PATH_NAMES = YES + +# If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag +# can be used to strip a user-defined part of the path. Stripping is +# only done if one of the specified strings matches the left-hand part of +# the path. The tag can be used to show relative paths in the file list. +# If left blank the directory from which doxygen is run is used as the +# path to strip. + +STRIP_FROM_PATH = + +# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of +# the path mentioned in the documentation of a class, which tells +# the reader which header file to include in order to use a class. +# If left blank only the name of the header file containing the class +# definition is used. Otherwise one should specify the include paths that +# are normally passed to the compiler using the -I flag. + +STRIP_FROM_INC_PATH = + +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter +# (but less readable) file names. This can be useful if your file system +# doesn't support long names like on DOS, Mac, or CD-ROM. + +SHORT_NAMES = NO + +# If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen +# will interpret the first line (until the first dot) of a JavaDoc-style +# comment as the brief description. If set to NO, the JavaDoc +# comments will behave just like regular Qt-style comments +# (thus requiring an explicit @brief command for a brief description.) + +JAVADOC_AUTOBRIEF = YES + +# If the QT_AUTOBRIEF tag is set to YES then Doxygen will +# interpret the first line (until the first dot) of a Qt-style +# comment as the brief description. If set to NO, the comments +# will behave just like regular Qt-style comments (thus requiring +# an explicit \brief command for a brief description.) + +QT_AUTOBRIEF = NO + +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make Doxygen +# treat a multi-line C++ special comment block (i.e. a block of //! or /// +# comments) as a brief description. This used to be the default behaviour. +# The new default is to treat a multi-line C++ comment block as a detailed +# description. Set this tag to YES if you prefer the old behaviour instead. + +MULTILINE_CPP_IS_BRIEF = NO + +# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented +# member inherits the documentation from any documented member that it +# re-implements. + +INHERIT_DOCS = YES + +# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce +# a new page for each member. If set to NO, the documentation of a member will +# be part of the file/class/namespace that contains it. + +SEPARATE_MEMBER_PAGES = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. +# Doxygen uses this value to replace tabs by spaces in code fragments. + +TAB_SIZE = 8 + +# This tag can be used to specify a number of aliases that acts +# as commands in the documentation. An alias has the form "name=value". +# For example adding "sideeffect=\par Side Effects:\n" will allow you to +# put the command \sideeffect (or @sideeffect) in the documentation, which +# will result in a user-defined paragraph with heading "Side Effects:". +# You can put \n's in the value part of an alias to insert newlines. + +ALIASES = + +# This tag can be used to specify a number of word-keyword mappings (TCL only). +# A mapping has the form "name=value". For example adding +# "class=itcl::class" will allow you to use the command class in the +# itcl::class meaning. + +TCL_SUBST = + +# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C +# sources only. Doxygen will then generate output that is more tailored for C. +# For instance, some of the names that are used will be different. The list +# of all members will be omitted, etc. + +OPTIMIZE_OUTPUT_FOR_C = NO + +# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java +# sources only. Doxygen will then generate output that is more tailored for +# Java. For instance, namespaces will be presented as packages, qualified +# scopes will look different, etc. + +OPTIMIZE_OUTPUT_JAVA = NO + +# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran +# sources only. Doxygen will then generate output that is more tailored for +# Fortran. + +OPTIMIZE_FOR_FORTRAN = NO + +# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL +# sources. Doxygen will then generate output that is tailored for +# VHDL. + +OPTIMIZE_OUTPUT_VHDL = NO + +# Doxygen selects the parser to use depending on the extension of the files it +# parses. With this tag you can assign which parser to use for a given extension. +# Doxygen has a built-in mapping, but you can override or extend it using this +# tag. The format is ext=language, where ext is a file extension, and language +# is one of the parsers supported by doxygen: IDL, Java, Javascript, CSharp, C, +# C++, D, PHP, Objective-C, Python, Fortran, VHDL, C, C++. For instance to make +# doxygen treat .inc files as Fortran files (default is PHP), and .f files as C +# (default is Fortran), use: inc=Fortran f=C. Note that for custom extensions +# you also need to set FILE_PATTERNS otherwise the files are not read by doxygen. + +EXTENSION_MAPPING = + +# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want +# to include (a tag file for) the STL sources as input, then you should +# set this tag to YES in order to let doxygen match functions declarations and +# definitions whose arguments contain STL classes (e.g. func(std::string); v.s. +# func(std::string) {}). This also makes the inheritance and collaboration +# diagrams that involve STL classes more complete and accurate. + +BUILTIN_STL_SUPPORT = NO + +# If you use Microsoft's C++/CLI language, you should set this option to YES to +# enable parsing support. + +CPP_CLI_SUPPORT = NO + +# Set the SIP_SUPPORT tag to YES if your project consists of sip sources only. +# Doxygen will parse them like normal C++ but will assume all classes use public +# instead of private inheritance when no explicit protection keyword is present. + +SIP_SUPPORT = NO + +# For Microsoft's IDL there are propget and propput attributes to indicate getter +# and setter methods for a property. Setting this option to YES (the default) +# will make doxygen replace the get and set methods by a property in the +# documentation. This will only work if the methods are indeed getting or +# setting a simple type. If this is not the case, or you want to show the +# methods anyway, you should set this option to NO. + +IDL_PROPERTY_SUPPORT = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES, then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. + +DISTRIBUTE_GROUP_DOC = NO + +# Set the SUBGROUPING tag to YES (the default) to allow class member groups of +# the same type (for instance a group of public functions) to be put as a +# subgroup of that type (e.g. under the Public Functions section). Set it to +# NO to prevent subgrouping. Alternatively, this can be done per class using +# the \nosubgrouping command. + +SUBGROUPING = YES + +# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and +# unions are shown inside the group in which they are included (e.g. using +# @ingroup) instead of on a separate page (for HTML and Man pages) or +# section (for LaTeX and RTF). + +INLINE_GROUPED_CLASSES = NO + +# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and +# unions with only public data fields will be shown inline in the documentation +# of the scope in which they are defined (i.e. file, namespace, or group +# documentation), provided this scope is documented. If set to NO (the default), +# structs, classes, and unions are shown on a separate page (for HTML and Man +# pages) or section (for LaTeX and RTF). + +INLINE_SIMPLE_STRUCTS = NO + +# When TYPEDEF_HIDES_STRUCT is enabled, a typedef of a struct, union, or enum +# is documented as struct, union, or enum with the name of the typedef. So +# typedef struct TypeS {} TypeT, will appear in the documentation as a struct +# with name TypeT. When disabled the typedef will appear as a member of a file, +# namespace, or class. And the struct will be named TypeS. This can typically +# be useful for C code in case the coding convention dictates that all compound +# types are typedef'ed and only the typedef is referenced, never the tag name. + +TYPEDEF_HIDES_STRUCT = NO + +# The SYMBOL_CACHE_SIZE determines the size of the internal cache use to +# determine which symbols to keep in memory and which to flush to disk. +# When the cache is full, less often used symbols will be written to disk. +# For small to medium size projects (<1000 input files) the default value is +# probably good enough. For larger projects a too small cache size can cause +# doxygen to be busy swapping symbols to and from disk most of the time +# causing a significant performance penalty. +# If the system has enough physical memory increasing the cache will improve the +# performance by keeping more symbols in memory. Note that the value works on +# a logarithmic scale so increasing the size by one will roughly double the +# memory usage. The cache size is given by this formula: +# 2^(16+SYMBOL_CACHE_SIZE). The valid range is 0..9, the default is 0, +# corresponding to a cache size of 2^16 = 65536 symbols. + +SYMBOL_CACHE_SIZE = 0 + +# Similar to the SYMBOL_CACHE_SIZE the size of the symbol lookup cache can be +# set using LOOKUP_CACHE_SIZE. This cache is used to resolve symbols given +# their name and scope. Since this can be an expensive process and often the +# same symbol appear multiple times in the code, doxygen keeps a cache of +# pre-resolved symbols. If the cache is too small doxygen will become slower. +# If the cache is too large, memory is wasted. The cache size is given by this +# formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range is 0..9, the default is 0, +# corresponding to a cache size of 2^16 = 65536 symbols. + +LOOKUP_CACHE_SIZE = 0 + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- + +# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in +# documentation are documented, even if no documentation was available. +# Private class members and static file members will be hidden unless +# the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES + +EXTRACT_ALL = YES + +# If the EXTRACT_PRIVATE tag is set to YES all private members of a class +# will be included in the documentation. + +EXTRACT_PRIVATE = YES + +# If the EXTRACT_STATIC tag is set to YES all static members of a file +# will be included in the documentation. + +EXTRACT_STATIC = YES + +# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) +# defined locally in source files will be included in the documentation. +# If set to NO only classes defined in header files are included. + +EXTRACT_LOCAL_CLASSES = YES + +# This flag is only useful for Objective-C code. When set to YES local +# methods, which are defined in the implementation section but not in +# the interface are included in the documentation. +# If set to NO (the default) only methods in the interface are included. + +EXTRACT_LOCAL_METHODS = NO + +# If this flag is set to YES, the members of anonymous namespaces will be +# extracted and appear in the documentation as a namespace called +# 'anonymous_namespace{file}', where file will be replaced with the base +# name of the file that contains the anonymous namespace. By default +# anonymous namespaces are hidden. + +EXTRACT_ANON_NSPACES = NO + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all +# undocumented members of documented classes, files or namespaces. +# If set to NO (the default) these members will be included in the +# various overviews, but no documentation section is generated. +# This option has no effect if EXTRACT_ALL is enabled. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. +# If set to NO (the default) these classes will be included in the various +# overviews. This option has no effect if EXTRACT_ALL is enabled. + +HIDE_UNDOC_CLASSES = NO + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, Doxygen will hide all +# friend (class|struct|union) declarations. +# If set to NO (the default) these declarations will be included in the +# documentation. + +HIDE_FRIEND_COMPOUNDS = NO + +# If the HIDE_IN_BODY_DOCS tag is set to YES, Doxygen will hide any +# documentation blocks found inside the body of a function. +# If set to NO (the default) these blocks will be appended to the +# function's detailed documentation block. + +HIDE_IN_BODY_DOCS = NO + +# The INTERNAL_DOCS tag determines if documentation +# that is typed after a \internal command is included. If the tag is set +# to NO (the default) then the documentation will be excluded. +# Set it to YES to include the internal documentation. + +INTERNAL_DOCS = NO + +# If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate +# file names in lower-case letters. If set to YES upper-case letters are also +# allowed. This is useful if you have classes or files whose names only differ +# in case and if your file system supports case sensitive file names. Windows +# and Mac users are advised to set this option to NO. + +CASE_SENSE_NAMES = YES + +# If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen +# will show members with their full class and namespace scopes in the +# documentation. If set to YES the scope will be hidden. + +HIDE_SCOPE_NAMES = NO + +# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen +# will put a list of the files that are included by a file in the documentation +# of that file. + +SHOW_INCLUDE_FILES = YES + +# If the FORCE_LOCAL_INCLUDES tag is set to YES then Doxygen +# will list include files with double quotes in the documentation +# rather than with sharp brackets. + +FORCE_LOCAL_INCLUDES = NO + +# If the INLINE_INFO tag is set to YES (the default) then a tag [inline] +# is inserted in the documentation for inline members. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES (the default) then doxygen +# will sort the (detailed) documentation of file and class members +# alphabetically by member name. If set to NO the members will appear in +# declaration order. + +SORT_MEMBER_DOCS = YES + +# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the +# brief documentation of file, namespace and class members alphabetically +# by member name. If set to NO (the default) the members will appear in +# declaration order. + +SORT_BRIEF_DOCS = NO + +# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen +# will sort the (brief and detailed) documentation of class members so that +# constructors and destructors are listed first. If set to NO (the default) +# the constructors will appear in the respective orders defined by +# SORT_MEMBER_DOCS and SORT_BRIEF_DOCS. +# This tag will be ignored for brief docs if SORT_BRIEF_DOCS is set to NO +# and ignored for detailed docs if SORT_MEMBER_DOCS is set to NO. + +SORT_MEMBERS_CTORS_1ST = NO + +# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the +# hierarchy of group names into alphabetical order. If set to NO (the default) +# the group names will appear in their defined order. + +SORT_GROUP_NAMES = NO + +# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be +# sorted by fully-qualified names, including namespaces. If set to +# NO (the default), the class list will be sorted only by class name, +# not including the namespace part. +# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. +# Note: This option applies only to the class list, not to the +# alphabetical list. + +SORT_BY_SCOPE_NAME = NO + +# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to +# do proper type resolution of all parameters of a function it will reject a +# match between the prototype and the implementation of a member function even +# if there is only one candidate or it is obvious which candidate to choose +# by doing a simple string match. By disabling STRICT_PROTO_MATCHING doxygen +# will still accept a match between prototype and implementation in such cases. + +STRICT_PROTO_MATCHING = NO + +# The GENERATE_TODOLIST tag can be used to enable (YES) or +# disable (NO) the todo list. This list is created by putting \todo +# commands in the documentation. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable (YES) or +# disable (NO) the test list. This list is created by putting \test +# commands in the documentation. + +GENERATE_TESTLIST = YES + +# The GENERATE_BUGLIST tag can be used to enable (YES) or +# disable (NO) the bug list. This list is created by putting \bug +# commands in the documentation. + +GENERATE_BUGLIST = YES + +# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or +# disable (NO) the deprecated list. This list is created by putting +# \deprecated commands in the documentation. + +GENERATE_DEPRECATEDLIST= YES + +# The ENABLED_SECTIONS tag can be used to enable conditional +# documentation sections, marked by \if sectionname ... \endif. + +ENABLED_SECTIONS = + +# The MAX_INITIALIZER_LINES tag determines the maximum number of lines +# the initial value of a variable or macro consists of for it to appear in +# the documentation. If the initializer consists of more lines than specified +# here it will be hidden. Use a value of 0 to hide initializers completely. +# The appearance of the initializer of individual variables and macros in the +# documentation can be controlled using \showinitializer or \hideinitializer +# command in the documentation regardless of this setting. + +MAX_INITIALIZER_LINES = 30 + +# Set the SHOW_USED_FILES tag to NO to disable the list of files generated +# at the bottom of the documentation of classes and structs. If set to YES the +# list will mention the files that were used to generate the documentation. + +SHOW_USED_FILES = YES + +# If the sources in your project are distributed over multiple directories +# then setting the SHOW_DIRECTORIES tag to YES will show the directory hierarchy +# in the documentation. The default is NO. + +SHOW_DIRECTORIES = NO + +# Set the SHOW_FILES tag to NO to disable the generation of the Files page. +# This will remove the Files entry from the Quick Index and from the +# Folder Tree View (if specified). The default is YES. + +SHOW_FILES = YES + +# Set the SHOW_NAMESPACES tag to NO to disable the generation of the +# Namespaces page. +# This will remove the Namespaces entry from the Quick Index +# and from the Folder Tree View (if specified). The default is YES. + +SHOW_NAMESPACES = YES + +# The FILE_VERSION_FILTER tag can be used to specify a program or script that +# doxygen should invoke to get the current version for each file (typically from +# the version control system). Doxygen will invoke the program by executing (via +# popen()) the command , where is the value of +# the FILE_VERSION_FILTER tag, and is the name of an input file +# provided by doxygen. Whatever the program writes to standard output +# is used as the file version. See the manual for examples. + +FILE_VERSION_FILTER = + +# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed +# by doxygen. The layout file controls the global structure of the generated +# output files in an output format independent way. The create the layout file +# that represents doxygen's defaults, run doxygen with the -l option. +# You can optionally specify a file name after the option, if omitted +# DoxygenLayout.xml will be used as the name of the layout file. + +LAYOUT_FILE = + +# The CITE_BIB_FILES tag can be used to specify one or more bib files +# containing the references data. This must be a list of .bib files. The +# .bib extension is automatically appended if omitted. Using this command +# requires the bibtex tool to be installed. See also +# http://en.wikipedia.org/wiki/BibTeX for more info. For LaTeX the style +# of the bibliography can be controlled using LATEX_BIB_STYLE. To use this +# feature you need bibtex and perl available in the search path. + +CITE_BIB_FILES = + +#--------------------------------------------------------------------------- +# configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated +# by doxygen. Possible values are YES and NO. If left blank NO is used. + +QUIET = NO + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated by doxygen. Possible values are YES and NO. If left blank +# NO is used. + +WARNINGS = YES + +# If WARN_IF_UNDOCUMENTED is set to YES, then doxygen will generate warnings +# for undocumented members. If EXTRACT_ALL is set to YES then this flag will +# automatically be disabled. + +WARN_IF_UNDOCUMENTED = YES + +# If WARN_IF_DOC_ERROR is set to YES, doxygen will generate warnings for +# potential errors in the documentation, such as not documenting some +# parameters in a documented function, or documenting parameters that +# don't exist or using markup commands wrongly. + +WARN_IF_DOC_ERROR = YES + +# The WARN_NO_PARAMDOC option can be enabled to get warnings for +# functions that are documented, but have no documentation for their parameters +# or return value. If set to NO (the default) doxygen will only warn about +# wrong or incomplete parameter documentation, but not about the absence of +# documentation. + +WARN_NO_PARAMDOC = NO + +# The WARN_FORMAT tag determines the format of the warning messages that +# doxygen can produce. The string should contain the $file, $line, and $text +# tags, which will be replaced by the file and line number from which the +# warning originated and the warning text. Optionally the format may contain +# $version, which will be replaced by the version of the file (if it could +# be obtained via FILE_VERSION_FILTER) + +WARN_FORMAT = "$file:$line: $text" + +# The WARN_LOGFILE tag can be used to specify a file to which warning +# and error messages should be written. If left blank the output is written +# to stderr. + +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag can be used to specify the files and/or directories that contain +# documented source files. You may enter file names like "myfile.cpp" or +# directories like "/usr/src/myproject". Separate the files or directories +# with spaces. + +INPUT = "src", "include/nano_slam" + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is +# also the default input encoding. Doxygen uses libiconv (or the iconv built +# into libc) for the transcoding. See http://www.gnu.org/software/libiconv for +# the list of possible encodings. + +INPUT_ENCODING = UTF-8 + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp +# and *.h) to filter out the source-files in the directories. If left +# blank the following patterns are tested: +# *.c *.cc *.cxx *.cpp *.c++ *.d *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh +# *.hxx *.hpp *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm *.dox *.py +# *.f90 *.f *.for *.vhd *.vhdl + +FILE_PATTERNS = + +# The RECURSIVE tag can be used to turn specify whether or not subdirectories +# should be searched for input files as well. Possible values are YES and NO. +# If left blank NO is used. + +RECURSIVE = NO + +# The EXCLUDE tag can be used to specify files and/or directories that should be +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. +# Note that relative paths are relative to the directory from which doxygen is +# run. + +EXCLUDE = + +# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or +# directories that are symbolic links (a Unix file system feature) are excluded +# from the input. + +EXCLUDE_SYMLINKS = NO + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. Note that the wildcards are matched +# against the file with absolute path, so to exclude all test directories +# for example use the pattern */test/* + +EXCLUDE_PATTERNS = + +# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names +# (namespaces, classes, functions, etc.) that should be excluded from the +# output. The symbol name can be a fully qualified name, a word, or if the +# wildcard * is used, a substring. Examples: ANamespace, AClass, +# AClass::ANamespace, ANamespace::*Test + +EXCLUDE_SYMBOLS = g2o +EXCLUDE_SYMBOLS += vk + +# The EXAMPLE_PATH tag can be used to specify one or more files or +# directories that contain example code fragments that are included (see +# the \include command). + +EXAMPLE_PATH = + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp +# and *.h) to filter out the source-files in the directories. If left +# blank all files are included. + +EXAMPLE_PATTERNS = + +# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be +# searched for input files to be used with the \include or \dontinclude +# commands irrespective of the value of the RECURSIVE tag. +# Possible values are YES and NO. If left blank NO is used. + +EXAMPLE_RECURSIVE = NO + +# The IMAGE_PATH tag can be used to specify one or more files or +# directories that contain image that are included in the documentation (see +# the \image command). + +IMAGE_PATH = + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command , where +# is the value of the INPUT_FILTER tag, and is the name of an +# input file. Doxygen will then use the output that the filter program writes +# to standard output. +# If FILTER_PATTERNS is specified, this tag will be +# ignored. + +INPUT_FILTER = + +# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern +# basis. +# Doxygen will compare the file name with each pattern and apply the +# filter if there is a match. +# The filters are a list of the form: +# pattern=filter (like *.cpp=my_cpp_filter). See INPUT_FILTER for further +# info on how filters are used. If FILTER_PATTERNS is empty or if +# non of the patterns match the file name, INPUT_FILTER is applied. + +FILTER_PATTERNS = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER) will be used to filter the input files when producing source +# files to browse (i.e. when SOURCE_BROWSER is set to YES). + +FILTER_SOURCE_FILES = NO + +# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file +# pattern. A pattern will override the setting for FILTER_PATTERN (if any) +# and it is also possible to disable source filtering for a specific pattern +# using *.ext= (so without naming a filter). This option only has effect when +# FILTER_SOURCE_FILES is enabled. + +FILTER_SOURCE_PATTERNS = + +#--------------------------------------------------------------------------- +# configuration options related to source browsing +#--------------------------------------------------------------------------- + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will +# be generated. Documented entities will be cross-referenced with these sources. +# Note: To get rid of all source code in the generated output, make sure also +# VERBATIM_HEADERS is set to NO. + +SOURCE_BROWSER = NO + +# Setting the INLINE_SOURCES tag to YES will include the body +# of functions and classes directly in the documentation. + +INLINE_SOURCES = NO + +# Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct +# doxygen to hide any special comment blocks from generated source code +# fragments. Normal C and C++ comments will always remain visible. + +STRIP_CODE_COMMENTS = YES + +# If the REFERENCED_BY_RELATION tag is set to YES +# then for each documented function all documented +# functions referencing it will be listed. + +REFERENCED_BY_RELATION = NO + +# If the REFERENCES_RELATION tag is set to YES +# then for each documented function all documented entities +# called/used by that function will be listed. + +REFERENCES_RELATION = NO + +# If the REFERENCES_LINK_SOURCE tag is set to YES (the default) +# and SOURCE_BROWSER tag is set to YES, then the hyperlinks from +# functions in REFERENCES_RELATION and REFERENCED_BY_RELATION lists will +# link to the source code. +# Otherwise they will link to the documentation. + +REFERENCES_LINK_SOURCE = YES + +# If the USE_HTAGS tag is set to YES then the references to source code +# will point to the HTML generated by the htags(1) tool instead of doxygen +# built-in source browser. The htags tool is part of GNU's global source +# tagging system (see http://www.gnu.org/software/global/global.html). You +# will need version 4.8.6 or higher. + +USE_HTAGS = NO + +# If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen +# will generate a verbatim copy of the header file for each class for +# which an include is specified. Set to NO to disable this. + +VERBATIM_HEADERS = YES + +#--------------------------------------------------------------------------- +# configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index +# of all compounds will be generated. Enable this if the project +# contains a lot of classes, structs, unions or interfaces. + +ALPHABETICAL_INDEX = YES + +# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then +# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns +# in which this list will be split (can be a number in the range [1..20]) + +COLS_IN_ALPHA_INDEX = 5 + +# In case all classes in a project start with a common prefix, all +# classes will be put under the same header in the alphabetical index. +# The IGNORE_PREFIX tag can be used to specify one or more prefixes that +# should be ignored while generating the index headers. + +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES (the default) Doxygen will +# generate HTML output. + +GENERATE_HTML = YES + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `html' will be used as the default path. + +HTML_OUTPUT = html + +# The HTML_FILE_EXTENSION tag can be used to specify the file extension for +# each generated HTML page (for example: .htm,.php,.asp). If it is left blank +# doxygen will generate files with .html extension. + +HTML_FILE_EXTENSION = .html + +# The HTML_HEADER tag can be used to specify a personal HTML header for +# each generated HTML page. If it is left blank doxygen will generate a +# standard header. Note that when using a custom header you are responsible +# for the proper inclusion of any scripts and style sheets that doxygen +# needs, which is dependent on the configuration options used. +# It is advised to generate a default header using "doxygen -w html +# header.html footer.html stylesheet.css YourConfigFile" and then modify +# that header. Note that the header is subject to change so you typically +# have to redo this when upgrading to a newer version of doxygen or when +# changing the value of configuration settings such as GENERATE_TREEVIEW! + +HTML_HEADER = + +# The HTML_FOOTER tag can be used to specify a personal HTML footer for +# each generated HTML page. If it is left blank doxygen will generate a +# standard footer. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user-defined cascading +# style sheet that is used by each HTML page. It can be used to +# fine-tune the look of the HTML output. If the tag is left blank doxygen +# will generate a default style sheet. Note that doxygen will try to copy +# the style sheet file to the HTML output directory, so don't put your own +# style sheet in the HTML output directory as well, or it will be erased! + +HTML_STYLESHEET = + +# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or +# other source files which should be copied to the HTML output directory. Note +# that these files will be copied to the base HTML output directory. Use the +# $relpath$ marker in the HTML_HEADER and/or HTML_FOOTER files to load these +# files. In the HTML_STYLESHEET file, use the file name only. Also note that +# the files will be copied as-is; there are no commands or markers available. + +HTML_EXTRA_FILES = + +# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. +# Doxygen will adjust the colors in the style sheet and background images +# according to this color. Hue is specified as an angle on a colorwheel, +# see http://en.wikipedia.org/wiki/Hue for more information. +# For instance the value 0 represents red, 60 is yellow, 120 is green, +# 180 is cyan, 240 is blue, 300 purple, and 360 is red again. +# The allowed range is 0 to 359. + +HTML_COLORSTYLE_HUE = 220 + +# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of +# the colors in the HTML output. For a value of 0 the output will use +# grayscales only. A value of 255 will produce the most vivid colors. + +HTML_COLORSTYLE_SAT = 100 + +# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to +# the luminance component of the colors in the HTML output. Values below +# 100 gradually make the output lighter, whereas values above 100 make +# the output darker. The value divided by 100 is the actual gamma applied, +# so 80 represents a gamma of 0.8, The value 220 represents a gamma of 2.2, +# and 100 does not change the gamma. + +HTML_COLORSTYLE_GAMMA = 80 + +# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML +# page will contain the date and time when the page was generated. Setting +# this to NO can help when comparing the output of multiple runs. + +HTML_TIMESTAMP = YES + +# If the HTML_ALIGN_MEMBERS tag is set to YES, the members of classes, +# files or namespaces will be aligned in HTML using tables. If set to +# NO a bullet list will be used. + +HTML_ALIGN_MEMBERS = YES + +# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML +# documentation will contain sections that can be hidden and shown after the +# page has loaded. For this to work a browser that supports +# JavaScript and DHTML is required (for instance Mozilla 1.0+, Firefox +# Netscape 6.0+, Internet explorer 5.0+, Konqueror, or Safari). + +HTML_DYNAMIC_SECTIONS = NO + +# If the GENERATE_DOCSET tag is set to YES, additional index files +# will be generated that can be used as input for Apple's Xcode 3 +# integrated development environment, introduced with OSX 10.5 (Leopard). +# To create a documentation set, doxygen will generate a Makefile in the +# HTML output directory. Running make will produce the docset in that +# directory and running "make install" will install the docset in +# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find +# it at startup. +# See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html +# for more information. + +GENERATE_DOCSET = NO + +# When GENERATE_DOCSET tag is set to YES, this tag determines the name of the +# feed. A documentation feed provides an umbrella under which multiple +# documentation sets from a single provider (such as a company or product suite) +# can be grouped. + +DOCSET_FEEDNAME = "Doxygen generated docs" + +# When GENERATE_DOCSET tag is set to YES, this tag specifies a string that +# should uniquely identify the documentation set bundle. This should be a +# reverse domain-name style string, e.g. com.mycompany.MyDocSet. Doxygen +# will append .docset to the name. + +DOCSET_BUNDLE_ID = org.doxygen.Project + +# When GENERATE_PUBLISHER_ID tag specifies a string that should uniquely identify +# the documentation publisher. This should be a reverse domain-name style +# string, e.g. com.mycompany.MyDocSet.documentation. + +DOCSET_PUBLISHER_ID = org.doxygen.Publisher + +# The GENERATE_PUBLISHER_NAME tag identifies the documentation publisher. + +DOCSET_PUBLISHER_NAME = Publisher + +# If the GENERATE_HTMLHELP tag is set to YES, additional index files +# will be generated that can be used as input for tools like the +# Microsoft HTML help workshop to generate a compiled HTML help file (.chm) +# of the generated HTML documentation. + +GENERATE_HTMLHELP = NO + +# If the GENERATE_HTMLHELP tag is set to YES, the CHM_FILE tag can +# be used to specify the file name of the resulting .chm file. You +# can add a path in front of the file if the result should not be +# written to the html output directory. + +CHM_FILE = + +# If the GENERATE_HTMLHELP tag is set to YES, the HHC_LOCATION tag can +# be used to specify the location (absolute path including file name) of +# the HTML help compiler (hhc.exe). If non-empty doxygen will try to run +# the HTML help compiler on the generated index.hhp. + +HHC_LOCATION = + +# If the GENERATE_HTMLHELP tag is set to YES, the GENERATE_CHI flag +# controls if a separate .chi index file is generated (YES) or that +# it should be included in the master .chm file (NO). + +GENERATE_CHI = NO + +# If the GENERATE_HTMLHELP tag is set to YES, the CHM_INDEX_ENCODING +# is used to encode HtmlHelp index (hhk), content (hhc) and project file +# content. + +CHM_INDEX_ENCODING = + +# If the GENERATE_HTMLHELP tag is set to YES, the BINARY_TOC flag +# controls whether a binary table of contents is generated (YES) or a +# normal table of contents (NO) in the .chm file. + +BINARY_TOC = NO + +# The TOC_EXPAND flag can be set to YES to add extra items for group members +# to the contents of the HTML help documentation and to the tree view. + +TOC_EXPAND = NO + +# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and +# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated +# that can be used as input for Qt's qhelpgenerator to generate a +# Qt Compressed Help (.qch) of the generated HTML documentation. + +GENERATE_QHP = NO + +# If the QHG_LOCATION tag is specified, the QCH_FILE tag can +# be used to specify the file name of the resulting .qch file. +# The path specified is relative to the HTML output folder. + +QCH_FILE = + +# The QHP_NAMESPACE tag specifies the namespace to use when generating +# Qt Help Project output. For more information please see +# http://doc.trolltech.com/qthelpproject.html#namespace + +QHP_NAMESPACE = org.doxygen.Project + +# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating +# Qt Help Project output. For more information please see +# http://doc.trolltech.com/qthelpproject.html#virtual-folders + +QHP_VIRTUAL_FOLDER = doc + +# If QHP_CUST_FILTER_NAME is set, it specifies the name of a custom filter to +# add. For more information please see +# http://doc.trolltech.com/qthelpproject.html#custom-filters + +QHP_CUST_FILTER_NAME = + +# The QHP_CUST_FILT_ATTRS tag specifies the list of the attributes of the +# custom filter to add. For more information please see +# +# Qt Help Project / Custom Filters. + +QHP_CUST_FILTER_ATTRS = + +# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this +# project's +# filter section matches. +# +# Qt Help Project / Filter Attributes. + +QHP_SECT_FILTER_ATTRS = + +# If the GENERATE_QHP tag is set to YES, the QHG_LOCATION tag can +# be used to specify the location of Qt's qhelpgenerator. +# If non-empty doxygen will try to run qhelpgenerator on the generated +# .qhp file. + +QHG_LOCATION = + +# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files +# will be generated, which together with the HTML files, form an Eclipse help +# plugin. To install this plugin and make it available under the help contents +# menu in Eclipse, the contents of the directory containing the HTML and XML +# files needs to be copied into the plugins directory of eclipse. The name of +# the directory within the plugins directory should be the same as +# the ECLIPSE_DOC_ID value. After copying Eclipse needs to be restarted before +# the help appears. + +GENERATE_ECLIPSEHELP = NO + +# A unique identifier for the eclipse help plugin. When installing the plugin +# the directory name containing the HTML and XML files should also have +# this name. + +ECLIPSE_DOC_ID = org.doxygen.Project + +# The DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) +# at top of each HTML page. The value NO (the default) enables the index and +# the value YES disables it. Since the tabs have the same information as the +# navigation tree you can set this option to NO if you already set +# GENERATE_TREEVIEW to YES. + +DISABLE_INDEX = NO + +# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index +# structure should be generated to display hierarchical information. +# If the tag value is set to YES, a side panel will be generated +# containing a tree-like index structure (just like the one that +# is generated for HTML Help). For this to work a browser that supports +# JavaScript, DHTML, CSS and frames is required (i.e. any modern browser). +# Windows users are probably better off using the HTML help feature. +# Since the tree basically has the same information as the tab index you +# could consider to set DISABLE_INDEX to NO when enabling this option. + +GENERATE_TREEVIEW = NO + +# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values +# (range [0,1..20]) that doxygen will group on one line in the generated HTML +# documentation. Note that a value of 0 will completely suppress the enum +# values from appearing in the overview section. + +ENUM_VALUES_PER_LINE = 4 + +# By enabling USE_INLINE_TREES, doxygen will generate the Groups, Directories, +# and Class Hierarchy pages using a tree view instead of an ordered list. + +USE_INLINE_TREES = NO + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be +# used to set the initial width (in pixels) of the frame in which the tree +# is shown. + +TREEVIEW_WIDTH = 250 + +# When the EXT_LINKS_IN_WINDOW option is set to YES doxygen will open +# links to external symbols imported via tag files in a separate window. + +EXT_LINKS_IN_WINDOW = NO + +# Use this tag to change the font size of Latex formulas included +# as images in the HTML documentation. The default is 10. Note that +# when you change the font size after a successful doxygen run you need +# to manually remove any form_*.png images from the HTML output directory +# to force them to be regenerated. + +FORMULA_FONTSIZE = 10 + +# Use the FORMULA_TRANPARENT tag to determine whether or not the images +# generated for formulas are transparent PNGs. Transparent PNGs are +# not supported properly for IE 6.0, but are supported on all modern browsers. +# Note that when changing this option you need to delete any form_*.png files +# in the HTML output before the changes have effect. + +FORMULA_TRANSPARENT = YES + +# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax +# (see http://www.mathjax.org) which uses client side Javascript for the +# rendering instead of using prerendered bitmaps. Use this if you do not +# have LaTeX installed or if you want to formulas look prettier in the HTML +# output. When enabled you also need to install MathJax separately and +# configure the path to it using the MATHJAX_RELPATH option. + +USE_MATHJAX = NO + +# When MathJax is enabled you need to specify the location relative to the +# HTML output directory using the MATHJAX_RELPATH option. The destination +# directory should contain the MathJax.js script. For instance, if the mathjax +# directory is located at the same level as the HTML output directory, then +# MATHJAX_RELPATH should be ../mathjax. The default value points to the +# mathjax.org site, so you can quickly see the result without installing +# MathJax, but it is strongly recommended to install a local copy of MathJax +# before deployment. + +MATHJAX_RELPATH = http://www.mathjax.org/mathjax + +# The MATHJAX_EXTENSIONS tag can be used to specify one or MathJax extension +# names that should be enabled during MathJax rendering. + +MATHJAX_EXTENSIONS = + +# When the SEARCHENGINE tag is enabled doxygen will generate a search box +# for the HTML output. The underlying search engine uses javascript +# and DHTML and should work on any modern browser. Note that when using +# HTML help (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets +# (GENERATE_DOCSET) there is already a search function so this one should +# typically be disabled. For large projects the javascript based search engine +# can be slow, then enabling SERVER_BASED_SEARCH may provide a better solution. + +SEARCHENGINE = YES + +# When the SERVER_BASED_SEARCH tag is enabled the search engine will be +# implemented using a PHP enabled web server instead of at the web client +# using Javascript. Doxygen will generate the search PHP script and index +# file to put on the web server. The advantage of the server +# based approach is that it scales better to large projects and allows +# full text search. The disadvantages are that it is more difficult to setup +# and does not have live searching capabilities. + +SERVER_BASED_SEARCH = NO + +#--------------------------------------------------------------------------- +# configuration options related to the LaTeX output +#--------------------------------------------------------------------------- + +# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will +# generate Latex output. + +GENERATE_LATEX = YES + +# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `latex' will be used as the default path. + +LATEX_OUTPUT = latex + +# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be +# invoked. If left blank `latex' will be used as the default command name. +# Note that when enabling USE_PDFLATEX this option is only used for +# generating bitmaps for formulas in the HTML output, but not in the +# Makefile that is written to the output directory. + +LATEX_CMD_NAME = latex + +# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to +# generate index for LaTeX. If left blank `makeindex' will be used as the +# default command name. + +MAKEINDEX_CMD_NAME = makeindex + +# If the COMPACT_LATEX tag is set to YES Doxygen generates more compact +# LaTeX documents. This may be useful for small projects and may help to +# save some trees in general. + +COMPACT_LATEX = NO + +# The PAPER_TYPE tag can be used to set the paper type that is used +# by the printer. Possible values are: a4, letter, legal and +# executive. If left blank a4wide will be used. + +PAPER_TYPE = a4 + +# The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX +# packages that should be included in the LaTeX output. + +EXTRA_PACKAGES = + +# The LATEX_HEADER tag can be used to specify a personal LaTeX header for +# the generated latex document. The header should contain everything until +# the first chapter. If it is left blank doxygen will generate a +# standard header. Notice: only use this tag if you know what you are doing! + +LATEX_HEADER = + +# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for +# the generated latex document. The footer should contain everything after +# the last chapter. If it is left blank doxygen will generate a +# standard footer. Notice: only use this tag if you know what you are doing! + +LATEX_FOOTER = + +# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated +# is prepared for conversion to pdf (using ps2pdf). The pdf file will +# contain links (just like the HTML output) instead of page references +# This makes the output suitable for online browsing using a pdf viewer. + +PDF_HYPERLINKS = YES + +# If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of +# plain latex in the generated Makefile. Set this option to YES to get a +# higher quality PDF documentation. + +USE_PDFLATEX = YES + +# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \\batchmode. +# command to the generated LaTeX files. This will instruct LaTeX to keep +# running if errors occur, instead of asking the user for help. +# This option is also used when generating formulas in HTML. + +LATEX_BATCHMODE = NO + +# If LATEX_HIDE_INDICES is set to YES then doxygen will not +# include the index chapters (such as File Index, Compound Index, etc.) +# in the output. + +LATEX_HIDE_INDICES = NO + +# If LATEX_SOURCE_CODE is set to YES then doxygen will include +# source code with syntax highlighting in the LaTeX output. +# Note that which sources are shown also depends on other settings +# such as SOURCE_BROWSER. + +LATEX_SOURCE_CODE = NO + +# The LATEX_BIB_STYLE tag can be used to specify the style to use for the +# bibliography, e.g. plainnat, or ieeetr. The default style is "plain". See +# http://en.wikipedia.org/wiki/BibTeX for more info. + +LATEX_BIB_STYLE = plain + +#--------------------------------------------------------------------------- +# configuration options related to the RTF output +#--------------------------------------------------------------------------- + +# If the GENERATE_RTF tag is set to YES Doxygen will generate RTF output +# The RTF output is optimized for Word 97 and may not look very pretty with +# other RTF readers or editors. + +GENERATE_RTF = NO + +# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `rtf' will be used as the default path. + +RTF_OUTPUT = rtf + +# If the COMPACT_RTF tag is set to YES Doxygen generates more compact +# RTF documents. This may be useful for small projects and may help to +# save some trees in general. + +COMPACT_RTF = NO + +# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated +# will contain hyperlink fields. The RTF file will +# contain links (just like the HTML output) instead of page references. +# This makes the output suitable for online browsing using WORD or other +# programs which support those fields. +# Note: wordpad (write) and others do not support links. + +RTF_HYPERLINKS = NO + +# Load style sheet definitions from file. Syntax is similar to doxygen's +# config file, i.e. a series of assignments. You only have to provide +# replacements, missing definitions are set to their default value. + +RTF_STYLESHEET_FILE = + +# Set optional variables used in the generation of an rtf document. +# Syntax is similar to doxygen's config file. + +RTF_EXTENSIONS_FILE = + +#--------------------------------------------------------------------------- +# configuration options related to the man page output +#--------------------------------------------------------------------------- + +# If the GENERATE_MAN tag is set to YES (the default) Doxygen will +# generate man pages + +GENERATE_MAN = NO + +# The MAN_OUTPUT tag is used to specify where the man pages will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `man' will be used as the default path. + +MAN_OUTPUT = man + +# The MAN_EXTENSION tag determines the extension that is added to +# the generated man pages (default is the subroutine's section .3) + +MAN_EXTENSION = .3 + +# If the MAN_LINKS tag is set to YES and Doxygen generates man output, +# then it will generate one additional man file for each entity +# documented in the real man page(s). These additional files +# only source the real man page, but without them the man command +# would be unable to find the correct page. The default is NO. + +MAN_LINKS = NO + +#--------------------------------------------------------------------------- +# configuration options related to the XML output +#--------------------------------------------------------------------------- + +# If the GENERATE_XML tag is set to YES Doxygen will +# generate an XML file that captures the structure of +# the code including all documentation. + +GENERATE_XML = NO + +# The XML_OUTPUT tag is used to specify where the XML pages will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `xml' will be used as the default path. + +XML_OUTPUT = xml + +# The XML_SCHEMA tag can be used to specify an XML schema, +# which can be used by a validating XML parser to check the +# syntax of the XML files. + +XML_SCHEMA = + +# The XML_DTD tag can be used to specify an XML DTD, +# which can be used by a validating XML parser to check the +# syntax of the XML files. + +XML_DTD = + +# If the XML_PROGRAMLISTING tag is set to YES Doxygen will +# dump the program listings (including syntax highlighting +# and cross-referencing information) to the XML output. Note that +# enabling this will significantly increase the size of the XML output. + +XML_PROGRAMLISTING = YES + +#--------------------------------------------------------------------------- +# configuration options for the AutoGen Definitions output +#--------------------------------------------------------------------------- + +# If the GENERATE_AUTOGEN_DEF tag is set to YES Doxygen will +# generate an AutoGen Definitions (see autogen.sf.net) file +# that captures the structure of the code including all +# documentation. Note that this feature is still experimental +# and incomplete at the moment. + +GENERATE_AUTOGEN_DEF = NO + +#--------------------------------------------------------------------------- +# configuration options related to the Perl module output +#--------------------------------------------------------------------------- + +# If the GENERATE_PERLMOD tag is set to YES Doxygen will +# generate a Perl module file that captures the structure of +# the code including all documentation. Note that this +# feature is still experimental and incomplete at the +# moment. + +GENERATE_PERLMOD = NO + +# If the PERLMOD_LATEX tag is set to YES Doxygen will generate +# the necessary Makefile rules, Perl scripts and LaTeX code to be able +# to generate PDF and DVI output from the Perl module output. + +PERLMOD_LATEX = NO + +# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be +# nicely formatted so it can be parsed by a human reader. +# This is useful +# if you want to understand what is going on. +# On the other hand, if this +# tag is set to NO the size of the Perl module output will be much smaller +# and Perl will parse it just the same. + +PERLMOD_PRETTY = YES + +# The names of the make variables in the generated doxyrules.make file +# are prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. +# This is useful so different doxyrules.make files included by the same +# Makefile don't overwrite each other's variables. + +PERLMOD_MAKEVAR_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the preprocessor +#--------------------------------------------------------------------------- + +# If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will +# evaluate all C-preprocessor directives found in the sources and include +# files. + +ENABLE_PREPROCESSING = YES + +# If the MACRO_EXPANSION tag is set to YES Doxygen will expand all macro +# names in the source code. If set to NO (the default) only conditional +# compilation will be performed. Macro expansion can be done in a controlled +# way by setting EXPAND_ONLY_PREDEF to YES. + +MACRO_EXPANSION = NO + +# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES +# then the macro expansion is limited to the macros specified with the +# PREDEFINED and EXPAND_AS_DEFINED tags. + +EXPAND_ONLY_PREDEF = NO + +# If the SEARCH_INCLUDES tag is set to YES (the default) the includes files +# pointed to by INCLUDE_PATH will be searched when a #include is found. + +SEARCH_INCLUDES = YES + +# The INCLUDE_PATH tag can be used to specify one or more directories that +# contain include files that are not input files but should be processed by +# the preprocessor. + +INCLUDE_PATH = + +# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard +# patterns (like *.h and *.hpp) to filter out the header-files in the +# directories. If left blank, the patterns specified with FILE_PATTERNS will +# be used. + +INCLUDE_FILE_PATTERNS = + +# The PREDEFINED tag can be used to specify one or more macro names that +# are defined before the preprocessor is started (similar to the -D option of +# gcc). The argument of the tag is a list of macros of the form: name +# or name=definition (no spaces). If the definition and the = are +# omitted =1 is assumed. To prevent a macro definition from being +# undefined via #undef or recursively expanded use the := operator +# instead of the = operator. + +PREDEFINED = + +# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then +# this tag can be used to specify a list of macro names that should be expanded. +# The macro definition that is found in the sources will be used. +# Use the PREDEFINED tag if you want to use a different macro definition that +# overrules the definition found in the source code. + +EXPAND_AS_DEFINED = + +# If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then +# doxygen's preprocessor will remove all references to function-like macros +# that are alone on a line, have an all uppercase name, and do not end with a +# semicolon, because these will confuse the parser if not removed. + +SKIP_FUNCTION_MACROS = YES + +#--------------------------------------------------------------------------- +# Configuration::additions related to external references +#--------------------------------------------------------------------------- + +# The TAGFILES option can be used to specify one or more tagfiles. +# Optionally an initial location of the external documentation +# can be added for each tagfile. The format of a tag file without +# this location is as follows: +# +# TAGFILES = file1 file2 ... +# Adding location for the tag files is done as follows: +# +# TAGFILES = file1=loc1 "file2 = loc2" ... +# where "loc1" and "loc2" can be relative or absolute paths or +# URLs. If a location is present for each tag, the installdox tool +# does not have to be run to correct the links. +# Note that each tag file must have a unique name +# (where the name does NOT include the path) +# If a tag file is not located in the directory in which doxygen +# is run, you must also specify the path to the tagfile here. + +TAGFILES = + +# When a file name is specified after GENERATE_TAGFILE, doxygen will create +# a tag file that is based on the input files it reads. + +GENERATE_TAGFILE = + +# If the ALLEXTERNALS tag is set to YES all external classes will be listed +# in the class index. If set to NO only the inherited external classes +# will be listed. + +ALLEXTERNALS = NO + +# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed +# in the modules index. If set to NO, only the current project's groups will +# be listed. + +EXTERNAL_GROUPS = YES + +# The PERL_PATH should be the absolute path and name of the perl script +# interpreter (i.e. the result of `which perl'). + +PERL_PATH = /usr/bin/perl + +#--------------------------------------------------------------------------- +# Configuration options related to the dot tool +#--------------------------------------------------------------------------- + +# If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will +# generate a inheritance diagram (in HTML, RTF and LaTeX) for classes with base +# or super classes. Setting the tag to NO turns the diagrams off. Note that +# this option also works with HAVE_DOT disabled, but it is recommended to +# install and use dot, since it yields more powerful graphs. + +CLASS_DIAGRAMS = YES + +# You can define message sequence charts within doxygen comments using the \msc +# command. Doxygen will then run the mscgen tool (see +# http://www.mcternan.me.uk/mscgen/) to produce the chart and insert it in the +# documentation. The MSCGEN_PATH tag allows you to specify the directory where +# the mscgen tool resides. If left empty the tool is assumed to be found in the +# default search path. + +MSCGEN_PATH = + +# If set to YES, the inheritance and collaboration graphs will hide +# inheritance and usage relations if the target is undocumented +# or is not a class. + +HIDE_UNDOC_RELATIONS = YES + +# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is +# available from the path. This tool is part of Graphviz, a graph visualization +# toolkit from AT&T and Lucent Bell Labs. The other options in this section +# have no effect if this option is set to NO (the default) + +HAVE_DOT = NO + +# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is +# allowed to run in parallel. When set to 0 (the default) doxygen will +# base this on the number of processors available in the system. You can set it +# explicitly to a value larger than 0 to get control over the balance +# between CPU load and processing speed. + +DOT_NUM_THREADS = 0 + +# By default doxygen will use the Helvetica font for all dot files that +# doxygen generates. When you want a differently looking font you can specify +# the font name using DOT_FONTNAME. You need to make sure dot is able to find +# the font, which can be done by putting it in a standard location or by setting +# the DOTFONTPATH environment variable or by setting DOT_FONTPATH to the +# directory containing the font. + +DOT_FONTNAME = Helvetica + +# The DOT_FONTSIZE tag can be used to set the size of the font of dot graphs. +# The default size is 10pt. + +DOT_FONTSIZE = 10 + +# By default doxygen will tell dot to use the Helvetica font. +# If you specify a different font using DOT_FONTNAME you can use DOT_FONTPATH to +# set the path where dot can find it. + +DOT_FONTPATH = + +# If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for each documented class showing the direct and +# indirect inheritance relations. Setting this tag to YES will force the +# CLASS_DIAGRAMS tag to NO. + +CLASS_GRAPH = YES + +# If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for each documented class showing the direct and +# indirect implementation dependencies (inheritance, containment, and +# class references variables) of the class with other documented classes. + +COLLABORATION_GRAPH = YES + +# If the GROUP_GRAPHS and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for groups, showing the direct groups dependencies + +GROUP_GRAPHS = YES + +# If the UML_LOOK tag is set to YES doxygen will generate inheritance and +# collaboration diagrams in a style similar to the OMG's Unified Modeling +# Language. + +UML_LOOK = NO + +# If set to YES, the inheritance and collaboration graphs will show the +# relations between templates and their instances. + +TEMPLATE_RELATIONS = NO + +# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDE_GRAPH, and HAVE_DOT +# tags are set to YES then doxygen will generate a graph for each documented +# file showing the direct and indirect include dependencies of the file with +# other documented files. + +INCLUDE_GRAPH = YES + +# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDED_BY_GRAPH, and +# HAVE_DOT tags are set to YES then doxygen will generate a graph for each +# documented header file showing the documented files that directly or +# indirectly include this file. + +INCLUDED_BY_GRAPH = YES + +# If the CALL_GRAPH and HAVE_DOT options are set to YES then +# doxygen will generate a call dependency graph for every global function +# or class method. Note that enabling this option will significantly increase +# the time of a run. So in most cases it will be better to enable call graphs +# for selected functions only using the \callgraph command. + +CALL_GRAPH = NO + +# If the CALLER_GRAPH and HAVE_DOT tags are set to YES then +# doxygen will generate a caller dependency graph for every global function +# or class method. Note that enabling this option will significantly increase +# the time of a run. So in most cases it will be better to enable caller +# graphs for selected functions only using the \callergraph command. + +CALLER_GRAPH = NO + +# If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen +# will generate a graphical hierarchy of all classes instead of a textual one. + +GRAPHICAL_HIERARCHY = YES + +# If the DIRECTORY_GRAPH, SHOW_DIRECTORIES and HAVE_DOT tags are set to YES +# then doxygen will show the dependencies a directory has on other directories +# in a graphical way. The dependency relations are determined by the #include +# relations between the files in the directories. + +DIRECTORY_GRAPH = YES + +# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images +# generated by dot. Possible values are svg, png, jpg, or gif. +# If left blank png will be used. If you choose svg you need to set +# HTML_FILE_EXTENSION to xhtml in order to make the SVG files +# visible in IE 9+ (other browsers do not have this requirement). + +DOT_IMAGE_FORMAT = png + +# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to +# enable generation of interactive SVG images that allow zooming and panning. +# Note that this requires a modern browser other than Internet Explorer. +# Tested and working are Firefox, Chrome, Safari, and Opera. For IE 9+ you +# need to set HTML_FILE_EXTENSION to xhtml in order to make the SVG files +# visible. Older versions of IE do not have SVG support. + +INTERACTIVE_SVG = NO + +# The tag DOT_PATH can be used to specify the path where the dot tool can be +# found. If left blank, it is assumed the dot tool can be found in the path. + +DOT_PATH = + +# The DOTFILE_DIRS tag can be used to specify one or more directories that +# contain dot files that are included in the documentation (see the +# \dotfile command). + +DOTFILE_DIRS = + +# The MSCFILE_DIRS tag can be used to specify one or more directories that +# contain msc files that are included in the documentation (see the +# \mscfile command). + +MSCFILE_DIRS = + +# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of +# nodes that will be shown in the graph. If the number of nodes in a graph +# becomes larger than this value, doxygen will truncate the graph, which is +# visualized by representing a node as a red box. Note that doxygen if the +# number of direct children of the root node in a graph is already larger than +# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note +# that the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH. + +DOT_GRAPH_MAX_NODES = 50 + +# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the +# graphs generated by dot. A depth value of 3 means that only nodes reachable +# from the root by following a path via at most 3 edges will be shown. Nodes +# that lay further from the root node will be omitted. Note that setting this +# option to 1 or 2 may greatly reduce the computation time needed for large +# code bases. Also note that the size of a graph can be further restricted by +# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction. + +MAX_DOT_GRAPH_DEPTH = 0 + +# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent +# background. This is disabled by default, because dot on Windows does not +# seem to support this out of the box. Warning: Depending on the platform used, +# enabling this option may lead to badly anti-aliased labels on the edges of +# a graph (i.e. they become hard to read). + +DOT_TRANSPARENT = NO + +# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output +# files in one run (i.e. multiple -o and -T options on the command line). This +# makes dot run faster, but since only newer versions of dot (>1.8.10) +# support this, this feature is disabled by default. + +DOT_MULTI_TARGETS = YES + +# If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will +# generate a legend page explaining the meaning of the various boxes and +# arrows in the dot generated graphs. + +GENERATE_LEGEND = YES + +# If the DOT_CLEANUP tag is set to YES (the default) Doxygen will +# remove the intermediate dot files that are used to generate +# the various graphs. + +DOT_CLEANUP = YES diff --git a/svo/doc/.gitignore b/svo/doc/.gitignore new file mode 100644 index 0000000..c96a04f --- /dev/null +++ b/svo/doc/.gitignore @@ -0,0 +1,2 @@ +* +!.gitignore \ No newline at end of file diff --git a/svo/include/svo/bundle_adjustment.h b/svo/include/svo/bundle_adjustment.h new file mode 100644 index 0000000..215d563 --- /dev/null +++ b/svo/include/svo/bundle_adjustment.h @@ -0,0 +1,103 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_BUNDLE_ADJUSTMENT_H_ +#define SVO_BUNDLE_ADJUSTMENT_H_ + +#include + +namespace g2o { +class EdgeProjectXYZ2UV; +class SparseOptimizer; +} +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* core_kfs, + Map* map, + size_t& n_incorrect_edges_1, + size_t& n_incorrect_edges_2, + double& init_error, + double& final_error); + +/// Global bundle adjustment. +/// Optimizes the whole map. Is currently not used in SVO. +void globalBA(Map* map); + +/// Initialize g2o with solver type, optimization strategy and camera model. +void setupG2o(g2o::SparseOptimizer * optimizer); + +/// Run the optimization on the provided graph. +void runSparseBAOptimizer( + g2o::SparseOptimizer* optimizer, + unsigned int num_iter, + double& init_error, + double& final_error); + +/// Create a g2o vertice from a keyframe object. +g2oFrameSE3* createG2oFrameSE3( + Frame* kf, + size_t id, + bool fixed); + +/// Creates a g2o vertice from a mappoint object. +g2oPoint* createG2oPoint( + Vector3d pos, + size_t id, + bool fixed); + +/// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement. +g2oEdgeSE3* createG2oEdgeSE3( + g2oFrameSE3* v_kf, + g2oPoint* v_mp, + const Vector2d& f_up, + bool robust_kernel, + double huber_width, + double weight = 1); + +} // namespace ba +} // namespace svo + +#endif // SVO_BUNDLE_ADJUSTMENT_H_ diff --git a/svo/include/svo/config.h b/svo/include/svo/config.h new file mode 100644 index 0000000..3ad547b --- /dev/null +++ b/svo/include/svo/config.h @@ -0,0 +1,159 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef NSLAM_CONFIG_H_ +#define NSLAM_CONFIG_H_ + +#include +#include +#include + +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_ diff --git a/svo/include/svo/depth_filter.h b/svo/include/svo/depth_filter.h new file mode 100644 index 0000000..d943764 --- /dev/null +++ b/svo/include/svo/depth_filter.h @@ -0,0 +1,163 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_DEPTH_FILTER_H_ +#define SVO_DEPTH_FILTER_H_ + +#include +#include +#include +#include +#include +#include + +namespace svo { + +class Frame; +class Feature; +class Point; + +/// A seed is a probabilistic depth estimate for a single pixel. +struct Seed +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + static int batch_counter; + static int seed_counter; + int batch_id; //!< Batch id is the id of the keyframe for which the seed was created. + int id; //!< Seed ID, only used for visualization. + Feature* ftr; //!< Feature in the keyframe for which the depth should be computed. + float a; //!< a of Beta distribution: When high, probability of inlier is large. + float b; //!< b of Beta distribution: When high, probability of outlier is large. + float mu; //!< Mean of normal distribution. + float z_range; //!< Max range of the possible depth. + float sigma2; //!< Variance of normal distribution. + Matrix2d patch_cov; //!< Patch covariance in reference image. + Seed(Feature* ftr, float 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 lock_t; + typedef boost::function callback_t; + + /// Depth-filter config parameters + struct Options { + bool check_ftr_angle; //!< gradient features are only updated if the epipolar line is orthogonal to the gradient. + bool epi_search_1d; //!< restrict Gauss Newton in the epipolar search to the epipolar line. + bool verbose; //!< display output. + bool use_photometric_disparity_error; //!< use photometric disparity error instead of 1px error in tau computation. + int max_n_kfs; //!< maximum number of keyframes for which we maintain seeds. + double sigma_i_sq; //!< image noise. + double seed_convergence_sigma2_thresh; //!< threshold on depth uncertainty for convergence. + Options() + : check_ftr_angle(false), + epi_search_1d(false), + verbose(false), + use_photometric_disparity_error(false), + max_n_kfs(3), + sigma_i_sq(5e-4), + seed_convergence_sigma2_thresh(200.0) + {} + } options_; + + DepthFilter(feature_detection::DetectorPtr feature_detector, callback_t seed_converged_cb); + ~DepthFilter(); + + /// Start this thread when seed updating should be in a parallel thread. + void startThread(); + + /// Stop the parallel thread that is running. + void stopThread(); + + /// Add frame to the queue to be processed. + void addFrame(FramePtr frame); + + /// Add new keyframe to the queue + void addKeyframe(FramePtr frame, double depth_mean, double depth_min); + + /// Remove all seeds which are initialized from the specified keyframe. This + /// function is used to make sure that no seeds points to a non-existent frame + /// when a frame is removed from the map. + void removeKeyframe(FramePtr frame); + + /// If the map is reset, call this function such that we don't have pointers + /// to old frames. + void reset(); + + /// Returns a copy of the seeds belonging to frame. Thread-safe. + /// Can be used to compute the Next-Best-View in parallel. + /// IMPORTANT! Make sure you hold a valid reference counting pointer to frame + /// so it is not being deleted while you use it. + void getSeedsCopy(const FramePtr& frame, std::list& seeds); + + /// Return a reference to the seeds. This is NOT THREAD SAFE! + std::list >& getSeeds() { return seeds_; } + + /// 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 > seeds_; + boost::mutex seeds_mut_; + bool seeds_updating_halt_; //!< Set this value to true when seeds updating should be interrupted. + boost::thread* thread_; + std::queue frame_queue_; + boost::mutex frame_queue_mut_; + boost::condition_variable frame_queue_cond_; + FramePtr new_keyframe_; //!< Next keyframe to extract new seeds. + bool new_keyframe_set_; //!< Do we have a new keyframe to process?. + double new_keyframe_min_depth_; //!< Minimum depth in the new keyframe. Used for range in new seeds. + double new_keyframe_mean_depth_; //!< Maximum depth in the new keyframe. Used for range in new seeds. + vk::PerformanceMonitor permon_; //!< Separate performance monitor since the DepthFilter runs in a parallel thread. + + /// 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_ diff --git a/svo/include/svo/feature.h b/svo/include/svo/feature.h new file mode 100644 index 0000000..a0a30f4 --- /dev/null +++ b/svo/include/svo/feature.h @@ -0,0 +1,62 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_FEATURE_H_ +#define SVO_FEATURE_H_ + +#include + +namespace svo { + +/// A salient image region that is tracked across frames. +struct Feature +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + 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_ diff --git a/svo/include/svo/feature_alignment.h b/svo/include/svo/feature_alignment.h new file mode 100644 index 0000000..12523e3 --- /dev/null +++ b/svo/include/svo/feature_alignment.h @@ -0,0 +1,63 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_FEATURE_ALIGNMENT_H_ +#define SVO_FEATURE_ALIGNMENT_H_ + +#include + +namespace svo { + +/// Subpixel refinement of a reference feature patch with the current image. +/// Implements the inverse-compositional approach (see "Lucas-Kanade 20 Years on" +/// paper by Baker. +namespace feature_alignment { + +bool align1D( + const cv::Mat& cur_img, + const Vector2f& dir, // direction in which the patch is allowed to move + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate, + double& h_inv); + +bool align2D( + const cv::Mat& cur_img, + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate, + bool no_simd = false); + +bool align2D_SSE2( + const cv::Mat& cur_img, + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate); + +bool align2D_NEON( + const cv::Mat& cur_img, + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate); + +} // namespace feature_alignment +} // namespace svo + +#endif // SVO_FEATURE_ALIGNMENT_H_ diff --git a/svo/include/svo/feature_detection.h b/svo/include/svo/feature_detection.h new file mode 100644 index 0000000..a84586a --- /dev/null +++ b/svo/include/svo/feature_detection.h @@ -0,0 +1,76 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_FEATURE_DETECTION_H_ +#define SVO_FEATURE_DETECTION_H_ + +#include +#include + +namespace svo { + +/// Implementation of various feature detectors. +namespace feature_detection { + +/// Temporary container used for corner detection. Features are initialized from these. +// 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 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 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_ diff --git a/svo/include/svo/frame.h b/svo/include/svo/frame.h new file mode 100644 index 0000000..68aa548 --- /dev/null +++ b/svo/include/svo/frame.h @@ -0,0 +1,130 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_FRAME_H_ +#define SVO_FRAME_H_ + +#include +#include +#include +#include +#include +#include + +namespace g2o { +class VertexSE3Expmap; +} +typedef g2o::VertexSE3Expmap g2oFrameSE3; + +namespace svo { + +class Point; +class Frame; +struct Feature; + +typedef list Features; +typedef vector ImgPyr; + +/// A frame saves the image, the associated features and the estimated pose. +class Frame : boost::noncopyable +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + static int frame_counter_; //!< Counts the number of created frames. Used to set the unique id. + int id_; //!< Unique id of the frame. + double timestamp_; //!< Timestamp of when the image was recorded. + vk::AbstractCamera* cam_; //!< Camera model. + Sophus::SE3 T_f_w_; //!< Transform (f)rame from (w)orld. + Matrix Cov_; //!< Covariance. + ImgPyr img_pyr_; //!< Image Pyramid. + Features fts_; //!< List of features in the image. + vector key_pts_; //!< Five features and associated 3D points which are used to detect if two frames have overlapping field of view. + bool is_keyframe_; //!< Was this frames selected as keyframe? + g2oFrameSE3* v_kf_; //!< Temporary pointer to the g2o node object of the keyframe. + int last_published_ts_; //!< Timestamp of last publishing. + + Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp=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_ diff --git a/svo/include/svo/frame_handler_base.h b/svo/include/svo/frame_handler_base.h new file mode 100644 index 0000000..51286c7 --- /dev/null +++ b/svo/include/svo/frame_handler_base.h @@ -0,0 +1,116 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_FRAME_HANDLER_BASE_H_ +#define SVO_FRAME_HANDLER_BASE_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk { + class AbstractCamera; + class PerformanceMonitor; +} + +namespace svo { + +class Point; +class Matcher; +class DepthFilter; + +/// Base class for various VO pipelines. Manages the map and the state machine. +class FrameHandlerBase : boost::noncopyable +{ +public: + enum Stage { 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 acc_frame_timings_; //!< Total processing time of the last 10 frames, used to give some user feedback on the performance. + vk::RingBuffer acc_num_obs_; //!< Number of observed features of the last 10 frames, used to give some user feedback on the tracking performance. + size_t num_obs_last_; //!< Number of observations in the previous frame. + TrackingQuality tracking_quality_; //!< An estimate of the tracking quality based on the number of tracked features. + 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_ diff --git a/svo/include/svo/frame_handler_mono.h b/svo/include/svo/frame_handler_mono.h new file mode 100644 index 0000000..3dc80ab --- /dev/null +++ b/svo/include/svo/frame_handler_mono.h @@ -0,0 +1,75 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_FRAME_HANDLER_H_ +#define SVO_FRAME_HANDLER_H_ + +#include +#include +#include +#include + +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& coreKeyframes() { return core_kfs_; } + + /// Return the feature track to visualize the KLT tracking during initialization. + const vector& initFeatureTrackRefPx() const { return klt_homography_init_.px_ref_; } + const vector& initFeatureTrackCurPx() const { return klt_homography_init_.px_cur_; } + +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 core_kfs_; //!< Keyframes in the closer neighbourhood. + vector< pair > overlap_kfs_; //!< All keyframes with overlapping field of view. the paired number specifies how many common mappoints are observed TODO: why vector!? + initialization::KltHomographyInit klt_homography_init_; //!< Used to estimate pose of the first two keyframes by estimating a homography. + + /// 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_ diff --git a/svo/include/svo/global.h b/svo/include/svo/global.h new file mode 100644 index 0000000..01d8d52 --- /dev/null +++ b/svo/include/svo/global.h @@ -0,0 +1,95 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_GLOBAL_H_ +#define SVO_GLOBAL_H_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#ifdef USE_ROS + #include + #define SVO_DEBUG_STREAM(x) ROS_DEBUG_STREAM(x) + #define SVO_INFO_STREAM(x) ROS_INFO_STREAM(x) + #define SVO_WARN_STREAM(x) ROS_WARN_STREAM(x) + #define SVO_WARN_STREAM_THROTTLE(rate, x) ROS_WARN_STREAM_THROTTLE(rate, x) + #define SVO_ERROR_STREAM(x) ROS_ERROR_STREAM(x) +#else + #define SVO_INFO_STREAM(x) std::cerr<<"\033[0;0m[INFO] "< // Adapted from rosconsole. Copyright (c) 2008, Willow Garage, Inc. + #define SVO_WARN_STREAM_THROTTLE(rate, x) \ + do { \ + static double __log_stream_throttle__last_hit__ = 0.0; \ + std::chrono::time_point __log_stream_throttle__now__ = \ + std::chrono::system_clock::now(); \ + if (__log_stream_throttle__last_hit__ + rate <= \ + std::chrono::duration_cast( \ + __log_stream_throttle__now__.time_since_epoch()).count()) { \ + __log_stream_throttle__last_hit__ = \ + std::chrono::duration_cast( \ + __log_stream_throttle__now__.time_since_epoch()).count(); \ + SVO_WARN_STREAM(x); \ + } \ + } while(0) +#endif + +namespace svo +{ + using namespace Eigen; + using namespace Sophus; + + const double EPS = 0.0000000001; + const double PI = 3.14159265; + +#ifdef SVO_TRACE + extern vk::PerformanceMonitor* g_permon; + #define SVO_LOG(value) g_permon->log(std::string((#value)),(value)) + #define SVO_LOG2(value1, value2) SVO_LOG(value1); SVO_LOG(value2) + #define SVO_LOG3(value1, value2, value3) SVO_LOG2(value1, value2); SVO_LOG(value3) + #define SVO_LOG4(value1, value2, value3, value4) SVO_LOG2(value1, value2); SVO_LOG2(value3, value4) + #define SVO_START_TIMER(name) g_permon->startTimer((name)) + #define SVO_STOP_TIMER(name) g_permon->stopTimer((name)) +#else + #define SVO_LOG(v) + #define SVO_LOG2(v1, v2) + #define SVO_LOG3(v1, v2, v3) + #define SVO_LOG4(v1, v2, v3, v4) + #define SVO_START_TIMER(name) + #define SVO_STOP_TIMER(name) +#endif + + class Frame; + typedef boost::shared_ptr FramePtr; +} // namespace svo + +#endif // SVO_GLOBAL_H_ diff --git a/svo/include/svo/initialization.h b/svo/include/svo/initialization.h new file mode 100644 index 0000000..4513086 --- /dev/null +++ b/svo/include/svo/initialization.h @@ -0,0 +1,83 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_INITIALIZATION_H +#define SVO_INITIALIZATION_H + +#include + +namespace svo { + +class FrameHandlerMono; + +/// Bootstrapping the map from the first two views. +namespace initialization { + +enum InitResult { FAILURE, NO_KEYFRAME, SUCCESS }; + +/// Tracks features using Lucas-Kanade tracker and then estimates a homography. +class KltHomographyInit { + friend class svo::FrameHandlerMono; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + FramePtr frame_ref_; + KltHomographyInit() {}; + ~KltHomographyInit() {}; + InitResult addFirstFrame(FramePtr frame_ref); + InitResult addSecondFrame(FramePtr frame_ref); + void reset(); + +protected: + vector px_ref_; //!< keypoints to be tracked in reference frame. + vector px_cur_; //!< tracked keypoints in current frame. + vector f_ref_; //!< bearing vectors corresponding to the keypoints in the reference image. + vector f_cur_; //!< bearing vectors corresponding to the keypoints in the current image. + vector disparities_; //!< disparity between first and second frame. + vector inliers_; //!< inliers after the geometric check (e.g., Homography). + vector xyz_in_cur_; //!< 3D points computed during the geometric check. + SE3 T_cur_from_ref_; //!< computed transformation between the first two frames. +}; + +/// Detect Fast corners in the image. +void detectFeatures( + FramePtr frame, + vector& px_vec, + vector& f_vec); + +/// Compute optical flow (Lucas Kanade) for selected keypoints. +void trackKlt( + FramePtr frame_ref, + FramePtr frame_cur, + vector& px_ref, + vector& px_cur, + vector& f_ref, + vector& f_cur, + vector& disparities); + +void computeHomography( + const vector& f_ref, + const vector& f_cur, + double focal_length, + double reprojection_threshold, + vector& inliers, + vector& xyz_in_cur, + SE3& T_cur_from_ref); + +} // namespace initialization +} // namespace nslam + +#endif // NSLAM_INITIALIZATION_H diff --git a/svo/include/svo/map.h b/svo/include/svo/map.h new file mode 100644 index 0000000..076ee01 --- /dev/null +++ b/svo/include/svo/map.h @@ -0,0 +1,134 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_MAP_H_ +#define SVO_MAP_H_ + +#include +#include +#include +#include + +namespace svo { + +class Point; +class Feature; +class Seed; + +/// Container for converged 3D points that are not already assigned to two keyframes. +class MapPointCandidates +{ +public: + typedef pair PointCandidate; + typedef list PointCandidateList; + + /// The depth-filter is running in a parallel thread and fills the canidate list. + /// This mutex controls concurrent access to point_candidates. + boost::mutex mut_; + + /// Candidate points are created from converged seeds. + /// Until the next keyframe, these points can be used for reprojection and pose optimization. + PointCandidateList candidates_; + list< Point* > trash_points_; + + MapPointCandidates(); + ~MapPointCandidates(); + + /// Add a candidate point. + void newCandidatePoint(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 >& 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_ diff --git a/svo/include/svo/matcher.h b/svo/include/svo/matcher.h new file mode 100644 index 0000000..b79340b --- /dev/null +++ b/svo/include/svo/matcher.h @@ -0,0 +1,105 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_MATCHER_H_ +#define SVO_MATCHER_H_ + +#include + +namespace vk { + class AbstractCamera; +} + +namespace 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_ diff --git a/svo/include/svo/point.h b/svo/include/svo/point.h new file mode 100644 index 0000000..1fa26f0 --- /dev/null +++ b/svo/include/svo/point.h @@ -0,0 +1,83 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_POINT_H_ +#define SVO_POINT_H_ + +#include +#include + +namespace g2o { + class VertexSBAPointXYZ; +} +typedef g2o::VertexSBAPointXYZ g2oPoint; + +namespace svo { + +class Feature; + +typedef Matrix Matrix23d; + +/// A 3D point 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 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_ diff --git a/svo/include/svo/pose_optimizer.h b/svo/include/svo/pose_optimizer.h new file mode 100644 index 0000000..575da32 --- /dev/null +++ b/svo/include/svo/pose_optimizer.h @@ -0,0 +1,77 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_POSE_OPTIMIZER_H_ +#define SVO_POSE_OPTIMIZER_H_ + +#include + +namespace svo { + +using namespace Eigen; +using namespace Sophus; +using namespace std; + +typedef Matrix Matrix6d; +typedef Matrix Matrix26d; +typedef Matrix Vector6d; + +class Point; + +/// Motion-only bundle adjustment. Minimize the reprojection error of a single frame. +namespace pose_optimizer { + +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_ diff --git a/svo/include/svo/reprojection.h b/svo/include/svo/reprojection.h new file mode 100644 index 0000000..a6ce153 --- /dev/null +++ b/svo/include/svo/reprojection.h @@ -0,0 +1,98 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_REPROJECTION_H_ +#define SVO_REPROJECTION_H_ + +#include +#include + +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 > Cell; +typedef std::vector CandidateGrid; + +/// The grid stores a set of candidate matches. For every grid cell we try to find one match. +struct Grid +{ + CandidateGrid cells; + vector cell_order; + int cell_size; + int grid_n_cols; + int grid_n_rows; + Grid(); + 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 >& 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_ diff --git a/svo/include/svo/sparse_img_align.h b/svo/include/svo/sparse_img_align.h new file mode 100644 index 0000000..7df0456 --- /dev/null +++ b/svo/include/svo/sparse_img_align.h @@ -0,0 +1,78 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_SPARSE_IMG_ALIGN_H_ +#define SVO_SPARSE_IMG_ALIGN_H_ + +#include +#include +#include + +namespace vk { +class AbstractCamera; +} + +namespace svo { + +class Feature; + +/// Optimize the pose of the frame by minimizing the photometric error of feature patches. +class SparseImgAlign : public vk::NLLSSolver<6, SE3> +{ + static const int patch_halfsize_ = 2; + static const int patch_size_ = 2*patch_halfsize_; + static const int patch_area_ = patch_size_*patch_size_; +public: + + 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 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 visible_fts_; + + void precomputeReferencePatches(); + virtual double computeResiduals(const SE3& model, bool linearize_system, bool compute_weight_scale = false); + virtual int solve(); + virtual void update (const ModelType& old_model, ModelType& new_model); + virtual void startIteration(); + virtual void finishIteration(); +}; + +} // namespace svo + +#endif // SVO_SPARSE_IMG_ALIGN_H_ diff --git a/svo/package.xml b/svo/package.xml new file mode 100644 index 0000000..c2997c2 --- /dev/null +++ b/svo/package.xml @@ -0,0 +1,24 @@ + + + svo + 0.1.0 + + Monocular Visual Odometry + + Christian Forster + Christian Forster + GPLv3 + + + catkin + + + roscpp + vikit_common + vikit_ros + + + roscpp + vikit_common + vikit_ros + \ No newline at end of file diff --git a/svo/src/bundle_adjustment.cpp b/svo/src/bundle_adjustment.cpp new file mode 100644 index 0000000..a2075f4 --- /dev/null +++ b/svo/src/bundle_adjustment.cpp @@ -0,0 +1,435 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define SCHUR_TRICK 1 + +namespace svo { +namespace ba { + +void +twoViewBA(Frame* frame1, Frame* frame2, + double reproj_thresh, + Map* map) +{ + // scale reprojection threshold in pixels to unit plane + reproj_thresh /= frame1->cam_->errorMultiplier2(); + + // init g2o + g2o::SparseOptimizer optimizer; + setupG2o(&optimizer); + + list edges; + size_t v_id = 0; + + // New Keyframe Vertex 1: This Keyframe is set to fixed! + g2oFrameSE3* v_frame1 = createG2oFrameSE3(frame1, v_id++, true); + optimizer.addVertex(v_frame1); + + // New Keyframe Vertex 2 + g2oFrameSE3* v_frame2 = createG2oFrameSE3(frame2, v_id++, false); + optimizer.addVertex(v_frame2); + + // Create Point Vertices + for(Features::iterator it_ftr=frame1->fts_.begin(); it_ftr!=frame1->fts_.end(); ++it_ftr) + { + Point* pt = (*it_ftr)->point; + if(pt == NULL) + continue; + g2oPoint* v_pt = createG2oPoint(pt->pos_, v_id++, false); + optimizer.addVertex(v_pt); + pt->v_pt_ = v_pt; + g2oEdgeSE3* e = createG2oEdgeSE3(v_frame1, v_pt, vk::project2d((*it_ftr)->f), true, reproj_thresh*Config::lobaRobustHuberWidth()); + optimizer.addEdge(e); + edges.push_back(EdgeContainerSE3(e, frame1, *it_ftr)); // TODO feature now links to frame, so we can simplify edge container! + + // find at which index the second frame observes the point + Feature* ftr_frame2 = pt->findFrameRef(frame2); + e = createG2oEdgeSE3(v_frame2, v_pt, vk::project2d(ftr_frame2->f), true, reproj_thresh*Config::lobaRobustHuberWidth()); + optimizer.addEdge(e); + edges.push_back(EdgeContainerSE3(e, frame2, ftr_frame2)); + } + + // Optimization + double init_error, final_error; + runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error); + printf("2-View BA: Error before/after = %f / %f\n", init_error, final_error); + + // Update Keyframe Positions + frame1->T_f_w_.rotation_matrix() = v_frame1->estimate().rotation().toRotationMatrix(); + frame1->T_f_w_.translation() = v_frame1->estimate().translation(); + frame2->T_f_w_.rotation_matrix() = v_frame2->estimate().rotation().toRotationMatrix(); + frame2->T_f_w_.translation() = v_frame2->estimate().translation(); + + // Update Mappoint Positions + for(Features::iterator it=frame1->fts_.begin(); it!=frame1->fts_.end(); ++it) + { + if((*it)->point == NULL) + continue; + (*it)->point->pos_ = (*it)->point->v_pt_->estimate(); + (*it)->point->v_pt_ = NULL; + } + + // Find Mappoints with too large reprojection error + const double reproj_thresh_squared = reproj_thresh*reproj_thresh; + size_t n_incorrect_edges = 0; + for(list::iterator it_e = edges.begin(); it_e != edges.end(); ++it_e) + if(it_e->edge_->chi2() > reproj_thresh_squared) + { + if(it_e->feature_->point != NULL) + { + map->safeDeletePoint(it_e->feature_->point); + it_e->feature_->point = NULL; + } + ++n_incorrect_edges; + } + + printf("2-View BA: Wrong edges = %zu\n", n_incorrect_edges); +} + +void +localBA(Frame* center_kf, + set* core_kfs, + Map* map, + size_t& n_incorrect_edges_1, + size_t& n_incorrect_edges_2, + double& init_error, + double& final_error) +{ + + // init g2o + g2o::SparseOptimizer optimizer; + setupG2o(&optimizer); + + list edges; + set mps; + list neib_kfs; + size_t v_id = 0; + size_t n_mps = 0; + size_t n_fix_kfs = 0; + size_t n_var_kfs = 1; + size_t n_edges = 0; + n_incorrect_edges_1 = 0; + n_incorrect_edges_2 = 0; + + // Add all core keyframes + for(set::iterator it_kf = core_kfs->begin(); it_kf != core_kfs->end(); ++it_kf) + { + g2oFrameSE3* v_kf = createG2oFrameSE3(it_kf->get(), v_id++, false); + (*it_kf)->v_kf_ = v_kf; + ++n_var_kfs; + assert(optimizer.addVertex(v_kf)); + + // all points that the core keyframes observe are also optimized: + for(Features::iterator it_pt=(*it_kf)->fts_.begin(); it_pt!=(*it_kf)->fts_.end(); ++it_pt) + if((*it_pt)->point != NULL) + mps.insert((*it_pt)->point); + } + + // Now go throug all the points and add a measurement. Add a fixed neighbour + // Keyframe if it is not in the set of core kfs + double reproj_thresh_2 = Config::lobaThresh() / center_kf->cam_->errorMultiplier2(); + double reproj_thresh_1 = Config::poseOptimThresh() / center_kf->cam_->errorMultiplier2(); + double reproj_thresh_1_squared = reproj_thresh_1*reproj_thresh_1; + for(set::iterator it_pt = mps.begin(); it_pt!=mps.end(); ++it_pt) + { + // Create point vertex + g2oPoint* v_pt = createG2oPoint((*it_pt)->pos_, v_id++, false); + (*it_pt)->v_pt_ = v_pt; + assert(optimizer.addVertex(v_pt)); + ++n_mps; + + // Add edges + 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(it->second); + if (v->dimension() == 3 && v->edges().size() >= 2) + points.push_back(v); + } + structure_only_ba.calc(points, 10); + + // Optimization + if(Config::lobaNumIter() > 0) + runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error); + + // Update Keyframes + for(set::iterator it = core_kfs->begin(); it != core_kfs->end(); ++it) + { + (*it)->T_f_w_ = SE3( (*it)->v_kf_->estimate().rotation(), + (*it)->v_kf_->estimate().translation()); + (*it)->v_kf_ = NULL; + } + + for(list::iterator it = neib_kfs.begin(); it != neib_kfs.end(); ++it) + (*it)->v_kf_ = NULL; + + // Update Mappoints + for(set::iterator it = mps.begin(); it != mps.end(); ++it) + { + (*it)->pos_ = (*it)->v_pt_->estimate(); + (*it)->v_pt_ = NULL; + } + + // Remove Measurements with too large reprojection error + double reproj_thresh_2_squared = reproj_thresh_2*reproj_thresh_2; + for(list::iterator it = edges.begin(); it != edges.end(); ++it) + { + if(it->edge_->chi2() > reproj_thresh_2_squared) //*(1<feature_->level)) + { + map->removePtFrameRef(it->frame_, it->feature_); + ++n_incorrect_edges_2; + } + } + + // TODO: delete points and edges! + init_error = sqrt(init_error)*center_kf->cam_->errorMultiplier2(); + final_error = sqrt(final_error)*center_kf->cam_->errorMultiplier2(); +} + +void +globalBA(Map* map) +{ + // init g2o + g2o::SparseOptimizer optimizer; + setupG2o(&optimizer); + + list edges; + list< pair > incorrect_edges; + + // Go through all Keyframes + size_t v_id = 0; + double reproj_thresh_2 = Config::lobaThresh() / map->lastKeyframe()->cam_->errorMultiplier2(); + double reproj_thresh_1_squared = Config::poseOptimThresh() / map->lastKeyframe()->cam_->errorMultiplier2(); + reproj_thresh_1_squared *= reproj_thresh_1_squared; + for(list::iterator it_kf = map->keyframes_.begin(); + it_kf != map->keyframes_.end(); ++it_kf) + { + // New Keyframe Vertex + g2oFrameSE3* v_kf = createG2oFrameSE3(it_kf->get(), v_id++, false); + (*it_kf)->v_kf_ = v_kf; + optimizer.addVertex(v_kf); + for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr) + { + // for each keyframe add edges to all observed mapoints + Point* mp = (*it_ftr)->point; + if(mp == NULL) + continue; + g2oPoint* v_mp = mp->v_pt_; + if(v_mp == NULL) + { + // mappoint-vertex doesn't exist yet. create a new one: + v_mp = createG2oPoint(mp->pos_, v_id++, false); + mp->v_pt_ = v_mp; + optimizer.addVertex(v_mp); + } + + // Due to merging of mappoints it is possible that references in kfs suddenly + // have a very large reprojection error which may result in distorted results. + Vector2d error = vk::project2d((*it_ftr)->f) - vk::project2d((*it_kf)->w2f(mp->pos_)); + if(error.squaredNorm() > reproj_thresh_1_squared) + incorrect_edges.push_back(pair(*it_kf, *it_ftr)); + else + { + g2oEdgeSE3* e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d((*it_ftr)->f), + true, + reproj_thresh_2*Config::lobaRobustHuberWidth()); + + edges.push_back(EdgeContainerSE3(e, it_kf->get(), *it_ftr)); + optimizer.addEdge(e); + } + } + } + + // Optimization + double init_error=0.0, final_error=0.0; + if(Config::lobaNumIter() > 0) + runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error); + + // Update Keyframe and MapPoint Positions + for(list::iterator it_kf = map->keyframes_.begin(); + it_kf != map->keyframes_.end(); ++it_kf) + { + (*it_kf)->T_f_w_ = SE3( (*it_kf)->v_kf_->estimate().rotation(), + (*it_kf)->v_kf_->estimate().translation()); + (*it_kf)->v_kf_ = NULL; + for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr) + { + Point* mp = (*it_ftr)->point; + if(mp == NULL) + continue; + if(mp->v_pt_ == NULL) + continue; // mp was updated before + mp->pos_ = mp->v_pt_->estimate(); + mp->v_pt_ = NULL; + } + } + + // Remove Measurements with too large reprojection error + for(list< pair >::iterator it=incorrect_edges.begin(); + it!=incorrect_edges.end(); ++it) + map->removePtFrameRef(it->first.get(), it->second); + + double reproj_thresh_2_squared = reproj_thresh_2*reproj_thresh_2; + for(list::iterator it = edges.begin(); it != edges.end(); ++it) + { + if(it->edge_->chi2() > reproj_thresh_2_squared) + { + map->removePtFrameRef(it->frame_, it->feature_); + } + } +} + +void +setupG2o(g2o::SparseOptimizer * optimizer) +{ + // TODO: What's happening with all this HEAP stuff? Memory Leak? + optimizer->setVerbose(false); + +#if SCHUR_TRICK + // solver + g2o::BlockSolver_6_3::LinearSolverType* linearSolver; + linearSolver = new g2o::LinearSolverCholmod(); + //linearSolver = new g2o::LinearSolverCSparse(); + + g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver); + g2o::OptimizationAlgorithmLevenberg * solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); +#else + g2o::BlockSolverX::LinearSolverType * linearSolver; + linearSolver = new g2o::LinearSolverCholmod(); + //linearSolver = new g2o::LinearSolverCSparse(); + g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver); + g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); +#endif + + solver->setMaxTrialsAfterFailure(5); + optimizer->setAlgorithm(solver); + + // setup camera + g2o::CameraParameters * cam_params = new g2o::CameraParameters(1.0, Vector2d(0.,0.), 0.); + cam_params->setId(0); + if (!optimizer->addParameter(cam_params)) { + assert(false); + } +} + +void +runSparseBAOptimizer(g2o::SparseOptimizer* optimizer, + unsigned int num_iter, + double& init_error, double& final_error) +{ + optimizer->initializeOptimization(); + optimizer->computeActiveErrors(); + init_error = optimizer->activeChi2(); + optimizer->optimize(num_iter); + final_error = optimizer->activeChi2(); +} + +g2oFrameSE3* +createG2oFrameSE3(Frame* frame, size_t id, bool fixed) +{ + g2oFrameSE3* v = new g2oFrameSE3(); + v->setId(id); + v->setFixed(fixed); + v->setEstimate(g2o::SE3Quat(frame->T_f_w_.unit_quaternion(), frame->T_f_w_.translation())); + return v; +} + +g2oPoint* +createG2oPoint(Vector3d pos, + size_t id, + bool fixed) +{ + g2oPoint* v = new g2oPoint(); + v->setId(id); +#if SCHUR_TRICK + v->setMarginalized(true); +#endif + v->setFixed(fixed); + v->setEstimate(pos); + return v; +} + +g2oEdgeSE3* +createG2oEdgeSE3( g2oFrameSE3* v_frame, + g2oPoint* v_point, + const Vector2d& f_up, + bool robust_kernel, + double huber_width, + double weight) +{ + g2oEdgeSE3* e = new g2oEdgeSE3(); + e->setVertex(0, dynamic_cast(v_point)); + e->setVertex(1, dynamic_cast(v_frame)); + e->setMeasurement(f_up); + e->information() = weight * Eigen::Matrix2d::Identity(2,2); + g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber(); // TODO: memory leak + rk->setDelta(huber_width); + e->setRobustKernel(rk); + e->setParameterId(0, 0); //old: e->setId(v_point->id()); + return e; +} + +} // namespace ba +} // namespace svo diff --git a/svo/src/config.cpp b/svo/src/config.cpp new file mode 100644 index 0000000..c8593c4 --- /dev/null +++ b/svo/src/config.cpp @@ -0,0 +1,95 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifdef USE_ROS +#include +#endif +#include + +namespace svo { + +Config::Config() : +#ifdef USE_ROS + trace_name(vk::getParam("svo/trace_name", "svo")), + trace_dir(vk::getParam("svo/trace_dir", "/tmp")), + n_pyr_levels(vk::getParam("svo/n_pyr_levels", 3)), + use_imu(vk::getParam("svo/use_imu", false)), + core_n_kfs(vk::getParam("svo/core_n_kfs", 3)), + map_scale(vk::getParam("svo/map_scale", 1.0)), + grid_size(vk::getParam("svo/grid_size", 30)), + init_min_disparity(vk::getParam("svo/init_min_disparity", 50.0)), + init_min_tracked(vk::getParam("svo/init_min_tracked", 50)), + init_min_inliers(vk::getParam("svo/init_min_inliers", 40)), + klt_max_level(vk::getParam("svo/klt_max_level", 4)), + klt_min_level(vk::getParam("svo/klt_min_level", 2)), + reproj_thresh(vk::getParam("svo/reproj_thresh", 2.0)), + poseoptim_thresh(vk::getParam("svo/poseoptim_thresh", 2.0)), + poseoptim_num_iter(vk::getParam("svo/poseoptim_num_iter", 10)), + structureoptim_max_pts(vk::getParam("svo/structureoptim_max_pts", 20)), + structureoptim_num_iter(vk::getParam("svo/structureoptim_num_iter", 5)), + loba_thresh(vk::getParam("svo/loba_thresh", 2.0)), + loba_robust_huber_width(vk::getParam("svo/loba_robust_huber_width", 1.0)), + loba_num_iter(vk::getParam("svo/loba_num_iter", 0)), + kfselect_mindist(vk::getParam("svo/kfselect_mindist", 0.12)), + triang_min_corner_score(vk::getParam("svo/triang_min_corner_score", 20.0)), + triang_half_patch_size(vk::getParam("svo/triang_half_patch_size", 4)), + subpix_n_iter(vk::getParam("svo/subpix_n_iter", 10)), + max_n_kfs(vk::getParam("svo/max_n_kfs", 10)), + img_imu_delay(vk::getParam("svo/img_imu_delay", 0.0)), + max_fts(vk::getParam("svo/max_fts", 120)), + quality_min_fts(vk::getParam("svo/quality_min_fts", 50)), + quality_max_drop_fts(vk::getParam("svo/quality_max_drop_fts", 40)) +#else + trace_name("svo"), + trace_dir("/tmp"), + n_pyr_levels(3), + use_imu(false), + core_n_kfs(3), + map_scale(1.0), + grid_size(25), + init_min_disparity(50.0), + init_min_tracked(50), + init_min_inliers(40), + klt_max_level(4), + klt_min_level(2), + reproj_thresh(2.0), + poseoptim_thresh(2.0), + poseoptim_num_iter(10), + structureoptim_max_pts(20), + structureoptim_num_iter(5), + loba_thresh(2.0), + loba_robust_huber_width(1.0), + loba_num_iter(0), + kfselect_mindist(0.12), + triang_min_corner_score(20.0), + triang_half_patch_size(4), + subpix_n_iter(10), + max_n_kfs(0), + img_imu_delay(0.0), + max_fts(120), + quality_min_fts(50), + quality_max_drop_fts(40) +#endif +{} + +Config& Config::getInstance() +{ + static Config instance; // Instantiated on first use and guaranteed to be destroyed + return instance; +} + +} // namespace svo + diff --git a/svo/src/depth_filter.cpp b/svo/src/depth_filter.cpp new file mode 100644 index 0000000..8acd71d --- /dev/null +++ b/svo/src/depth_filter.cpp @@ -0,0 +1,362 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +int Seed::batch_counter = 0; +int Seed::seed_counter = 0; + +Seed::Seed(Feature* ftr, float 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 "<::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::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())) { + ++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 "<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& seeds) +{ + lock_t lock(seeds_mut_); + for(std::list::iterator it=seeds_.begin(); it!=seeds_.end(); ++it) + { + if (it->ftr->frame == frame.get()) + seeds.push_back(*it); + } +} + +void DepthFilter::updateSeed(const float x, const float tau2, Seed* seed) +{ + float norm_scale = sqrt(seed->sigma2 + tau2); + if(isnanf(norm_scale)) + return; + boost::math::normal_distribution nd(seed->mu, norm_scale); + float s2 = 1./(1./seed->sigma2 + 1./tau2); + float m = s2*(seed->mu/seed->sigma2 + x/tau2); + float C1 = seed->a/(seed->a+seed->b) * boost::math::pdf(nd, x); + float C2 = seed->b/(seed->a+seed->b) * 1./seed->z_range; + float normalization_constant = C1 + C2; + C1 /= normalization_constant; + C2 /= normalization_constant; + float f = C1*(seed->a+1.)/(seed->a+seed->b+1.) + C2*seed->a/(seed->a+seed->b+1.); + float e = C1*(seed->a+1.)*(seed->a+2.)/((seed->a+seed->b+1.)*(seed->a+seed->b+2.)) + + C2*seed->a*(seed->a+1.0f)/((seed->a+seed->b+1.0f)*(seed->a+seed->b+2.0f)); + + // update parameters + float mu_new = C1*m+C2*seed->mu; + seed->sigma2 = C1*(s2 + m*m) + C2*(seed->sigma2 + seed->mu*seed->mu) - mu_new*mu_new; + seed->mu = mu_new; + seed->a = (e-f)/(f-e/f); + seed->b = seed->a*(1.0f-f)/f; +} + +double DepthFilter::computeTau( + const SE3& T_ref_cur, + const Vector3d& f, + const double z, + const double px_error_angle) +{ + Vector3d t(T_ref_cur.translation()); + Vector3d a = f*z-t; + double t_norm = t.norm(); + double a_norm = a.norm(); + double alpha = acos(f.dot(t)/t_norm); // dot product + double beta = acos(a.dot(-t)/(t_norm*a_norm)); // dot product + double beta_plus = beta + px_error_angle; + double gamma_plus = PI-alpha-beta_plus; // triangle angles sum to PI + double z_plus = t_norm*sin(beta_plus)/sin(gamma_plus); // law of sines + return (z_plus - z); // tau +} + +} // namespace svo diff --git a/svo/src/feature_alignment.cpp b/svo/src/feature_alignment.cpp new file mode 100644 index 0000000..15dcee3 --- /dev/null +++ b/svo/src/feature_alignment.cpp @@ -0,0 +1,601 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifdef __SSE2__ +#include +#endif +#ifdef __ARM_NEON__ +#include +#endif +#include + +namespace svo { +namespace feature_alignment { + +#define SUBPIX_VERBOSE 0 + +bool align1D( + const cv::Mat& cur_img, + const Vector2f& dir, // direction in which the patch is allowed to move + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate, + double& h_inv) +{ + const int halfpatch_size_ = 4; + const int patch_size = 8; + const int patch_area = 64; + bool converged=false; + + // compute derivative of template and prepare inverse compositional + float __attribute__((__aligned__(16))) ref_patch_dv[patch_area]; + Matrix2f H; H.setZero(); + + // compute gradient and hessian + const int ref_step = patch_size+2; + float* it_dv = ref_patch_dv; + for(int y=0; y= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_) + break; + + if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check + return false; + + // compute interpolation weights + float subpix_x = u-u_r; + float subpix_y = v-v_r; + float wTL = (1.0-subpix_x)*(1.0-subpix_y); + float wTR = subpix_x * (1.0-subpix_y); + float wBL = (1.0-subpix_x)*subpix_y; + float wBR = subpix_x * subpix_y; + + // loop through search_patch, interpolate + uint8_t* it_ref = ref_patch; + float* it_ref_dv = ref_patch_dv; + float new_chi2 = 0.0; + Vector2f Jres; Jres.setZero(); + for(int y=0; y 0 && new_chi2 > chi2) + { +#if SUBPIX_VERBOSE + cout << "error increased." << endl; +#endif + u -= update[0]; + v -= update[1]; + break; + } + + chi2 = new_chi2; + update = Hinv * Jres; + u += update[0]*dir[0]; + v += update[0]*dir[1]; + mean_diff += update[1]; + +#if SUBPIX_VERBOSE + cout << "Iter " << iter << ":" + << "\t u=" << u << ", v=" << v + << "\t update = " << update[0] << ", " << update[1] + << "\t new chi2 = " << new_chi2 << endl; +#endif + + if(update[0]*update[0]+update[1]*update[1] < min_update_squared) + { +#if SUBPIX_VERBOSE + cout << "converged." << endl; +#endif + converged=true; + break; + } + } + + cur_px_estimate << u, v; + return converged; +} + +bool align2D( + const cv::Mat& cur_img, + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate, + bool no_simd) +{ +#ifdef __ARM_NEON__ + if(!no_simd) + return align2D_NEON(cur_img, ref_patch_with_border, ref_patch, n_iter, cur_px_estimate); +#endif + + const int halfpatch_size_ = 4; + const int patch_size_ = 8; + const int patch_area_ = 64; + bool converged=false; + + // compute derivative of template and prepare inverse compositional + float __attribute__((__aligned__(16))) ref_patch_dx[patch_area_]; + float __attribute__((__aligned__(16))) ref_patch_dy[patch_area_]; + Matrix3f H; H.setZero(); + + // compute gradient and hessian + const int ref_step = patch_size_+2; + float* it_dx = ref_patch_dx; + float* it_dy = ref_patch_dy; + for(int y=0; y= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_) + break; + + if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check + return false; + + // compute interpolation weights + float subpix_x = u-u_r; + float subpix_y = v-v_r; + float wTL = (1.0-subpix_x)*(1.0-subpix_y); + float wTR = subpix_x * (1.0-subpix_y); + float wBL = (1.0-subpix_x)*subpix_y; + float wBR = subpix_x * subpix_y; + + // loop through search_patch, interpolate + uint8_t* it_ref = ref_patch; + float* it_ref_dx = ref_patch_dx; + float* it_ref_dy = ref_patch_dy; + float new_chi2 = 0.0; + Vector3f Jres; Jres.setZero(); + for(int y=0; y 0 && new_chi2 > chi2) + { +#if SUBPIX_VERBOSE + cout << "error increased." << endl; +#endif + u -= update[0]; + v -= update[1]; + break; + } + + chi2 = new_chi2; + update = Hinv * Jres; + u += update[0]; + v += update[1]; + mean_diff += update[2]; + +#if SUBPIX_VERBOSE + cout << "Iter " << iter << ":" + << "\t u=" << u << ", v=" << v + << "\t update = " << update[0] << ", " << update[1] + << "\t new chi2 = " << new_chi2 << endl; +#endif + + if(update[0]*update[0]+update[1]*update[1] < min_update_squared) + { +#if SUBPIX_VERBOSE + cout << "converged." << endl; +#endif + converged=true; + break; + } + } + + cur_px_estimate << u, v; + return converged; +} + +#define DESCALE(x,n) (((x) + (1 << ((n)-1))) >> (n)) // rounds to closest integer and descales + +bool align2D_SSE2( + const cv::Mat& cur_img, + uint8_t* ref_patch_with_border, + uint8_t* ref_patch, + const int n_iter, + Vector2d& cur_px_estimate) +{ + // TODO: This function should not be used as the alignment is not robust to illumination changes! + const int halfpatch_size = 4; + const int patch_size = 8; + const int patch_area = 64; + bool converged=false; + const int W_BITS = 14; + + // compute derivative of template and prepare inverse compositional + int16_t __attribute__((__aligned__(16))) ref_patch_dx[patch_area]; + int16_t __attribute__((__aligned__(16))) ref_patch_dy[patch_area]; + + // compute gradient and hessian + const int ref_step = patch_size+2; + int16_t* it_dx = ref_patch_dx; + int16_t* it_dy = ref_patch_dy; + float A11=0, A12=0, A22=0; + for(int y=0; y(it[1]) - it[-1]; + int16_t dy = static_cast(it[ref_step]) - it[-ref_step]; + *it_dx = dx; + *it_dy = dy; // we are missing a factor 1/2 + A11 += static_cast(dx*dx); // we are missing a factor 1/4 + A12 += static_cast(dx*dy); + A22 += static_cast(dy*dy); + } + } + + // Compute pixel location in new image: + float u = cur_px_estimate.x(); + float v = cur_px_estimate.y(); + + // termination condition + const float min_update_squared = 0.03*0.03; + const int cur_step = cur_img.step.p[0]; + const float Dinv = 1.0f/(A11*A22 - A12*A12); // we are missing an extra factor 16 + float chi2 = 0; + float update_u = 0, update_v = 0; + + for(int iter = 0; iter= cur_img.cols-halfpatch_size || v_r >= cur_img.rows-halfpatch_size) + break; + + if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check + return false; + + float subpix_x = u-u_r; + float subpix_y = v-v_r; + float b1=0, b2=0; + float new_chi2 = 0.0; + +#ifdef __SSE2__ + // compute bilinear interpolation weights + int wTL = static_cast((1.0f-subpix_x)*(1.0f-subpix_y)*(1 << W_BITS)); + int wTR = static_cast(subpix_x * (1.0f-subpix_y)*(1 << W_BITS)); + int wBL = static_cast((1.0f-subpix_x)*subpix_y*(1 << W_BITS)); + int wBR = (1 << W_BITS) - wTL - wTR - wBL; + + __m128i qw0 = _mm_set1_epi32(wTL + (wTR << 16)); // Sets the 4 signed 32-bit integer values to [wTL, wTR]. + __m128i qw1 = _mm_set1_epi32(wBL + (wBR << 16)); + __m128i z = _mm_setzero_si128(); + __m128 qb0 = _mm_setzero_ps(); // 4 floats + __m128 qb1 = _mm_setzero_ps(); // 4 floats + __m128i qdelta = _mm_set1_epi32(1 << (W_BITS-1)); + for(int y=0; y(it[1] - it[-1]); + *it_dy = static_cast(it[ref_step] - it[-ref_step]); // divide by 2 missing + Vector3f J(*it_dx, *it_dy, 1.0f); + H += J*J.transpose(); + } + } + Matrix3f Hinv = H.inverse(); + float mean_diff = 0.0; + + // Compute pixel location in new image: + float u = cur_px_estimate.x(); + float v = cur_px_estimate.y(); + + // termination condition + const float min_update_squared = 0.03*0.03; + const int cur_step = cur_img.step.p[0]; + Vector3f update; + Vector3f Jres; + for(int iter = 0; iter= cur_img.cols-halfpatch_size || v_r >= cur_img.rows-halfpatch_size) + break; + + if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check + return false; + + float subpix_x = u-u_r; + float subpix_y = v-v_r; + float b1=0, b2=0; + +#ifdef __ARM_NEON__ + const int SHIFT_BITS = 7; + const uint16_t wTL = static_cast((1.0f-subpix_x)*(1.0f-subpix_y)*(1<(subpix_x*(1.0f-subpix_y)*(1<((1.0f-subpix_x)*subpix_y*(1<((1 << SHIFT_BITS) - wTL - wTR - wBL); + + // initialize result to zero + int32x4_t vb1 = vdupq_n_s32(0); + int32x4_t vb2 = vdupq_n_s32(0); + int16x8_t vmean_diff = vdupq_n_s16( (int16_t) (mean_diff+0.5) ); + int16x8_t vres_sum = vdupq_n_s16(0); + for(int y=0; y Vr[i] := Va[i] + Vb[i] * Vc[i] + // int32x4_t vmlal_s16(int32x4_t a, int16x4_t b, int16x4_t c); // VMLAL.S16 q0,d0,d0 + int16x8_t grad = vld1q_s16(ref_patch_dx + y*patch_size); + vb1 = vmlal_s16(vb1, vget_low_s16(grad), vget_low_s16(res)); + vb1 = vmlal_s16(vb1, vget_high_s16(grad), vget_high_s16(res)); + + grad = vld1q_s16(ref_patch_dy + y*patch_size); + vb2 = vmlal_s16(vb2, vget_low_s16(grad), vget_low_s16(res)); + vb2 = vmlal_s16(vb2, vget_high_s16(grad), vget_high_s16(res)); + } + + // finally, sum results of vb1, vb2 and vres_sum + int32x2_t tmp; + tmp = vpadd_s32(vget_low_s32(vb1), vget_high_s32(vb1)); + Jres[0] = -vget_lane_s32(tmp, 0) - vget_lane_s32(tmp, 1); + + tmp = vpadd_s32(vget_low_s32(vb2), vget_high_s32(vb2)); + Jres[1] = -vget_lane_s32(tmp, 0) - vget_lane_s32(tmp, 1); + + int32x4_t vres_sum1 = vpaddlq_s16(vres_sum); + tmp = vpadd_s32(vget_low_s32(vres_sum1), vget_high_s32(vres_sum1)); + Jres[2] = -vget_lane_s32(tmp, 0) - vget_lane_s32(tmp, 1); +#endif + + update = Hinv * Jres * 2; // * 2 to compensate because above, we did not compute the derivative correctly + u += update[0]; + v += update[1]; + mean_diff += update[2]; + +#if SUBPIX_VERBOSE + cout << "Iter " << iter << ":" + << "\t u=" << u << ", v=" << v + << "\t update = " << update[0] << ", " << update[1] << endl; +#endif + + if(update[0]*update[0]+update[1]*update[1] < min_update_squared) + { +#if SUBPIX_VERBOSE + cout << "converged." << endl; +#endif + converged=true; + break; + } + } + + cur_px_estimate << u, v; + return converged; +} + +} // namespace feature_alignment +} // namespace svo diff --git a/svo/src/feature_detection.cpp b/svo/src/feature_detection.cpp new file mode 100644 index 0000000..be0ca02 --- /dev/null +++ b/svo/src/feature_detection.cpp @@ -0,0 +1,77 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include + +namespace svo { +namespace feature_detection { + +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(img_pyr[0].cols)/cell_size); + const int grid_n_rows = ceil(static_cast(img_pyr[0].rows)/cell_size); + vector 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((*it)->px[1]/cell_size)*grid_n_cols + static_cast((*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 fast_corners; +#if __SSE2__ + fast::fast_corner_detect_10_sse2((fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners); +#elif HAVE_FAST_NEON + fast::fast_corner_detect_9_neon((fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners); +#else + fast::fast_corner_detect_10((fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners); +#endif + vector scores, nm_corners; + fast::fast_corner_score_10((fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, fast_corners, 20, scores); + fast::fast_nonmax_3x3(fast_corners, scores, nm_corners); + + for(vector::iterator it=nm_corners.begin(); it!=nm_corners.end(); ++it) + { + fast::fast_xy& xy = fast_corners.at(*it); + const int k = static_cast((xy.y*scale)/cell_size)*grid_n_cols + static_cast((xy.x*scale)/cell_size); + if(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 + diff --git a/svo/src/frame.cpp b/svo/src/frame.cpp new file mode 100644 index 0000000..e5f4501 --- /dev/null +++ b/svo/src/frame.cpp @@ -0,0 +1,197 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +int Frame::frame_counter_ = 0; + +Frame::Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp) : + id_(frame_counter_++), + timestamp_(timestamp), + cam_(cam), + key_pts_(5), + is_keyframe_(false), + v_kf_(NULL) +{ + initFrame(img); +} + +Frame::~Frame() +{ + std::for_each(fts_.begin(), fts_.end(), [&](Feature* i){delete i;}); +} + +void Frame::initFrame(const cv::Mat& img) +{ + // check image + if(img.empty() || img.type() != CV_8UC1 || img.cols != cam_->width() || img.rows != cam_->height()) + throw std::runtime_error("Frame: provided image has not the same size as the camera model"); + + // 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 depth_vec; + depth_vec.reserve(frame.fts_.size()); + depth_min = std::numeric_limits::max(); + for(auto it=frame.fts_.begin(), ite=frame.fts_.end(); it!=ite; ++it) + { + if((*it)->point != NULL) + { + const double z = frame.w2f((*it)->point->pos_).z(); + depth_vec.push_back(z); + depth_min = fmin(z, depth_min); + } + } + if(depth_vec.empty()) + { + SVO_WARN_STREAM("Cannot set scene depth. Frame has no point-observations!"); + return false; + } + depth_mean = vk::getMedian(depth_vec); + return true; +} + +} // namespace frame_utils +} // namespace svo diff --git a/svo/src/frame_handler_base.cpp b/svo/src/frame_handler_base.cpp new file mode 100644 index 0000000..3d530f7 --- /dev/null +++ b/svo/src/frame_handler_base.cpp @@ -0,0 +1,200 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +// definition of global and static variables which were declared in the header +#ifdef SVO_TRACE +vk::PerformanceMonitor* g_permon = NULL; +#endif + +FrameHandlerBase:: +FrameHandlerBase() : + stage_(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: "<writeToFile(); + { + boost::unique_lock 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(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 pts; + for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) + { + if((*it)->point != NULL) + pts.push_back((*it)->point); + } + max_n_pts = min(max_n_pts, pts.size()); + nth_element(pts.begin(), pts.begin() + max_n_pts, pts.end(), ptLastOptimComparator); + for(deque::iterator it=pts.begin(); it!=pts.begin()+max_n_pts; ++it) + { + (*it)->optimize(max_iter); + (*it)->last_structure_optim_ = frame->id_; + } +} + + +} // namespace svo diff --git a/svo/src/frame_handler_mono.cpp b/svo/src/frame_handler_mono.cpp new file mode 100644 index 0000000..95a9787 --- /dev/null +++ b/svo/src/frame_handler_mono.cpp @@ -0,0 +1,254 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef USE_BUNDLE_ADJUSTMENT +#include +#endif + +namespace svo { + +FrameHandlerMono:: +FrameHandlerMono(vk::AbstractCamera* cam) : + 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 = "<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 {"<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::second, _1) > + boost::bind(&pair::second, _2)); + std::for_each(overlap_kfs_.begin(), overlap_kfs_.end(), [&](pair& i){ core_kfs_.insert(i.first); }); +} + +} // namespace svo diff --git a/svo/src/initialization.cpp b/svo/src/initialization.cpp new file mode 100644 index 0000000..1087e61 --- /dev/null +++ b/svo/src/initialization.cpp @@ -0,0 +1,205 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { +namespace initialization { + +InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref) +{ + reset(); + detectFeatures(frame_ref, px_ref_, f_ref_); + if(px_ref_.size() < 100) + { + SVO_WARN_STREAM_THROTTLE(2.0, "First image has less than 100 features. Retry in more textured environment."); + return FAILURE; + } + frame_ref_ = frame_ref; + px_cur_.insert(px_cur_.begin(), px_ref_.begin(), px_ref_.end()); + return SUCCESS; +} + +InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur) +{ + trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, f_ref_, f_cur_, disparities_); + SVO_INFO_STREAM("Init: KLT tracked "<< disparities_.size() <<" features"); + + if(disparities_.size() < Config::initMinTracked()) + return FAILURE; + + double disparity = vk::getMedian(disparities_); + SVO_INFO_STREAM("Init: KLT "<cam_->errorMultiplier2(), Config::poseOptimThresh(), + inliers_, xyz_in_cur_, T_cur_from_ref_); + SVO_INFO_STREAM("Init: Homography RANSAC "< depth_vec; + for(size_t i=0; iT_f_w_ = T_cur_from_ref_ * frame_ref_->T_f_w_; + frame_cur->T_f_w_.translation() = + -frame_cur->T_f_w_.rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos())); + + // For each inlier create 3D point and add feature in both frames + SE3 T_world_cur = frame_cur->T_f_w_.inverse(); + for(vector::iterator it=inliers_.begin(); it!=inliers_.end(); ++it) + { + Vector2d px_cur(px_cur_[*it].x, px_cur_[*it].y); + Vector2d px_ref(px_ref_[*it].x, px_ref_[*it].y); + if(frame_ref_->cam_->isInFrame(px_cur.cast(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast(), 10) && xyz_in_cur_[*it].z() > 0) + { + Vector3d pos = T_world_cur * (xyz_in_cur_[*it]*scale); + Point* new_point = new Point(pos, 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& px_vec, + vector& 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& px_ref, + vector& px_cur, + vector& f_ref, + vector& f_cur, + vector& disparities) +{ + const double klt_win_size = 30.0; + const int klt_max_iter = 30; + const double klt_eps = 0.001; + vector status; + vector error; + vector min_eig_vec; + cv::TermCriteria termcrit(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, klt_max_iter, klt_eps); + cv::calcOpticalFlowPyrLK(frame_ref->img_pyr_[0], frame_cur->img_pyr_[0], + px_ref, px_cur, + status, error, + cv::Size2i(klt_win_size, klt_win_size), + 4, termcrit); + + vector::iterator px_ref_it = px_ref.begin(); + vector::iterator px_cur_it = px_cur.begin(); + vector::iterator f_ref_it = f_ref.begin(); + f_cur.clear(); f_cur.reserve(px_cur.size()); + disparities.clear(); disparities.reserve(px_cur.size()); + for(size_t i=0; px_ref_it != px_ref.end(); ++i) + { + if(!status[i]) + { + px_ref_it = px_ref.erase(px_ref_it); + px_cur_it = px_cur.erase(px_cur_it); + f_ref_it = f_ref.erase(f_ref_it); + continue; + } + f_cur.push_back(frame_cur->c2f(px_cur_it->x, px_cur_it->y)); + disparities.push_back(Vector2d(px_ref_it->x - px_cur_it->x, px_ref_it->y - px_cur_it->y).norm()); + ++px_ref_it; + ++px_cur_it; + ++f_ref_it; + } +} + +void computeHomography( + const vector& f_ref, + const vector& f_cur, + double focal_length, + double reprojection_threshold, + vector& inliers, + vector& xyz_in_cur, + SE3& T_cur_from_ref) +{ + vector > uv_ref(f_ref.size()); + vector > uv_cur(f_cur.size()); + for(size_t i=0, i_max=f_ref.size(); i outliers; + vk::computeInliers(f_cur, f_ref, + Homography.T_c2_from_c1.rotation_matrix(), Homography.T_c2_from_c1.translation(), + reprojection_threshold, focal_length, + xyz_in_cur, inliers, outliers); + T_cur_from_ref = Homography.T_c2_from_c1; +} + + +} // namespace initialization +} // namespace svo diff --git a/svo/src/map.cpp b/svo/src/map.cpp new file mode 100644 index 0000000..808917c --- /dev/null +++ b/svo/src/map.cpp @@ -0,0 +1,357 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include + +namespace svo { + +Map::Map() {} + +Map::~Map() +{ + reset(); + SVO_INFO_STREAM("Map destructed"); +} + +void Map::reset() +{ + keyframes_.clear(); + point_candidates_.reset(); + emptyTrash(); +} + +bool Map::safeDeleteFrame(FramePtr frame) +{ + bool found = false; + for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it) + { + if(*it == frame) + { + std::for_each((*it)->fts_.begin(), (*it)->fts_.end(), [&](Feature* ftr){ + removePtFrameRef(it->get(), ftr); + }); + keyframes_.erase(it); + found = true; + break; + } + } + + point_candidates_.removeFrameCandidates(frame); + + if(found) + return true; + + SVO_ERROR_STREAM("Tried to delete Keyframe in map which was not there."); + return false; +} + +void Map::removePtFrameRef(Frame* frame, Feature* ftr) +{ + if(ftr->point == NULL) + return; // mappoint may have been deleted in a previous ref. removal + Point* pt = ftr->point; + ftr->point = NULL; + if(pt->obs_.size() <= 2) + { + // If the references list of mappoint has only size=2, delete mappoint + safeDeletePoint(pt); + return; + } + pt->deleteFrameRef(frame); // Remove reference from map_point + frame->removeKeyPoint(ftr); // Check if mp was keyMp in keyframe +} + +void Map::safeDeletePoint(Point* pt) +{ + // Delete references to mappoints in all keyframes + std::for_each(pt->obs_.begin(), pt->obs_.end(), [&](Feature* ftr){ + ftr->point=NULL; + ftr->frame->removeKeyPoint(ftr); + }); + pt->obs_.clear(); + + // Delete mappoint + deletePoint(pt); +} + +void Map::deletePoint(Point* pt) +{ + pt->type_ = Point::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 >& 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(*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 > close_kfs; + getCloseKeyframes(frame, close_kfs); + + // Sort KFs with overlap according to their closeness + close_kfs.sort(boost::bind(&std::pair::second, _1) < + boost::bind(&std::pair::second, _2)); + + 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 lock(mut_); + candidates_.push_back(PointCandidate(point, seed.ftr)); +} + +void MapPointCandidates::addCandidatePointToFrame(FramePtr frame) +{ + boost::unique_lock lock(mut_); + PointCandidateList::iterator it=candidates_.begin(); + while(it != candidates_.end()) + { + if(it->first->obs_.front()->frame == frame.get()) + { + // insert feature in the frame + it->first->type_ = Point::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 lock(mut_); + for(auto it=candidates_.begin(), ite=candidates_.end(); it!=ite; ++it) + { + if(it->first == point) + { + deleteCandidate(*it); + candidates_.erase(it); + return true; + } + } + return false; +} + +void MapPointCandidates::removeFrameCandidates(FramePtr frame) +{ + boost::unique_lock lock(mut_); + auto it=candidates_.begin(); + while(it!=candidates_.end()) + { + if(it->second->frame == frame.get()) + { + deleteCandidate(*it); + it = candidates_.erase(it); + } + else + ++it; + } +} + +void MapPointCandidates::reset() +{ + boost::unique_lock lock(mut_); + std::for_each(candidates_.begin(), candidates_.end(), [&](PointCandidate& c){ + delete c.first; + delete c.second; + }); + candidates_.clear(); +} + +void MapPointCandidates::deleteCandidate(PointCandidate& c) +{ + // camera-rig: another frame might still be pointing to the candidate point + // therefore, we can't delete it right now. + delete c.second; c.second=NULL; + c.first->type_ = Point::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 points; + for(auto it=map->keyframes_.begin(); it!=map->keyframes_.end(); ++it) + { + for(auto ftr=(*it)->fts_.begin(); ftr!=(*it)->fts_.end(); ++ftr) + { + if((*ftr)->point == NULL) + continue; + if(points.insert((*ftr)->point).second) { + ++n_pts; + n_frame_obs += (*ftr)->point->nRefs(); + } + } + } + printf("Map Statistics: Point avg. frame obs = %f\n\n", (float) n_frame_obs/n_pts); +} + +} // namespace map_debug +} // namespace svo diff --git a/svo/src/matcher.cpp b/svo/src/matcher.cpp new file mode 100644 index 0000000..9587608 --- /dev/null +++ b/svo/src/matcher.cpp @@ -0,0 +1,343 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { +namespace matcher { + +PatchData::PatchData(int patch_size) : + patch_size(patch_size) +{ + patch = vk::aligned_mem::aligned_alloc(patch_size*patch_size, 16); + patch_with_border = vk::aligned_mem::aligned_alloc((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< 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(); + 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() / (1<=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; yframe->cam_->isInFrame( + ref_ftr->px.cast()/(1<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< 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 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<().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<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; iworld2cam(uv)); + Vector2i pxi(px[0]/(1<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<world2cam(uv_best)); + Vector2d px_scaled(px/(1<().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<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 diff --git a/svo/src/point.cpp b/svo/src/point.cpp new file mode 100644 index 0000000..40aced9 --- /dev/null +++ b/svo/src/point.cpp @@ -0,0 +1,170 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include + +namespace svo { + +int Point::point_counter_ = 0; + +Point::Point(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; iframe->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 diff --git a/svo/src/pose_optimizer.cpp b/svo/src/pose_optimizer.cpp new file mode 100644 index 0000000..180bb1c --- /dev/null +++ b/svo/src/pose_optimizer.cpp @@ -0,0 +1,164 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include + +namespace svo { +namespace pose_optimizer { + +void optimizeGaussNewton( + const double reproj_thresh, + const size_t n_iter, + const bool verbose, + FramePtr& frame, + double& estimated_scale, + double& error_init, + double& error_final, + size_t& num_obs) +{ + // init + double chi2(0.0); + Vector2d focal_length(1,1); + vector chi2_vec_init, chi2_vec_final; + vk::robust_cost::TukeyWeightFunction weight_function; + SE3 T_old(frame->T_f_w_); + Matrix6d A; + Vector6d b; + + // compute the scale of the error for robust estimation + std::vector errors; errors.reserve(frame->fts_.size()); + for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) + { + if((*it)->point == NULL) + continue; + Vector2d e = vk::project2d((*it)->f) + - vk::project2d(frame->T_f_w_ * (*it)->point->pos_); + e *= 1.0 / (1<<(*it)->level); + errors.push_back(e.norm()); + } + if(errors.empty()) + return; + vk::robust_cost::MADScaleEstimator scale_estimator; + estimated_scale = scale_estimator.compute(errors); + + num_obs = errors.size(); + chi2_vec_init.reserve(num_obs); + chi2_vec_final.reserve(num_obs); + double scale = estimated_scale; + for(size_t iter=0; itercam_->errorMultiplier2(); + + b.setZero(); + A.setZero(); + double new_chi2(0.0); + + // compute residual + for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) + { + if((*it)->point == NULL) + continue; + Matrix26d J = 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 diff --git a/svo/src/reprojection.cpp b/svo/src/reprojection.cpp new file mode 100644 index 0000000..26de82c --- /dev/null +++ b/svo/src/reprojection.cpp @@ -0,0 +1,214 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { +namespace reprojection { + +Grid::Grid() +{} + +void Grid::initialize(vk::AbstractCamera* cam) +{ + cell_size = Config::gridSize(); + grid_n_cols = ceil(static_cast(cam->width())/cell_size); + grid_n_rows = ceil(static_cast(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; iclear(); }); +} + +void reprojectMap( + Map& map, + FramePtr frame, + Grid& grid, + MapPointCandidates& candidate_points, + std::vector< std::pair >& overlap_kfs, + size_t& n_matches, + size_t& n_trials) +{ + resetGrid(grid); + + // Identify those Keyframes which share a common field of view. + list< pair > 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::second, _1) < + boost::bind(&std::pair::second, _2)); + + // Reproject all mappoints of the closest N kfs with overlap. We only store + // in which grid cell the points fall. + size_t n = 0, max_n = 10; + overlap_kfs.reserve(max_n); + for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end(); + it_frame!=ite_frame && nfirst; + overlap_kfs.push_back(pair(ref_frame,0)); + + // Try to reproject each mappoint that the other KF observes + for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end(); + it_ftr!=ite_ftr; ++it_ftr) + { + // check if the feature has a mappoint assigned + if((*it_ftr)->point == NULL) + continue; + + // make sure we project a point only once + if((*it_ftr)->point->last_projected_kf_id_ == frame->id_) + continue; + (*it_ftr)->point->last_projected_kf_id_ = frame->id_; + if(reprojectPoint(grid, frame, (*it_ftr)->point)) + overlap_kfs.back().second++; + } + } + + // Now project all point candidates + { + boost::unique_lock 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 (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(), 8)) // 8px is the patch size in the matcher + { + const int k = static_cast(px[1]/grid.cell_size)*grid.grid_n_cols + static_cast(px[0]/grid.cell_size); + grid.cells.at(k)->push_back(Candidate(point, px)); + return true; + } + return false; +} + +} // namespace reprojection +} // namespace svo diff --git a/svo/src/sparse_img_align.cpp b/svo/src/sparse_img_align.cpp new file mode 100644 index 0000000..c4a2afd --- /dev/null +++ b/svo/src/sparse_img_align.cpp @@ -0,0 +1,296 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +SparseImgAlign:: +SparseImgAlign(int max_level, int min_level, int n_iter, + Method method, bool display, bool verbose) : + display_(display), + max_level_(max_level), + min_level_(min_level) +{ + n_iter_ = n_iter; + n_iter_init_ = n_iter_; + method_ = method; + verbose_ = verbose; + eps_ = 0.000001; +} + +size_t SparseImgAlign:: +run(FramePtr ref_frame, FramePtr cur_frame) +{ + reset(); + + if(ref_frame->fts_.empty()) + { + SVO_WARN_STREAM("SparseImgAlign: no features to track!"); + return 0; + } + + ref_frame_ = ref_frame; + cur_frame_ = cur_frame; + ref_patch_cache_ = cv::Mat(ref_frame_->fts_.size(), patch_area_, CV_32F); + jacobian_cache_.resize(Eigen::NoChange, ref_patch_cache_.rows*patch_area_); + visible_fts_.resize(ref_patch_cache_.rows, false); + + 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 & 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<pos(); + const double focal_length = ref_frame_->cam_->errorMultiplier2(); + size_t feature_counter = 0; + std::vector::iterator visiblity_it = visible_fts_.begin(); + for(auto it=ref_frame_->fts_.begin(), ite=ref_frame_->fts_.end(); + it!=ite; ++it, ++feature_counter, ++visiblity_it) + { + // check if reference with patch size is within image + const float u_ref = (*it)->px[0]*scale; + const float v_ref = (*it)->px[1]*scale; + const int u_ref_i = floorf(u_ref); + const int v_ref_i = floorf(v_ref); + if((*it)->point == NULL || u_ref_i-border < 0 || v_ref_i-border < 0 || u_ref_i+border >= ref_img.cols || v_ref_i+border >= ref_img.rows) + continue; + *visiblity_it = true; + + // cannot just take the 3d points coordinate because of the reprojection errors in the reference image!!! + const double depth(((*it)->point->pos_ - ref_pos).norm()); + const Vector3d xyz_ref((*it)->f*depth); + + // compute bilateral interpolation weights for reference image + const float subpix_u_ref = u_ref-u_ref_i; + const float subpix_v_ref = v_ref-v_ref_i; + const float w_ref_tl = (1.0-subpix_u_ref) * (1.0-subpix_v_ref); + const float w_ref_tr = subpix_u_ref * (1.0-subpix_v_ref); + const float w_ref_bl = (1.0-subpix_u_ref) * subpix_v_ref; + const float w_ref_br = subpix_u_ref * subpix_v_ref; + size_t pixel_counter = 0; + float* cache_ptr = reinterpret_cast(ref_patch_cache_.data) + patch_area_*feature_counter; + for(int y=0; y 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<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 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<pos()); + float chi2 = 0.0; + size_t feature_counter = 0; // is used to compute the index of the cached jacobian + std::vector::iterator visiblity_it = visible_fts_.begin(); + for(auto it=ref_frame_->fts_.begin(); it!=ref_frame_->fts_.end(); + ++it, ++feature_counter, ++visiblity_it) + { + // check if feature is within image + if(!*visiblity_it) + continue; + + // compute pixel location in cur img + const double depth = ((*it)->point->pos_ - ref_pos).norm(); + const Vector3d xyz_ref((*it)->f*depth); + const Vector3d xyz_cur(T_cur_from_ref * xyz_ref); + const Vector2f uv_cur_pyr(cur_frame_->cam_->world2cam(xyz_cur).cast() * scale); + const float u_cur = uv_cur_pyr[0]; + const float v_cur = uv_cur_pyr[1]; + const int u_cur_i = floorf(u_cur); + const int v_cur_i = floorf(v_cur); + + // check if projection is within the image + if(u_cur_i < 0 || v_cur_i < 0 || u_cur_i-border < 0 || v_cur_i-border < 0 || u_cur_i+border >= cur_img.cols || v_cur_i+border >= cur_img.rows) + continue; + + // compute bilateral interpolation weights for the current image + const float subpix_u_cur = u_cur-u_cur_i; + const float subpix_v_cur = v_cur-v_cur_i; + const float w_cur_tl = (1.0-subpix_u_cur) * (1.0-subpix_v_cur); + const float w_cur_tr = subpix_u_cur * (1.0-subpix_v_cur); + const float w_cur_bl = (1.0-subpix_u_cur) * subpix_v_cur; + const float w_cur_br = subpix_u_cur * subpix_v_cur; + float* ref_patch_cache_ptr = reinterpret_cast(ref_patch_cache_.data) + patch_area_*feature_counter; + size_t pixel_counter = 0; // is used to compute the index of the cached jacobian + for(int y=0; yvalue(res/scale_); + } + + chi2 += res*res*weight; + n_meas_++; + + if(linearize_system) + { + // compute Jacobian, weighted Hessian and weighted "steepest descend images" (times error) + const Vector6d J(jacobian_cache_.col(feature_counter*patch_area_ + pixel_counter)); + H_ += J*J.transpose()*weight; + Jres_ -= J*res*weight; + if(display_) + resimg_.at((int) v_cur+y-patch_halfsize_, (int) u_cur+x-patch_halfsize_) = res/255.0; + } + } + } + } + + // compute the weights on the first iteration + if(compute_weight_scale && iter_ == 0) + scale_ = scale_estimator_->compute(errors); + + return chi2/n_meas_; +} + +int SparseImgAlign:: +solve() +{ + x_ = H_.ldlt().solve(Jres_); + if((bool) std::isnan((double) x_[0])) + return 0; + return 1; +} + +void SparseImgAlign:: +update(const ModelType& T_curold_from_ref, ModelType& T_curnew_from_ref) +{ + // 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 + diff --git a/svo/test/README.md b/svo/test/README.md new file mode 100644 index 0000000..ce9f1fd --- /dev/null +++ b/svo/test/README.md @@ -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 diff --git a/svo/test/benchmark.csv b/svo/test/benchmark.csv new file mode 100644 index 0000000..e6f88a3 --- /dev/null +++ b/svo/test/benchmark.csv @@ -0,0 +1,307 @@ +local_ba,point_optimizer,pose_optimizer,pyramid_creation,reproject,sparse_img_align,tot_time,dropout,img_align_n_tracked,loba_err_fin,loba_err_init,loba_n_erredges_fin,loba_n_erredges_init,n_candidates,repr_n_mps,repr_n_new_references,sfba_error_final,sfba_error_init,sfba_n_edges_final,sfba_thresh +0.000000000000000,0.000000000000000,0.000000000000000,0.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 \ No newline at end of file diff --git a/svo/test/data/.gitignore b/svo/test/data/.gitignore new file mode 100644 index 0000000..c96a04f --- /dev/null +++ b/svo/test/data/.gitignore @@ -0,0 +1,2 @@ +* +!.gitignore \ No newline at end of file diff --git a/svo/test/results/.gitignore b/svo/test/results/.gitignore new file mode 100644 index 0000000..c96a04f --- /dev/null +++ b/svo/test/results/.gitignore @@ -0,0 +1,2 @@ +* +!.gitignore \ No newline at end of file diff --git a/svo/test/test_depth_filter.cpp b/svo/test/test_depth_filter.cpp new file mode 100644 index 0000000..09d0034 --- /dev/null +++ b/svo/test/test_depth_filter.cpp @@ -0,0 +1,200 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 results_; + std::vector 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(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 sequence_file_reader(dataset_dir+"/trajectory.txt"); + std::vector sequence; + sequence_file_reader.skipComments(); + if(!sequence_file_reader.next()) + std::runtime_error("Failed to open sequence file"); + sequence_file_reader.readAllEntries(sequence); + std::cout << "RUN EXPERIMENT: read " << sequence.size() << " dataset entries." << std::endl; + std::vector::iterator it = sequence.begin(); + vk::Timer t; + std::list n_converged_per_iteration; + + svo::feature_detection::DetectorPtr feature_detector(new svo::feature_detection::FastDetector()); + 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::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::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::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::iterator i=results_.begin(); i!=results_.end(); ++i) + { + cv::Vec3b c = frame_ref_->img_pyr_[0].at(i->y_, i->x_); + Eigen::Vector3d p = cam_->cam2world(i->x_, i->y_)*i->depth_; + ofs << p[0] << " " << p[1] << " " << p[2] << " " + << (int) c[0] << " " << (int) c[1] << " " << (int) c[2] << std::endl; + } +} + +} // namespace + +int main(int argc, char** argv) +{ + DepthFilterTest test; + std::string dataset_dir(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; +} diff --git a/svo/test/test_feature_alignment.cpp b/svo/test/test_feature_alignment.cpp new file mode 100644 index 0000000..04bddf7 --- /dev/null +++ b/svo/test/test_feature_alignment.cpp @@ -0,0 +1,122 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#if __ARM_NEON__ +#include +#endif + +using namespace Eigen; + +void generateRefPatchNoWarpInterpolate(const cv::Mat& img, const Vector2d& px, + cv::Mat& ref_patch_with_border) +{ + // compute interpolation weights + const int u_r = floor(px[0]); + const int v_r = floor(px[1]); + const float subpix_u = px[0]-u_r; + const float subpix_v = px[1]-v_r; + const float wTL = (1.0-subpix_u)*(1.0-subpix_v); + const float wTR = subpix_u * (1.0-subpix_v); + const float wBL = (1.0-subpix_u)*subpix_v; + const float wBR = subpix_u * subpix_v; + + // loop through search_patch, interpolate + ref_patch_with_border = cv::Mat(10, 10, CV_8UC1); + uint8_t* patch_ptr = ref_patch_with_border.data; + const int stride = img.step.p[0]; + for(int y=0; y<10; ++y) + { + uint8_t* img_ptr = (uint8_t*) img.data + (v_r+y-4-1)*stride + u_r-4-1; + for(int x=0; x<10; ++x, ++patch_ptr, ++img_ptr) + *patch_ptr = wTL*img_ptr[0] + wTR*img_ptr[1] + wBL*img_ptr[stride] + wBR*img_ptr[stride+1]; + } +} + +int main(int argc, char **argv) +{ + std::string img_name(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(64, 16); + uint8_t* ref_patch_ptr = ref_patch; + for(int y=1; y<9; ++y) + { + uint8_t* ref_patch_border_ptr = ref_patch_with_border.data + y*10 + 1; + for(int x=0; x<8; ++x, ++ref_patch_border_ptr, ++ref_patch_ptr) + *ref_patch_ptr = *ref_patch_border_ptr; + } + + Vector2d px_est, px_error(-1.1, -0.8); + double h_inv; + vk::Timer t; + for(int i=0; i<1000; ++i) + { + px_est = px_true-px_error; + Vector2f dir = px_error.normalized().cast(); + svo::feature_alignment::align1D(img, dir, ref_patch_with_border.data, ref_patch, 3, px_est, h_inv); + } + Vector2d e = px_est-px_true; + printf("1000Xalign 1D took %fms, error = %fpx \t (ref i7-W520: 1.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; +} diff --git a/svo/test/test_feature_detection.cpp b/svo/test/test_feature_detection.cpp new file mode 100644 index 0000000..00e21a2 --- /dev/null +++ b/svo/test/test_feature_detection.cpp @@ -0,0 +1,80 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { + +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; +} diff --git a/svo/test/test_matcher.cpp b/svo/test/test_matcher.cpp new file mode 100644 index 0000000..3138d48 --- /dev/null +++ b/svo/test/test_matcher.cpp @@ -0,0 +1,195 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { + +/// 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 errors; errors.reserve(depthmap.cols*depthmap.rows); + for(int y=4; y(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(y,x) = depth_estimate; + error_map.at(y,x) = fabsf(depth_estimate-depth_gt); + ++n_converged; + sum_error += error_map.at(y,x); + errors.push_back(error_map.at(y,x)); + } + else + mask.at(y,x) = 0; + } + } + + // compute mean, median and variance of error in converged area + printf("n converged: \t %zu (ref: 194472)\n", n_converged); + printf("mean error: \t %f (ref: 1.0848)\n", sum_error*100/n_converged); + std::vector::iterator it = errors.begin()+0.5*errors.size(); + std::nth_element(errors.begin(), it, errors.end()); + printf("50-percentile: \t %f (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<img_pyr_.at(level_cur)(cv::Range(pxi[1]-halfpatch_size, + pxi[1]+halfpatch_size), + cv::Range(pxi[0]-halfpatch_size, + pxi[0]+halfpatch_size)).copyTo(cur_patch); + + // display results + cv::circle(frame_cur_->img_pyr_[level_cur], + cv::Point2f(pxi[0], pxi[1]), + patch_size, cv::Scalar(255), 2); + cv::circle(frame_ref_->img_pyr_[ref_ftr_->level], + cv::Point2f(ref_ftr_->px[0]/(1<level), ref_ftr_->px[1]/(1<level)), + patch_size, cv::Scalar(255), 2); + cv::imshow("cur_patch", cur_patch); + cv::imshow("ref_patch", ref_patch); + cv::imshow("ref_img", frame_ref_->img_pyr_[ref_ftr_->level]); + cv::imshow("cur_img", frame_cur_->img_pyr_[level_cur]); + cv::waitKey(0); +} + +} // namespace + +int main(int argc, char** argv) +{ + MatcherTest test; + test.testEpipolarSearchFullImg(); +// test.testWarpAffine(); + return 0; +} diff --git a/svo/test/test_pipeline.cpp b/svo/test/test_pipeline.cpp new file mode 100644 index 0000000..d328bf1 --- /dev/null +++ b/svo/test/test_pipeline.cpp @@ -0,0 +1,94 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +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; +} + diff --git a/svo/test/test_pose_optimizer.cpp b/svo/test/test_pose_optimizer.cpp new file mode 100644 index 0000000..0c9a63f --- /dev/null +++ b/svo/test/test_pose_optimizer.cpp @@ -0,0 +1,119 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { + +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(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; +} diff --git a/svo/test/test_sparse_img_align.cpp b/svo/test/test_sparse_img_align.cpp new file mode 100644 index 0000000..1c4437c --- /dev/null +++ b/svo/test/test_sparse_img_align.cpp @@ -0,0 +1,165 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 sequence_file_reader(dataset_dir+"/trajectory.txt"); + std::vector sequence; + sequence_file_reader.skipComments(); + if(!sequence_file_reader.next()) + std::runtime_error("Failed to open sequence file"); + sequence_file_reader.readAllEntries(sequence); + printf("RUN EXPERIMENT: read %zu dataset entries.\n", sequence.size()); + std::vector::iterator iter = sequence.begin(); + std::list translation_error; + + Sophus::SE3 T_prev_w, T_prevgt_w; + std::string trace_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(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::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; +} diff --git a/svo_analysis/benchmark.py b/svo_analysis/benchmark.py new file mode 100755 index 0000000..da32e70 --- /dev/null +++ b/svo_analysis/benchmark.py @@ -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' + + diff --git a/svo_analysis/evaluate.py b/svo_analysis/evaluate.py new file mode 100755 index 0000000..05af1ee --- /dev/null +++ b/svo_analysis/evaluate.py @@ -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) \ No newline at end of file diff --git a/svo_analysis/experiments/asl2_acc.yaml b/svo_analysis/experiments/asl2_acc.yaml new file mode 100644 index 0000000..0b5bf3b --- /dev/null +++ b/svo_analysis/experiments/asl2_acc.yaml @@ -0,0 +1,2 @@ +dataset: asl_vicon_d2 +param_settings: vo_accurate \ No newline at end of file diff --git a/svo_analysis/experiments/asl2_fast.yaml b/svo_analysis/experiments/asl2_fast.yaml new file mode 100644 index 0000000..34285b5 --- /dev/null +++ b/svo_analysis/experiments/asl2_fast.yaml @@ -0,0 +1,2 @@ +dataset: asl_vicon_d2 +param_settings: vo_fast \ No newline at end of file diff --git a/svo_analysis/results/.gitignore b/svo_analysis/results/.gitignore new file mode 100644 index 0000000..d6b7ef3 --- /dev/null +++ b/svo_analysis/results/.gitignore @@ -0,0 +1,2 @@ +* +!.gitignore diff --git a/svo_analysis/scripts/__init__.py b/svo_analysis/scripts/__init__.py new file mode 100644 index 0000000..633f866 --- /dev/null +++ b/svo_analysis/scripts/__init__.py @@ -0,0 +1,2 @@ +# -*- coding: utf-8 -*- + diff --git a/svo_analysis/scripts/analyse_dataset.py b/svo_analysis/scripts/analyse_dataset.py new file mode 100644 index 0000000..fa257b8 --- /dev/null +++ b/svo_analysis/scripts/analyse_dataset.py @@ -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' + + diff --git a/svo_analysis/scripts/analyse_logs.py b/svo_analysis/scripts/analyse_logs.py new file mode 100755 index 0000000..be14feb --- /dev/null +++ b/svo_analysis/scripts/analyse_logs.py @@ -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) + diff --git a/svo_analysis/scripts/analyse_timing.py b/svo_analysis/scripts/analyse_timing.py new file mode 100755 index 0000000..b0d2199 --- /dev/null +++ b/svo_analysis/scripts/analyse_timing.py @@ -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) + + \ No newline at end of file diff --git a/svo_analysis/scripts/analyse_trajectory.py b/svo_analysis/scripts/analyse_trajectory.py new file mode 100644 index 0000000..f70cd62 --- /dev/null +++ b/svo_analysis/scripts/analyse_trajectory.py @@ -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() + diff --git a/svo_analysis/scripts/associate.py b/svo_analysis/scripts/associate.py new file mode 100644 index 0000000..8a7438c --- /dev/null +++ b/svo_analysis/scripts/associate.py @@ -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]))) + + diff --git a/svo_analysis/scripts/evaluate_rpe.py b/svo_analysis/scripts/evaluate_rpe.py new file mode 100755 index 0000000..1ba24a7 --- /dev/null +++ b/svo_analysis/scripts/evaluate_rpe.py @@ -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)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() \ No newline at end of file diff --git a/svo_analysis/scripts/filter_groundtruth_smooth.py b/svo_analysis/scripts/filter_groundtruth_smooth.py new file mode 100644 index 0000000..d0cc240 --- /dev/null +++ b/svo_analysis/scripts/filter_groundtruth_smooth.py @@ -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() diff --git a/svo_analysis/scripts/hand_eye_calib.py b/svo_analysis/scripts/hand_eye_calib.py new file mode 100644 index 0000000..7a7bb1b --- /dev/null +++ b/svo_analysis/scripts/hand_eye_calib.py @@ -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)) + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/svo_analysis/scripts/rotation_utils.py b/svo_analysis/scripts/rotation_utils.py new file mode 100644 index 0000000..4c7b711 --- /dev/null +++ b/svo_analysis/scripts/rotation_utils.py @@ -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) + diff --git a/svo_analysis/scripts/trajectory_comparison.py b/svo_analysis/scripts/trajectory_comparison.py new file mode 100644 index 0000000..1bc219e --- /dev/null +++ b/svo_analysis/scripts/trajectory_comparison.py @@ -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") +# + + + \ No newline at end of file diff --git a/svo_analysis/scripts/transformations.py b/svo_analysis/scripts/transformations.py new file mode 100644 index 0000000..6d6f19e --- /dev/null +++ b/svo_analysis/scripts/transformations.py @@ -0,0 +1,1705 @@ +# -*- coding: utf-8 -*- +# transformations.py + +# Copyright (c) 2006, Christoph Gohlke +# Copyright (c) 2006-2009, The Regents of the University of California +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the copyright holders nor the names of any +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Homogeneous Transformation Matrices and Quaternions. + +A library for calculating 4x4 matrices for translating, rotating, reflecting, +scaling, shearing, projecting, orthogonalizing, and superimposing arrays of +3D homogeneous coordinates as well as for converting between rotation matrices, +Euler angles, and quaternions. Also includes an Arcball control object and +functions to decompose transformation matrices. + +:Authors: + `Christoph Gohlke `__, + Laboratory for Fluorescence Dynamics, University of California, Irvine + +:Version: 20090418 + +Requirements +------------ + +* `Python 2.6 `__ +* `Numpy 1.3 `__ +* `transformations.c 20090418 `__ + (optional implementation of some functions in C) + +Notes +----- + +Matrices (M) can be inverted using numpy.linalg.inv(M), concatenated using +numpy.dot(M0, M1), or used to transform homogeneous coordinates (v) using +numpy.dot(M, v) for shape (4, \*) "point of arrays", respectively +numpy.dot(v, M.T) for shape (\*, 4) "array of points". + +Calculations are carried out with numpy.float64 precision. + +This Python implementation is not optimized for speed. + +Vector, point, quaternion, and matrix function arguments are expected to be +"array like", i.e. tuple, list, or numpy arrays. + +Return types are numpy arrays unless specified otherwise. + +Angles are in radians unless specified otherwise. + +Quaternions ix+jy+kz+w are represented as [x, y, z, w]. + +Use the transpose of transformation matrices for OpenGL glMultMatrixd(). + +A triple of Euler angles can be applied/interpreted in 24 ways, which can +be specified using a 4 character string or encoded 4-tuple: + + *Axes 4-string*: e.g. 'sxyz' or 'ryxy' + + - first character : rotations are applied to 's'tatic or 'r'otating frame + - remaining characters : successive rotation axis 'x', 'y', or 'z' + + *Axes 4-tuple*: e.g. (0, 0, 0, 0) or (1, 1, 1, 1) + + - inner axis: code of axis ('x':0, 'y':1, 'z':2) of rightmost matrix. + - parity : even (0) if inner axis 'x' is followed by 'y', 'y' is followed + by 'z', or 'z' is followed by 'x'. Otherwise odd (1). + - repetition : first and last axis are same (1) or different (0). + - frame : rotations are applied to static (0) or rotating (1) frame. + +References +---------- + +(1) Matrices and transformations. Ronald Goldman. + In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990. +(2) More matrices and transformations: shear and pseudo-perspective. + Ronald Goldman. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. +(3) Decomposing a matrix into simple transformations. Spencer Thomas. + In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. +(4) Recovering the data from the transformation matrix. Ronald Goldman. + In "Graphics Gems II", pp 324-331. Morgan Kaufmann, 1991. +(5) Euler angle conversion. Ken Shoemake. + In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994. +(6) Arcball rotation control. Ken Shoemake. + In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994. +(7) Representing attitude: Euler angles, unit quaternions, and rotation + vectors. James Diebel. 2006. +(8) A discussion of the solution for the best rotation to relate two sets + of vectors. W Kabsch. Acta Cryst. 1978. A34, 827-828. +(9) Closed-form solution of absolute orientation using unit quaternions. + BKP Horn. J Opt Soc Am A. 1987. 4(4), 629-642. +(10) Quaternions. Ken Shoemake. + http://www.sfu.ca/~jwa3/cmpt461/files/quatut.pdf +(11) From quaternion to matrix and back. JMP van Waveren. 2005. + http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm +(12) Uniform random rotations. Ken Shoemake. + In "Graphics Gems III", pp 124-132. Morgan Kaufmann, 1992. + + +Examples +-------- + +>>> alpha, beta, gamma = 0.123, -1.234, 2.345 +>>> origin, xaxis, yaxis, zaxis = (0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1) +>>> I = identity_matrix() +>>> Rx = rotation_matrix(alpha, xaxis) +>>> Ry = rotation_matrix(beta, yaxis) +>>> Rz = rotation_matrix(gamma, zaxis) +>>> R = concatenate_matrices(Rx, Ry, Rz) +>>> euler = euler_from_matrix(R, 'rxyz') +>>> numpy.allclose([alpha, beta, gamma], euler) +True +>>> Re = euler_matrix(alpha, beta, gamma, 'rxyz') +>>> is_same_transform(R, Re) +True +>>> al, be, ga = euler_from_matrix(Re, 'rxyz') +>>> is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz')) +True +>>> qx = quaternion_about_axis(alpha, xaxis) +>>> qy = quaternion_about_axis(beta, yaxis) +>>> qz = quaternion_about_axis(gamma, zaxis) +>>> q = quaternion_multiply(qx, qy) +>>> q = quaternion_multiply(q, qz) +>>> Rq = quaternion_matrix(q) +>>> is_same_transform(R, Rq) +True +>>> S = scale_matrix(1.23, origin) +>>> T = translation_matrix((1, 2, 3)) +>>> Z = shear_matrix(beta, xaxis, origin, zaxis) +>>> R = random_rotation_matrix(numpy.random.rand(3)) +>>> M = concatenate_matrices(T, R, Z, S) +>>> scale, shear, angles, trans, persp = decompose_matrix(M) +>>> numpy.allclose(scale, 1.23) +True +>>> numpy.allclose(trans, (1, 2, 3)) +True +>>> numpy.allclose(shear, (0, math.tan(beta), 0)) +True +>>> is_same_transform(R, euler_matrix(axes='sxyz', *angles)) +True +>>> M1 = compose_matrix(scale, shear, angles, trans, persp) +>>> is_same_transform(M, M1) +True + +""" + +from __future__ import division + +import warnings +import math + +import numpy + +# Documentation in HTML format can be generated with Epydoc +__docformat__ = "restructuredtext en" + + +def identity_matrix(): + """Return 4x4 identity/unit matrix. + + >>> I = identity_matrix() + >>> numpy.allclose(I, numpy.dot(I, I)) + True + >>> numpy.sum(I), numpy.trace(I) + (4.0, 4.0) + >>> numpy.allclose(I, numpy.identity(4, dtype=numpy.float64)) + True + + """ + return numpy.identity(4, dtype=numpy.float64) + + +def translation_matrix(direction): + """Return matrix to translate by direction vector. + + >>> v = numpy.random.random(3) - 0.5 + >>> numpy.allclose(v, translation_matrix(v)[:3, 3]) + True + + """ + M = numpy.identity(4) + M[:3, 3] = direction[:3] + return M + + +def translation_from_matrix(matrix): + """Return translation vector from translation matrix. + + >>> v0 = numpy.random.random(3) - 0.5 + >>> v1 = translation_from_matrix(translation_matrix(v0)) + >>> numpy.allclose(v0, v1) + True + + """ + return numpy.array(matrix, copy=False)[:3, 3].copy() + + +def reflection_matrix(point, normal): + """Return matrix to mirror at plane defined by point and normal vector. + + >>> v0 = numpy.random.random(4) - 0.5 + >>> v0[3] = 1.0 + >>> v1 = numpy.random.random(3) - 0.5 + >>> R = reflection_matrix(v0, v1) + >>> numpy.allclose(2., numpy.trace(R)) + True + >>> numpy.allclose(v0, numpy.dot(R, v0)) + True + >>> v2 = v0.copy() + >>> v2[:3] += v1 + >>> v3 = v0.copy() + >>> v2[:3] -= v1 + >>> numpy.allclose(v2, numpy.dot(R, v3)) + True + + """ + normal = unit_vector(normal[:3]) + M = numpy.identity(4) + M[:3, :3] -= 2.0 * numpy.outer(normal, normal) + M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal + return M + + +def reflection_from_matrix(matrix): + """Return mirror plane point and normal vector from reflection matrix. + + >>> v0 = numpy.random.random(3) - 0.5 + >>> v1 = numpy.random.random(3) - 0.5 + >>> M0 = reflection_matrix(v0, v1) + >>> point, normal = reflection_from_matrix(M0) + >>> M1 = reflection_matrix(point, normal) + >>> is_same_transform(M0, M1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False) + # normal: unit eigenvector corresponding to eigenvalue -1 + l, V = numpy.linalg.eig(M[:3, :3]) + i = numpy.where(abs(numpy.real(l) + 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no unit eigenvector corresponding to eigenvalue -1") + normal = numpy.real(V[:, i[0]]).squeeze() + # point: any unit eigenvector corresponding to eigenvalue 1 + l, V = numpy.linalg.eig(M) + i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no unit eigenvector corresponding to eigenvalue 1") + point = numpy.real(V[:, i[-1]]).squeeze() + point /= point[3] + return point, normal + + +def rotation_matrix(angle, direction, point=None): + """Return matrix to rotate about axis defined by point and direction. + + >>> angle = (random.random() - 0.5) * (2*math.pi) + >>> direc = numpy.random.random(3) - 0.5 + >>> point = numpy.random.random(3) - 0.5 + >>> R0 = rotation_matrix(angle, direc, point) + >>> R1 = rotation_matrix(angle-2*math.pi, direc, point) + >>> is_same_transform(R0, R1) + True + >>> R0 = rotation_matrix(angle, direc, point) + >>> R1 = rotation_matrix(-angle, -direc, point) + >>> is_same_transform(R0, R1) + True + >>> I = numpy.identity(4, numpy.float64) + >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc)) + True + >>> numpy.allclose(2., numpy.trace(rotation_matrix(math.pi/2, + ... direc, point))) + True + + """ + sina = math.sin(angle) + cosa = math.cos(angle) + direction = unit_vector(direction[:3]) + # rotation matrix around unit vector + R = numpy.array(((cosa, 0.0, 0.0), + (0.0, cosa, 0.0), + (0.0, 0.0, cosa)), dtype=numpy.float64) + R += numpy.outer(direction, direction) * (1.0 - cosa) + direction *= sina + R += numpy.array((( 0.0, -direction[2], direction[1]), + ( direction[2], 0.0, -direction[0]), + (-direction[1], direction[0], 0.0)), + dtype=numpy.float64) + M = numpy.identity(4) + M[:3, :3] = R + if point is not None: + # rotation not around origin + point = numpy.array(point[:3], dtype=numpy.float64, copy=False) + M[:3, 3] = point - numpy.dot(R, point) + return M + + +def rotation_from_matrix(matrix): + """Return rotation angle and axis from rotation matrix. + + >>> angle = (random.random() - 0.5) * (2*math.pi) + >>> direc = numpy.random.random(3) - 0.5 + >>> point = numpy.random.random(3) - 0.5 + >>> R0 = rotation_matrix(angle, direc, point) + >>> angle, direc, point = rotation_from_matrix(R0) + >>> R1 = rotation_matrix(angle, direc, point) + >>> is_same_transform(R0, R1) + True + + """ + R = numpy.array(matrix, dtype=numpy.float64, copy=False) + R33 = R[:3, :3] + # direction: unit eigenvector of R33 corresponding to eigenvalue of 1 + l, W = numpy.linalg.eig(R33.T) + i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no unit eigenvector corresponding to eigenvalue 1") + direction = numpy.real(W[:, i[-1]]).squeeze() + # point: unit eigenvector of R33 corresponding to eigenvalue of 1 + l, Q = numpy.linalg.eig(R) + i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no unit eigenvector corresponding to eigenvalue 1") + point = numpy.real(Q[:, i[-1]]).squeeze() + point /= point[3] + # rotation angle depending on direction + cosa = (numpy.trace(R33) - 1.0) / 2.0 + if abs(direction[2]) > 1e-8: + sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2] + elif abs(direction[1]) > 1e-8: + sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1] + else: + sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0] + angle = math.atan2(sina, cosa) + return angle, direction, point + + +def scale_matrix(factor, origin=None, direction=None): + """Return matrix to scale by factor around origin in direction. + + Use factor -1 for point symmetry. + + >>> v = (numpy.random.rand(4, 5) - 0.5) * 20.0 + >>> v[3] = 1.0 + >>> S = scale_matrix(-1.234) + >>> numpy.allclose(numpy.dot(S, v)[:3], -1.234*v[:3]) + True + >>> factor = random.random() * 10 - 5 + >>> origin = numpy.random.random(3) - 0.5 + >>> direct = numpy.random.random(3) - 0.5 + >>> S = scale_matrix(factor, origin) + >>> S = scale_matrix(factor, origin, direct) + + """ + if direction is None: + # uniform scaling + M = numpy.array(((factor, 0.0, 0.0, 0.0), + (0.0, factor, 0.0, 0.0), + (0.0, 0.0, factor, 0.0), + (0.0, 0.0, 0.0, 1.0)), dtype=numpy.float64) + if origin is not None: + M[:3, 3] = origin[:3] + M[:3, 3] *= 1.0 - factor + else: + # nonuniform scaling + direction = unit_vector(direction[:3]) + factor = 1.0 - factor + M = numpy.identity(4) + M[:3, :3] -= factor * numpy.outer(direction, direction) + if origin is not None: + M[:3, 3] = (factor * numpy.dot(origin[:3], direction)) * direction + return M + + +def scale_from_matrix(matrix): + """Return scaling factor, origin and direction from scaling matrix. + + >>> factor = random.random() * 10 - 5 + >>> origin = numpy.random.random(3) - 0.5 + >>> direct = numpy.random.random(3) - 0.5 + >>> S0 = scale_matrix(factor, origin) + >>> factor, origin, direction = scale_from_matrix(S0) + >>> S1 = scale_matrix(factor, origin, direction) + >>> is_same_transform(S0, S1) + True + >>> S0 = scale_matrix(factor, origin, direct) + >>> factor, origin, direction = scale_from_matrix(S0) + >>> S1 = scale_matrix(factor, origin, direction) + >>> is_same_transform(S0, S1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False) + M33 = M[:3, :3] + factor = numpy.trace(M33) - 2.0 + try: + # direction: unit eigenvector corresponding to eigenvalue factor + l, V = numpy.linalg.eig(M33) + i = numpy.where(abs(numpy.real(l) - factor) < 1e-8)[0][0] + direction = numpy.real(V[:, i]).squeeze() + direction /= vector_norm(direction) + except IndexError: + # uniform scaling + factor = (factor + 2.0) / 3.0 + direction = None + # origin: any eigenvector corresponding to eigenvalue 1 + l, V = numpy.linalg.eig(M) + i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no eigenvector corresponding to eigenvalue 1") + origin = numpy.real(V[:, i[-1]]).squeeze() + origin /= origin[3] + return factor, origin, direction + + +def projection_matrix(point, normal, direction=None, + perspective=None, pseudo=False): + """Return matrix to project onto plane defined by point and normal. + + Using either perspective point, projection direction, or none of both. + + If pseudo is True, perspective projections will preserve relative depth + such that Perspective = dot(Orthogonal, PseudoPerspective). + + >>> P = projection_matrix((0, 0, 0), (1, 0, 0)) + >>> numpy.allclose(P[1:, 1:], numpy.identity(4)[1:, 1:]) + True + >>> point = numpy.random.random(3) - 0.5 + >>> normal = numpy.random.random(3) - 0.5 + >>> direct = numpy.random.random(3) - 0.5 + >>> persp = numpy.random.random(3) - 0.5 + >>> P0 = projection_matrix(point, normal) + >>> P1 = projection_matrix(point, normal, direction=direct) + >>> P2 = projection_matrix(point, normal, perspective=persp) + >>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True) + >>> is_same_transform(P2, numpy.dot(P0, P3)) + True + >>> P = projection_matrix((3, 0, 0), (1, 1, 0), (1, 0, 0)) + >>> v0 = (numpy.random.rand(4, 5) - 0.5) * 20.0 + >>> v0[3] = 1.0 + >>> v1 = numpy.dot(P, v0) + >>> numpy.allclose(v1[1], v0[1]) + True + >>> numpy.allclose(v1[0], 3.0-v1[1]) + True + + """ + M = numpy.identity(4) + point = numpy.array(point[:3], dtype=numpy.float64, copy=False) + normal = unit_vector(normal[:3]) + if perspective is not None: + # perspective projection + perspective = numpy.array(perspective[:3], dtype=numpy.float64, + copy=False) + M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective-point, normal) + M[:3, :3] -= numpy.outer(perspective, normal) + if pseudo: + # preserve relative depth + M[:3, :3] -= numpy.outer(normal, normal) + M[:3, 3] = numpy.dot(point, normal) * (perspective+normal) + else: + M[:3, 3] = numpy.dot(point, normal) * perspective + M[3, :3] = -normal + M[3, 3] = numpy.dot(perspective, normal) + elif direction is not None: + # parallel projection + direction = numpy.array(direction[:3], dtype=numpy.float64, copy=False) + scale = numpy.dot(direction, normal) + M[:3, :3] -= numpy.outer(direction, normal) / scale + M[:3, 3] = direction * (numpy.dot(point, normal) / scale) + else: + # orthogonal projection + M[:3, :3] -= numpy.outer(normal, normal) + M[:3, 3] = numpy.dot(point, normal) * normal + return M + + +def projection_from_matrix(matrix, pseudo=False): + """Return projection plane and perspective point from projection matrix. + + Return values are same as arguments for projection_matrix function: + point, normal, direction, perspective, and pseudo. + + >>> point = numpy.random.random(3) - 0.5 + >>> normal = numpy.random.random(3) - 0.5 + >>> direct = numpy.random.random(3) - 0.5 + >>> persp = numpy.random.random(3) - 0.5 + >>> P0 = projection_matrix(point, normal) + >>> result = projection_from_matrix(P0) + >>> P1 = projection_matrix(*result) + >>> is_same_transform(P0, P1) + True + >>> P0 = projection_matrix(point, normal, direct) + >>> result = projection_from_matrix(P0) + >>> P1 = projection_matrix(*result) + >>> is_same_transform(P0, P1) + True + >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False) + >>> result = projection_from_matrix(P0, pseudo=False) + >>> P1 = projection_matrix(*result) + >>> is_same_transform(P0, P1) + True + >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True) + >>> result = projection_from_matrix(P0, pseudo=True) + >>> P1 = projection_matrix(*result) + >>> is_same_transform(P0, P1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False) + M33 = M[:3, :3] + l, V = numpy.linalg.eig(M) + i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] + if not pseudo and len(i): + # point: any eigenvector corresponding to eigenvalue 1 + point = numpy.real(V[:, i[-1]]).squeeze() + point /= point[3] + # direction: unit eigenvector corresponding to eigenvalue 0 + l, V = numpy.linalg.eig(M33) + i = numpy.where(abs(numpy.real(l)) < 1e-8)[0] + if not len(i): + raise ValueError("no eigenvector corresponding to eigenvalue 0") + direction = numpy.real(V[:, i[0]]).squeeze() + direction /= vector_norm(direction) + # normal: unit eigenvector of M33.T corresponding to eigenvalue 0 + l, V = numpy.linalg.eig(M33.T) + i = numpy.where(abs(numpy.real(l)) < 1e-8)[0] + if len(i): + # parallel projection + normal = numpy.real(V[:, i[0]]).squeeze() + normal /= vector_norm(normal) + return point, normal, direction, None, False + else: + # orthogonal projection, where normal equals direction vector + return point, direction, None, None, False + else: + # perspective projection + i = numpy.where(abs(numpy.real(l)) > 1e-8)[0] + if not len(i): + raise ValueError( + "no eigenvector not corresponding to eigenvalue 0") + point = numpy.real(V[:, i[-1]]).squeeze() + point /= point[3] + normal = - M[3, :3] + perspective = M[:3, 3] / numpy.dot(point[:3], normal) + if pseudo: + perspective -= normal + return point, normal, None, perspective, pseudo + + +def clip_matrix(left, right, bottom, top, near, far, perspective=False): + """Return matrix to obtain normalized device coordinates from frustrum. + + The frustrum bounds are axis-aligned along x (left, right), + y (bottom, top) and z (near, far). + + Normalized device coordinates are in range [-1, 1] if coordinates are + inside the frustrum. + + If perspective is True the frustrum is a truncated pyramid with the + perspective point at origin and direction along z axis, otherwise an + orthographic canonical view volume (a box). + + Homogeneous coordinates transformed by the perspective clip matrix + need to be dehomogenized (devided by w coordinate). + + >>> frustrum = numpy.random.rand(6) + >>> frustrum[1] += frustrum[0] + >>> frustrum[3] += frustrum[2] + >>> frustrum[5] += frustrum[4] + >>> M = clip_matrix(*frustrum, perspective=False) + >>> numpy.dot(M, [frustrum[0], frustrum[2], frustrum[4], 1.0]) + array([-1., -1., -1., 1.]) + >>> numpy.dot(M, [frustrum[1], frustrum[3], frustrum[5], 1.0]) + array([ 1., 1., 1., 1.]) + >>> M = clip_matrix(*frustrum, perspective=True) + >>> v = numpy.dot(M, [frustrum[0], frustrum[2], frustrum[4], 1.0]) + >>> v / v[3] + array([-1., -1., -1., 1.]) + >>> v = numpy.dot(M, [frustrum[1], frustrum[3], frustrum[4], 1.0]) + >>> v / v[3] + array([ 1., 1., -1., 1.]) + + """ + if left >= right or bottom >= top or near >= far: + raise ValueError("invalid frustrum") + if perspective: + if near <= _EPS: + raise ValueError("invalid frustrum: near <= 0") + t = 2.0 * near + M = ((-t/(right-left), 0.0, (right+left)/(right-left), 0.0), + (0.0, -t/(top-bottom), (top+bottom)/(top-bottom), 0.0), + (0.0, 0.0, -(far+near)/(far-near), t*far/(far-near)), + (0.0, 0.0, -1.0, 0.0)) + else: + M = ((2.0/(right-left), 0.0, 0.0, (right+left)/(left-right)), + (0.0, 2.0/(top-bottom), 0.0, (top+bottom)/(bottom-top)), + (0.0, 0.0, 2.0/(far-near), (far+near)/(near-far)), + (0.0, 0.0, 0.0, 1.0)) + return numpy.array(M, dtype=numpy.float64) + + +def shear_matrix(angle, direction, point, normal): + """Return matrix to shear by angle along direction vector on shear plane. + + The shear plane is defined by a point and normal vector. The direction + vector must be orthogonal to the plane's normal vector. + + A point P is transformed by the shear matrix into P" such that + the vector P-P" is parallel to the direction vector and its extent is + given by the angle of P-P'-P", where P' is the orthogonal projection + of P onto the shear plane. + + >>> angle = (random.random() - 0.5) * 4*math.pi + >>> direct = numpy.random.random(3) - 0.5 + >>> point = numpy.random.random(3) - 0.5 + >>> normal = numpy.cross(direct, numpy.random.random(3)) + >>> S = shear_matrix(angle, direct, point, normal) + >>> numpy.allclose(1.0, numpy.linalg.det(S)) + True + + """ + normal = unit_vector(normal[:3]) + direction = unit_vector(direction[:3]) + if abs(numpy.dot(normal, direction)) > 1e-6: + raise ValueError("direction and normal vectors are not orthogonal") + angle = math.tan(angle) + M = numpy.identity(4) + M[:3, :3] += angle * numpy.outer(direction, normal) + M[:3, 3] = -angle * numpy.dot(point[:3], normal) * direction + return M + + +def shear_from_matrix(matrix): + """Return shear angle, direction and plane from shear matrix. + + >>> angle = (random.random() - 0.5) * 4*math.pi + >>> direct = numpy.random.random(3) - 0.5 + >>> point = numpy.random.random(3) - 0.5 + >>> normal = numpy.cross(direct, numpy.random.random(3)) + >>> S0 = shear_matrix(angle, direct, point, normal) + >>> angle, direct, point, normal = shear_from_matrix(S0) + >>> S1 = shear_matrix(angle, direct, point, normal) + >>> is_same_transform(S0, S1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False) + M33 = M[:3, :3] + # normal: cross independent eigenvectors corresponding to the eigenvalue 1 + l, V = numpy.linalg.eig(M33) + i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-4)[0] + if len(i) < 2: + raise ValueError("No two linear independent eigenvectors found %s" % l) + V = numpy.real(V[:, i]).squeeze().T + lenorm = -1.0 + for i0, i1 in ((0, 1), (0, 2), (1, 2)): + n = numpy.cross(V[i0], V[i1]) + l = vector_norm(n) + if l > lenorm: + lenorm = l + normal = n + normal /= lenorm + # direction and angle + direction = numpy.dot(M33 - numpy.identity(3), normal) + angle = vector_norm(direction) + direction /= angle + angle = math.atan(angle) + # point: eigenvector corresponding to eigenvalue 1 + l, V = numpy.linalg.eig(M) + i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no eigenvector corresponding to eigenvalue 1") + point = numpy.real(V[:, i[-1]]).squeeze() + point /= point[3] + return angle, direction, point, normal + + +def decompose_matrix(matrix): + """Return sequence of transformations from transformation matrix. + + matrix : array_like + Non-degenerative homogeneous transformation matrix + + Return tuple of: + scale : vector of 3 scaling factors + shear : list of shear factors for x-y, x-z, y-z axes + angles : list of Euler angles about static x, y, z axes + translate : translation vector along x, y, z axes + perspective : perspective partition of matrix + + Raise ValueError if matrix is of wrong type or degenerative. + + >>> T0 = translation_matrix((1, 2, 3)) + >>> scale, shear, angles, trans, persp = decompose_matrix(T0) + >>> T1 = translation_matrix(trans) + >>> numpy.allclose(T0, T1) + True + >>> S = scale_matrix(0.123) + >>> scale, shear, angles, trans, persp = decompose_matrix(S) + >>> scale[0] + 0.123 + >>> R0 = euler_matrix(1, 2, 3) + >>> scale, shear, angles, trans, persp = decompose_matrix(R0) + >>> R1 = euler_matrix(*angles) + >>> numpy.allclose(R0, R1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=True).T + if abs(M[3, 3]) < _EPS: + raise ValueError("M[3, 3] is zero") + M /= M[3, 3] + P = M.copy() + P[:, 3] = 0, 0, 0, 1 + if not numpy.linalg.det(P): + raise ValueError("Matrix is singular") + + scale = numpy.zeros((3, ), dtype=numpy.float64) + shear = [0, 0, 0] + angles = [0, 0, 0] + + if any(abs(M[:3, 3]) > _EPS): + perspective = numpy.dot(M[:, 3], numpy.linalg.inv(P.T)) + M[:, 3] = 0, 0, 0, 1 + else: + perspective = numpy.array((0, 0, 0, 1), dtype=numpy.float64) + + translate = M[3, :3].copy() + M[3, :3] = 0 + + row = M[:3, :3].copy() + scale[0] = vector_norm(row[0]) + row[0] /= scale[0] + shear[0] = numpy.dot(row[0], row[1]) + row[1] -= row[0] * shear[0] + scale[1] = vector_norm(row[1]) + row[1] /= scale[1] + shear[0] /= scale[1] + shear[1] = numpy.dot(row[0], row[2]) + row[2] -= row[0] * shear[1] + shear[2] = numpy.dot(row[1], row[2]) + row[2] -= row[1] * shear[2] + scale[2] = vector_norm(row[2]) + row[2] /= scale[2] + shear[1:] /= scale[2] + + if numpy.dot(row[0], numpy.cross(row[1], row[2])) < 0: + scale *= -1 + row *= -1 + + angles[1] = math.asin(-row[0, 2]) + if math.cos(angles[1]): + angles[0] = math.atan2(row[1, 2], row[2, 2]) + angles[2] = math.atan2(row[0, 1], row[0, 0]) + else: + #angles[0] = math.atan2(row[1, 0], row[1, 1]) + angles[0] = math.atan2(-row[2, 1], row[1, 1]) + angles[2] = 0.0 + + return scale, shear, angles, translate, perspective + + +def compose_matrix(scale=None, shear=None, angles=None, translate=None, + perspective=None): + """Return transformation matrix from sequence of transformations. + + This is the inverse of the decompose_matrix function. + + Sequence of transformations: + scale : vector of 3 scaling factors + shear : list of shear factors for x-y, x-z, y-z axes + angles : list of Euler angles about static x, y, z axes + translate : translation vector along x, y, z axes + perspective : perspective partition of matrix + + >>> scale = numpy.random.random(3) - 0.5 + >>> shear = numpy.random.random(3) - 0.5 + >>> angles = (numpy.random.random(3) - 0.5) * (2*math.pi) + >>> trans = numpy.random.random(3) - 0.5 + >>> persp = numpy.random.random(4) - 0.5 + >>> M0 = compose_matrix(scale, shear, angles, trans, persp) + >>> result = decompose_matrix(M0) + >>> M1 = compose_matrix(*result) + >>> is_same_transform(M0, M1) + True + + """ + M = numpy.identity(4) + if perspective is not None: + P = numpy.identity(4) + P[3, :] = perspective[:4] + M = numpy.dot(M, P) + if translate is not None: + T = numpy.identity(4) + T[:3, 3] = translate[:3] + M = numpy.dot(M, T) + if angles is not None: + R = euler_matrix(angles[0], angles[1], angles[2], 'sxyz') + M = numpy.dot(M, R) + if shear is not None: + Z = numpy.identity(4) + Z[1, 2] = shear[2] + Z[0, 2] = shear[1] + Z[0, 1] = shear[0] + M = numpy.dot(M, Z) + if scale is not None: + S = numpy.identity(4) + S[0, 0] = scale[0] + S[1, 1] = scale[1] + S[2, 2] = scale[2] + M = numpy.dot(M, S) + M /= M[3, 3] + return M + + +def orthogonalization_matrix(lengths, angles): + """Return orthogonalization matrix for crystallographic cell coordinates. + + Angles are expected in degrees. + + The de-orthogonalization matrix is the inverse. + + >>> O = orthogonalization_matrix((10., 10., 10.), (90., 90., 90.)) + >>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10) + True + >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7]) + >>> numpy.allclose(numpy.sum(O), 43.063229) + True + + """ + a, b, c = lengths + angles = numpy.radians(angles) + sina, sinb, _ = numpy.sin(angles) + cosa, cosb, cosg = numpy.cos(angles) + co = (cosa * cosb - cosg) / (sina * sinb) + return numpy.array(( + ( a*sinb*math.sqrt(1.0-co*co), 0.0, 0.0, 0.0), + (-a*sinb*co, b*sina, 0.0, 0.0), + ( a*cosb, b*cosa, c, 0.0), + ( 0.0, 0.0, 0.0, 1.0)), + dtype=numpy.float64) + + +def superimposition_matrix(v0, v1, scaling=False, usesvd=True): + """Return matrix to transform given vector set into second vector set. + + v0 and v1 are shape (3, \*) or (4, \*) arrays of at least 3 vectors. + + If usesvd is True, the weighted sum of squared deviations (RMSD) is + minimized according to the algorithm by W. Kabsch [8]. Otherwise the + quaternion based algorithm by B. Horn [9] is used (slower when using + this Python implementation). + + The returned matrix performs rotation, translation and uniform scaling + (if specified). + + >>> v0 = numpy.random.rand(3, 10) + >>> M = superimposition_matrix(v0, v0) + >>> numpy.allclose(M, numpy.identity(4)) + True + >>> R = random_rotation_matrix(numpy.random.random(3)) + >>> v0 = ((1,0,0), (0,1,0), (0,0,1), (1,1,1)) + >>> v1 = numpy.dot(R, v0) + >>> M = superimposition_matrix(v0, v1) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20.0 + >>> v0[3] = 1.0 + >>> v1 = numpy.dot(R, v0) + >>> M = superimposition_matrix(v0, v1) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + >>> S = scale_matrix(random.random()) + >>> T = translation_matrix(numpy.random.random(3)-0.5) + >>> M = concatenate_matrices(T, R, S) + >>> v1 = numpy.dot(M, v0) + >>> v0[:3] += numpy.random.normal(0.0, 1e-9, 300).reshape(3, -1) + >>> M = superimposition_matrix(v0, v1, scaling=True) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + >>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + >>> v = numpy.empty((4, 100, 3), dtype=numpy.float64) + >>> v[:, :, 0] = v0 + >>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False) + >>> numpy.allclose(v1, numpy.dot(M, v[:, :, 0])) + True + + """ + v0 = numpy.array(v0, dtype=numpy.float64, copy=False)[:3] + v1 = numpy.array(v1, dtype=numpy.float64, copy=False)[:3] + + if v0.shape != v1.shape or v0.shape[1] < 3: + raise ValueError("Vector sets are of wrong shape or type.") + + # move centroids to origin + t0 = numpy.mean(v0, axis=1) + t1 = numpy.mean(v1, axis=1) + v0 = v0 - t0.reshape(3, 1) + v1 = v1 - t1.reshape(3, 1) + + if usesvd: + # Singular Value Decomposition of covariance matrix + u, s, vh = numpy.linalg.svd(numpy.dot(v1, v0.T)) + # rotation matrix from SVD orthonormal bases + R = numpy.dot(u, vh) + if numpy.linalg.det(R) < 0.0: + # R does not constitute right handed system + R -= numpy.outer(u[:, 2], vh[2, :]*2.0) + s[-1] *= -1.0 + # homogeneous transformation matrix + M = numpy.identity(4) + M[:3, :3] = R + else: + # compute symmetric matrix N + xx, yy, zz = numpy.sum(v0 * v1, axis=1) + xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1) + xz, yx, zy = numpy.sum(v0 * numpy.roll(v1, -2, axis=0), axis=1) + N = ((xx+yy+zz, yz-zy, zx-xz, xy-yx), + (yz-zy, xx-yy-zz, xy+yx, zx+xz), + (zx-xz, xy+yx, -xx+yy-zz, yz+zy), + (xy-yx, zx+xz, yz+zy, -xx-yy+zz)) + # quaternion: eigenvector corresponding to most positive eigenvalue + l, V = numpy.linalg.eig(N) + q = V[:, numpy.argmax(l)] + q /= vector_norm(q) # unit quaternion + q = numpy.roll(q, -1) # move w component to end + # homogeneous transformation matrix + M = quaternion_matrix(q) + + # scale: ratio of rms deviations from centroid + if scaling: + v0 *= v0 + v1 *= v1 + M[:3, :3] *= math.sqrt(numpy.sum(v1) / numpy.sum(v0)) + + # translation + M[:3, 3] = t1 + T = numpy.identity(4) + T[:3, 3] = -t0 + M = numpy.dot(M, T) + return M + + +def euler_matrix(ai, aj, ak, axes='sxyz'): + """Return homogeneous rotation matrix from Euler angles and axis sequence. + + ai, aj, ak : Euler's roll, pitch and yaw angles + axes : One of 24 axis sequences as string or encoded tuple + + >>> R = euler_matrix(1, 2, 3, 'syxz') + >>> numpy.allclose(numpy.sum(R[0]), -1.34786452) + True + >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1)) + >>> numpy.allclose(numpy.sum(R[0]), -0.383436184) + True + >>> ai, aj, ak = (4.0*math.pi) * (numpy.random.random(3) - 0.5) + >>> for axes in _AXES2TUPLE.keys(): + ... R = euler_matrix(ai, aj, ak, axes) + >>> for axes in _TUPLE2AXES.keys(): + ... R = euler_matrix(ai, aj, ak, axes) + + """ + try: + firstaxis, parity, repetition, frame = _AXES2TUPLE[axes] + except (AttributeError, KeyError): + _ = _TUPLE2AXES[axes] + firstaxis, parity, repetition, frame = axes + + i = firstaxis + j = _NEXT_AXIS[i+parity] + k = _NEXT_AXIS[i-parity+1] + + if frame: + ai, ak = ak, ai + if parity: + ai, aj, ak = -ai, -aj, -ak + + si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak) + ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak) + cc, cs = ci*ck, ci*sk + sc, ss = si*ck, si*sk + + M = numpy.identity(4) + if repetition: + M[i, i] = cj + M[i, j] = sj*si + M[i, k] = sj*ci + M[j, i] = sj*sk + M[j, j] = -cj*ss+cc + M[j, k] = -cj*cs-sc + M[k, i] = -sj*ck + M[k, j] = cj*sc+cs + M[k, k] = cj*cc-ss + else: + M[i, i] = cj*ck + M[i, j] = sj*sc-cs + M[i, k] = sj*cc+ss + M[j, i] = cj*sk + M[j, j] = sj*ss+cc + M[j, k] = sj*cs-sc + M[k, i] = -sj + M[k, j] = cj*si + M[k, k] = cj*ci + return M + + +def euler_from_matrix(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 = numpy.array(matrix, dtype=numpy.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 euler_from_quaternion(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 euler_from_matrix(quaternion_matrix(quaternion), axes) + + +def quaternion_from_euler(ai, aj, ak, axes='sxyz'): + """Return quaternion from Euler angles and axis sequence. + + ai, aj, ak : Euler's roll, pitch and yaw angles + axes : One of 24 axis sequences as string or encoded tuple + + >>> q = quaternion_from_euler(1, 2, 3, 'ryxz') + >>> numpy.allclose(q, [0.310622, -0.718287, 0.444435, 0.435953]) + True + + """ + 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] + + if frame: + ai, ak = ak, ai + if parity: + aj = -aj + + ai /= 2.0 + aj /= 2.0 + ak /= 2.0 + ci = math.cos(ai) + si = math.sin(ai) + cj = math.cos(aj) + sj = math.sin(aj) + ck = math.cos(ak) + sk = math.sin(ak) + cc = ci*ck + cs = ci*sk + sc = si*ck + ss = si*sk + + quaternion = numpy.empty((4, ), dtype=numpy.float64) + if repetition: + quaternion[i] = cj*(cs + sc) + quaternion[j] = sj*(cc + ss) + quaternion[k] = sj*(cs - sc) + quaternion[3] = cj*(cc - ss) + else: + quaternion[i] = cj*sc - sj*cs + quaternion[j] = cj*ss + sj*cc + quaternion[k] = cj*cs - sj*sc + quaternion[3] = cj*cc + sj*ss + if parity: + quaternion[j] *= -1 + + return quaternion + + +def quaternion_about_axis(angle, axis): + """Return quaternion for rotation about axis. + + >>> q = quaternion_about_axis(0.123, (1, 0, 0)) + >>> numpy.allclose(q, [0.06146124, 0, 0, 0.99810947]) + True + + """ + quaternion = numpy.zeros((4, ), dtype=numpy.float64) + quaternion[:3] = axis[:3] + qlen = vector_norm(quaternion) + if qlen > _EPS: + quaternion *= math.sin(angle/2.0) / qlen + quaternion[3] = math.cos(angle/2.0) + return quaternion + + +def quaternion_matrix(quaternion): + """Return homogeneous rotation matrix from quaternion. + + >>> R = quaternion_matrix([0.06146124, 0, 0, 0.99810947]) + >>> numpy.allclose(R, rotation_matrix(0.123, (1, 0, 0))) + True + + """ + q = numpy.array(quaternion[:4], dtype=numpy.float64, copy=True) + nq = numpy.dot(q, q) + if nq < _EPS: + return numpy.identity(4) + q *= math.sqrt(2.0 / nq) + q = numpy.outer(q, q) + return numpy.array(( + (1.0-q[1, 1]-q[2, 2], q[0, 1]-q[2, 3], q[0, 2]+q[1, 3], 0.0), + ( q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2], q[1, 2]-q[0, 3], 0.0), + ( q[0, 2]-q[1, 3], q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], 0.0), + ( 0.0, 0.0, 0.0, 1.0) + ), dtype=numpy.float64) + + +def quaternion_from_matrix(matrix): + """Return quaternion from rotation matrix. + + >>> R = rotation_matrix(0.123, (1, 2, 3)) + >>> q = quaternion_from_matrix(R) + >>> numpy.allclose(q, [0.0164262, 0.0328524, 0.0492786, 0.9981095]) + True + + """ + q = numpy.empty((4, ), dtype=numpy.float64) + M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4] + t = numpy.trace(M) + if t > M[3, 3]: + q[3] = t + q[2] = M[1, 0] - M[0, 1] + q[1] = M[0, 2] - M[2, 0] + q[0] = M[2, 1] - M[1, 2] + else: + i, j, k = 0, 1, 2 + if M[1, 1] > M[0, 0]: + i, j, k = 1, 2, 0 + if M[2, 2] > M[i, i]: + i, j, k = 2, 0, 1 + t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3] + q[i] = t + q[j] = M[i, j] + M[j, i] + q[k] = M[k, i] + M[i, k] + q[3] = M[k, j] - M[j, k] + q *= 0.5 / math.sqrt(t * M[3, 3]) + return q + + +def quaternion_multiply(quaternion1, quaternion0): + """Return multiplication of two quaternions. + + >>> q = quaternion_multiply([1, -2, 3, 4], [-5, 6, 7, 8]) + >>> numpy.allclose(q, [-44, -14, 48, 28]) + True + + """ + x0, y0, z0, w0 = quaternion0 + x1, y1, z1, w1 = quaternion1 + return numpy.array(( + x1*w0 + y1*z0 - z1*y0 + w1*x0, + -x1*z0 + y1*w0 + z1*x0 + w1*y0, + x1*y0 - y1*x0 + z1*w0 + w1*z0, + -x1*x0 - y1*y0 - z1*z0 + w1*w0), dtype=numpy.float64) + + +def quaternion_conjugate(quaternion): + """Return conjugate of quaternion. + + >>> q0 = random_quaternion() + >>> q1 = quaternion_conjugate(q0) + >>> q1[3] == q0[3] and all(q1[:3] == -q0[:3]) + True + + """ + return numpy.array((-quaternion[0], -quaternion[1], + -quaternion[2], quaternion[3]), dtype=numpy.float64) + + +def quaternion_inverse(quaternion): + """Return inverse of quaternion. + + >>> q0 = random_quaternion() + >>> q1 = quaternion_inverse(q0) + >>> numpy.allclose(quaternion_multiply(q0, q1), [0, 0, 0, 1]) + True + + """ + return quaternion_conjugate(quaternion) / numpy.dot(quaternion, quaternion) + + +def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True): + """Return spherical linear interpolation between two quaternions. + + >>> q0 = random_quaternion() + >>> q1 = random_quaternion() + >>> q = quaternion_slerp(q0, q1, 0.0) + >>> numpy.allclose(q, q0) + True + >>> q = quaternion_slerp(q0, q1, 1.0, 1) + >>> numpy.allclose(q, q1) + True + >>> q = quaternion_slerp(q0, q1, 0.5) + >>> angle = math.acos(numpy.dot(q0, q)) + >>> numpy.allclose(2.0, math.acos(numpy.dot(q0, q1)) / angle) or \ + numpy.allclose(2.0, math.acos(-numpy.dot(q0, q1)) / angle) + True + + """ + q0 = unit_vector(quat0[:4]) + q1 = unit_vector(quat1[:4]) + if fraction == 0.0: + return q0 + elif fraction == 1.0: + return q1 + d = numpy.dot(q0, q1) + if abs(abs(d) - 1.0) < _EPS: + return q0 + if shortestpath and d < 0.0: + # invert rotation + d = -d + q1 *= -1.0 + angle = math.acos(d) + spin * math.pi + if abs(angle) < _EPS: + return q0 + isin = 1.0 / math.sin(angle) + q0 *= math.sin((1.0 - fraction) * angle) * isin + q1 *= math.sin(fraction * angle) * isin + q0 += q1 + return q0 + + +def random_quaternion(rand=None): + """Return uniform random unit quaternion. + + rand: array like or None + Three independent random variables that are uniformly distributed + between 0 and 1. + + >>> q = random_quaternion() + >>> numpy.allclose(1.0, vector_norm(q)) + True + >>> q = random_quaternion(numpy.random.random(3)) + >>> q.shape + (4,) + + """ + if rand is None: + rand = numpy.random.rand(3) + else: + assert len(rand) == 3 + r1 = numpy.sqrt(1.0 - rand[0]) + r2 = numpy.sqrt(rand[0]) + pi2 = math.pi * 2.0 + t1 = pi2 * rand[1] + t2 = pi2 * rand[2] + return numpy.array((numpy.sin(t1)*r1, + numpy.cos(t1)*r1, + numpy.sin(t2)*r2, + numpy.cos(t2)*r2), dtype=numpy.float64) + + +def random_rotation_matrix(rand=None): + """Return uniform random rotation matrix. + + rnd: array like + Three independent random variables that are uniformly distributed + between 0 and 1 for each returned quaternion. + + >>> R = random_rotation_matrix() + >>> numpy.allclose(numpy.dot(R.T, R), numpy.identity(4)) + True + + """ + return quaternion_matrix(random_quaternion(rand)) + + +class Arcball(object): + """Virtual Trackball Control. + + >>> ball = Arcball() + >>> ball = Arcball(initial=numpy.identity(4)) + >>> ball.place([320, 320], 320) + >>> ball.down([500, 250]) + >>> ball.drag([475, 275]) + >>> R = ball.matrix() + >>> numpy.allclose(numpy.sum(R), 3.90583455) + True + >>> ball = Arcball(initial=[0, 0, 0, 1]) + >>> ball.place([320, 320], 320) + >>> ball.setaxes([1,1,0], [-1, 1, 0]) + >>> ball.setconstrain(True) + >>> ball.down([400, 200]) + >>> ball.drag([200, 400]) + >>> R = ball.matrix() + >>> numpy.allclose(numpy.sum(R), 0.2055924) + True + >>> ball.next() + + """ + + def __init__(self, initial=None): + """Initialize virtual trackball control. + + initial : quaternion or rotation matrix + + """ + self._axis = None + self._axes = None + self._radius = 1.0 + self._center = [0.0, 0.0] + self._vdown = numpy.array([0, 0, 1], dtype=numpy.float64) + self._constrain = False + + if initial is None: + self._qdown = numpy.array([0, 0, 0, 1], dtype=numpy.float64) + else: + initial = numpy.array(initial, dtype=numpy.float64) + if initial.shape == (4, 4): + self._qdown = quaternion_from_matrix(initial) + elif initial.shape == (4, ): + initial /= vector_norm(initial) + self._qdown = initial + else: + raise ValueError("initial not a quaternion or matrix.") + + self._qnow = self._qpre = self._qdown + + def place(self, center, radius): + """Place Arcball, e.g. when window size changes. + + center : sequence[2] + Window coordinates of trackball center. + radius : float + Radius of trackball in window coordinates. + + """ + self._radius = float(radius) + self._center[0] = center[0] + self._center[1] = center[1] + + def setaxes(self, *axes): + """Set axes to constrain rotations.""" + if axes is None: + self._axes = None + else: + self._axes = [unit_vector(axis) for axis in axes] + + def setconstrain(self, constrain): + """Set state of constrain to axis mode.""" + self._constrain = constrain == True + + def getconstrain(self): + """Return state of constrain to axis mode.""" + return self._constrain + + def down(self, point): + """Set initial cursor window coordinates and pick constrain-axis.""" + self._vdown = arcball_map_to_sphere(point, self._center, self._radius) + self._qdown = self._qpre = self._qnow + + if self._constrain and self._axes is not None: + self._axis = arcball_nearest_axis(self._vdown, self._axes) + self._vdown = arcball_constrain_to_axis(self._vdown, self._axis) + else: + self._axis = None + + def drag(self, point): + """Update current cursor window coordinates.""" + vnow = arcball_map_to_sphere(point, self._center, self._radius) + + if self._axis is not None: + vnow = arcball_constrain_to_axis(vnow, self._axis) + + self._qpre = self._qnow + + t = numpy.cross(self._vdown, vnow) + if numpy.dot(t, t) < _EPS: + self._qnow = self._qdown + else: + q = [t[0], t[1], t[2], numpy.dot(self._vdown, vnow)] + self._qnow = quaternion_multiply(q, self._qdown) + + def next(self, acceleration=0.0): + """Continue rotation in direction of last drag.""" + q = quaternion_slerp(self._qpre, self._qnow, 2.0+acceleration, False) + self._qpre, self._qnow = self._qnow, q + + def matrix(self): + """Return homogeneous rotation matrix.""" + return quaternion_matrix(self._qnow) + + +def arcball_map_to_sphere(point, center, radius): + """Return unit sphere coordinates from window coordinates.""" + v = numpy.array(((point[0] - center[0]) / radius, + (center[1] - point[1]) / radius, + 0.0), dtype=numpy.float64) + n = v[0]*v[0] + v[1]*v[1] + if n > 1.0: + v /= math.sqrt(n) # position outside of sphere + else: + v[2] = math.sqrt(1.0 - n) + return v + + +def arcball_constrain_to_axis(point, axis): + """Return sphere point perpendicular to axis.""" + v = numpy.array(point, dtype=numpy.float64, copy=True) + a = numpy.array(axis, dtype=numpy.float64, copy=True) + v -= a * numpy.dot(a, v) # on plane + n = vector_norm(v) + if n > _EPS: + if v[2] < 0.0: + v *= -1.0 + v /= n + return v + if a[2] == 1.0: + return numpy.array([1, 0, 0], dtype=numpy.float64) + return unit_vector([-a[1], a[0], 0]) + + +def arcball_nearest_axis(point, axes): + """Return axis, which arc is nearest to point.""" + point = numpy.array(point, dtype=numpy.float64, copy=False) + nearest = None + mx = -1.0 + for axis in axes: + t = numpy.dot(arcball_constrain_to_axis(point, axis), point) + if t > mx: + nearest = axis + mx = t + return nearest + + +# epsilon for testing whether a number is close to zero +_EPS = numpy.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()) + +# helper functions + +def vector_norm(data, axis=None, out=None): + """Return length, i.e. eucledian norm, of ndarray along axis. + + >>> v = numpy.random.random(3) + >>> n = vector_norm(v) + >>> numpy.allclose(n, numpy.linalg.norm(v)) + True + >>> v = numpy.random.rand(6, 5, 3) + >>> n = vector_norm(v, axis=-1) + >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2))) + True + >>> n = vector_norm(v, axis=1) + >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) + True + >>> v = numpy.random.rand(5, 4, 3) + >>> n = numpy.empty((5, 3), dtype=numpy.float64) + >>> vector_norm(v, axis=1, out=n) + >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) + True + >>> vector_norm([]) + 0.0 + >>> vector_norm([1.0]) + 1.0 + + """ + data = numpy.array(data, dtype=numpy.float64, copy=True) + if out is None: + if data.ndim == 1: + return math.sqrt(numpy.dot(data, data)) + data *= data + out = numpy.atleast_1d(numpy.sum(data, axis=axis)) + numpy.sqrt(out, out) + return out + else: + data *= data + numpy.sum(data, axis=axis, out=out) + numpy.sqrt(out, out) + + +def unit_vector(data, axis=None, out=None): + """Return ndarray normalized by length, i.e. eucledian norm, along axis. + + >>> v0 = numpy.random.random(3) + >>> v1 = unit_vector(v0) + >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0)) + True + >>> v0 = numpy.random.rand(5, 4, 3) + >>> v1 = unit_vector(v0, axis=-1) + >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2) + >>> numpy.allclose(v1, v2) + True + >>> v1 = unit_vector(v0, axis=1) + >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1) + >>> numpy.allclose(v1, v2) + True + >>> v1 = numpy.empty((5, 4, 3), dtype=numpy.float64) + >>> unit_vector(v0, axis=1, out=v1) + >>> numpy.allclose(v1, v2) + True + >>> list(unit_vector([])) + [] + >>> list(unit_vector([1.0])) + [1.0] + + """ + if out is None: + data = numpy.array(data, dtype=numpy.float64, copy=True) + if data.ndim == 1: + data /= math.sqrt(numpy.dot(data, data)) + return data + else: + if out is not data: + out[:] = numpy.array(data, copy=False) + data = out + length = numpy.atleast_1d(numpy.sum(data*data, axis)) + numpy.sqrt(length, length) + if axis is not None: + length = numpy.expand_dims(length, axis) + data /= length + if out is None: + return data + + +def random_vector(size): + """Return array of random doubles in the half-open interval [0.0, 1.0). + + >>> v = random_vector(10000) + >>> numpy.all(v >= 0.0) and numpy.all(v < 1.0) + True + >>> v0 = random_vector(10) + >>> v1 = random_vector(10) + >>> numpy.any(v0 == v1) + False + + """ + return numpy.random.random(size) + + +def inverse_matrix(matrix): + """Return inverse of square transformation matrix. + + >>> M0 = random_rotation_matrix() + >>> M1 = inverse_matrix(M0.T) + >>> numpy.allclose(M1, numpy.linalg.inv(M0.T)) + True + >>> for size in range(1, 7): + ... M0 = numpy.random.rand(size, size) + ... M1 = inverse_matrix(M0) + ... if not numpy.allclose(M1, numpy.linalg.inv(M0)): print size + + """ + return numpy.linalg.inv(matrix) + + +def concatenate_matrices(*matrices): + """Return concatenation of series of transformation matrices. + + >>> M = numpy.random.rand(16).reshape((4, 4)) - 0.5 + >>> numpy.allclose(M, concatenate_matrices(M)) + True + >>> numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T)) + True + + """ + M = numpy.identity(4) + for i in matrices: + M = numpy.dot(M, i) + return M + + +def is_same_transform(matrix0, matrix1): + """Return True if two matrices perform same transformation. + + >>> is_same_transform(numpy.identity(4), numpy.identity(4)) + True + >>> is_same_transform(numpy.identity(4), random_rotation_matrix()) + False + + """ + matrix0 = numpy.array(matrix0, dtype=numpy.float64, copy=True) + matrix0 /= matrix0[3, 3] + matrix1 = numpy.array(matrix1, dtype=numpy.float64, copy=True) + matrix1 /= matrix1[3, 3] + return numpy.allclose(matrix0, matrix1) + + +def _import_module(module_name, warn=True, prefix='_py_', ignore='_'): + """Try import all public attributes from module into global namespace. + + Existing attributes with name clashes are renamed with prefix. + Attributes starting with underscore are ignored by default. + + Return True on successful import. + + """ + try: + module = __import__(module_name) + except ImportError: + if warn: + warnings.warn("Failed to import module " + module_name) + else: + for attr in dir(module): + if ignore and attr.startswith(ignore): + continue + if prefix: + if attr in globals(): + globals()[prefix + attr] = globals()[attr] + elif warn: + warnings.warn("No Python implementation of " + attr) + globals()[attr] = getattr(module, attr) + return True diff --git a/svo_msgs/CMakeLists.txt b/svo_msgs/CMakeLists.txt new file mode 100644 index 0000000..23d4e3b --- /dev/null +++ b/svo_msgs/CMakeLists.txt @@ -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} +) \ No newline at end of file diff --git a/svo_msgs/msg/DenseInput.msg b/svo_msgs/msg/DenseInput.msg new file mode 100644 index 0000000..d36dee8 --- /dev/null +++ b/svo_msgs/msg/DenseInput.msg @@ -0,0 +1,6 @@ +Header header +uint32 frame_id +geometry_msgs/Pose pose +sensor_msgs/Image image +float32 min_depth +float32 max_depth \ No newline at end of file diff --git a/svo_msgs/msg/Feature.msg b/svo_msgs/msg/Feature.msg new file mode 100644 index 0000000..2972c32 --- /dev/null +++ b/svo_msgs/msg/Feature.msg @@ -0,0 +1,5 @@ +float32 u +float32 v +float32 x +float32 y +float32 z \ No newline at end of file diff --git a/svo_msgs/msg/Info.msg b/svo_msgs/msg/Info.msg new file mode 100644 index 0000000..b962e1b --- /dev/null +++ b/svo_msgs/msg/Info.msg @@ -0,0 +1,6 @@ +Header header +float32 processing_time +int32[] keyframes +int32 num_matches +int32 tracking_quality +int32 stage \ No newline at end of file diff --git a/svo_msgs/msg/NbvTrajectory.msg b/svo_msgs/msg/NbvTrajectory.msg new file mode 100644 index 0000000..03f0792 --- /dev/null +++ b/svo_msgs/msg/NbvTrajectory.msg @@ -0,0 +1,2 @@ +Header header +geometry_msgs/Pose[] trajectory \ No newline at end of file diff --git a/svo_msgs/package.xml b/svo_msgs/package.xml new file mode 100644 index 0000000..b122e2d --- /dev/null +++ b/svo_msgs/package.xml @@ -0,0 +1,19 @@ + + + svo_msgs + 0.0.0 + Custom messages for svo + Christian Forster + BSD + catkin + + message_generation + geometry_msgs + sensor_msgs + + message_runtime + geometry_msgs + sensor_msgs + + + \ No newline at end of file diff --git a/svo_ros/CMakeLists.txt b/svo_ros/CMakeLists.txt new file mode 100644 index 0000000..8fcd739 --- /dev/null +++ b/svo_ros/CMakeLists.txt @@ -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) \ No newline at end of file diff --git a/svo_ros/include/svo_ros/dataset_img.h b/svo_ros/include/svo_ros/dataset_img.h new file mode 100644 index 0000000..2d78ef7 --- /dev/null +++ b/svo_ros/include/svo_ros/dataset_img.h @@ -0,0 +1,62 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef DATASET_IMG_H +#define DATASET_IMG_H + +namespace FileType { + +#include +#include +#include + +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 */ diff --git a/svo_ros/include/svo_ros/dataset_imu.h b/svo_ros/include/svo_ros/dataset_imu.h new file mode 100644 index 0000000..9ee5e66 --- /dev/null +++ b/svo_ros/include/svo_ros/dataset_imu.h @@ -0,0 +1,70 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef DATASET_IMU_H_ +#define DATASET_IMU_H_ + +namespace FileType { + +#include +#include +#include + +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_ */ diff --git a/svo_ros/include/svo_ros/visualizer.h b/svo_ros/include/svo_ros/visualizer.h new file mode 100644 index 0000000..1a7df1f --- /dev/null +++ b/svo_ros/include/svo_ros/visualizer.h @@ -0,0 +1,75 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_VISUALIZER_H_ +#define SVO_VISUALIZER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +class Frame; +class Point; +class Map; +class FrameHandlerMono; + +typedef boost::shared_ptr 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& core_kfs, const Map& map); + void publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono& slam, const double timestamp); + void publishMapRegion(set 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_ */ diff --git a/svo_ros/launch/live.launch b/svo_ros/launch/live.launch new file mode 100644 index 0000000..738d97a --- /dev/null +++ b/svo_ros/launch/live.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/svo_ros/launch/test_rig3.launch b/svo_ros/launch/test_rig3.launch new file mode 100644 index 0000000..d03359f --- /dev/null +++ b/svo_ros/launch/test_rig3.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/svo_ros/package.xml b/svo_ros/package.xml new file mode 100644 index 0000000..efc72e0 --- /dev/null +++ b/svo_ros/package.xml @@ -0,0 +1,42 @@ + + + svo_ros + 0.1.0 + + Monocular Visual Odometry - ROS Nodes + + Christian Forster + Christian Forster + GPLv3 + + + catkin + + + roscpp + nav_msgs + std_msgs + visualization_msgs + sensor_msgs + image_transport + cv_bridge + tf + svo + svo_msgs + vikit_common + vikit_ros + + + roscpp + nav_msgs + std_msgs + visualization_msgs + sensor_msgs + image_transport + cv_bridge + tf + svo + svo_msgs + vikit_common + vikit_ros + \ No newline at end of file diff --git a/svo_ros/param/vo_accurate.yaml b/svo_ros/param/vo_accurate.yaml new file mode 100644 index 0000000..8446875 --- /dev/null +++ b/svo_ros/param/vo_accurate.yaml @@ -0,0 +1,4 @@ +grid_size: 20 +max_n_kfs: 50 +loba_num_iter: 10 +max_fts: 200 \ No newline at end of file diff --git a/svo_ros/param/vo_fast.yaml b/svo_ros/param/vo_fast.yaml new file mode 100644 index 0000000..3df87dd --- /dev/null +++ b/svo_ros/param/vo_fast.yaml @@ -0,0 +1,3 @@ +grid_size: 30 +max_n_kfs: 10 +loba_num_iter: 0 \ No newline at end of file diff --git a/svo_ros/rqt.perspective b/svo_ros/rqt.perspective new file mode 100644 index 0000000..66dd7ef --- /dev/null +++ b/svo_ros/rqt.perspective @@ -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": {} + } + } + } + } +} \ No newline at end of file diff --git a/svo_ros/rviz_config.rviz b/svo_ros/rviz_config.rviz new file mode 100644 index 0000000..95d7c0e --- /dev/null +++ b/svo_ros/rviz_config.rviz @@ -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: + 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: + 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 diff --git a/svo_ros/src/benchmark_node.cpp b/svo_ros/src/benchmark_node.cpp new file mode 100644 index 0000000..2d0ff6e --- /dev/null +++ b/svo_ros/src/benchmark_node.cpp @@ -0,0 +1,155 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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("svo/dataset_directory")); + std::string filename_benchmark(benchmark_dir + "/images.txt"); + vk::FileReader dataset_reader(filename_benchmark); + dataset_reader.skipComments(); + if(!dataset_reader.next()) + std::runtime_error("Failed to open images file"); + std::vector 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::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 "<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; +} diff --git a/svo_ros/src/remote_node.cpp b/svo_ros/src/remote_node.cpp new file mode 100644 index 0000000..8952b21 --- /dev/null +++ b/svo_ros/src/remote_node.cpp @@ -0,0 +1,169 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class RemoteHandler +{ +public: + ros::NodeHandle& nh_; + vk::UserInputThread input_thread_; + + ros::Publisher pub_key_; + ros::Publisher pub_frames_; + tf::TransformBroadcaster br_; + + vk::RingBuffer acc_timing_; + vk::RingBuffer acc_obs_; + typedef std::map > pose_map_t; + pose_map_t poses_; + std::set published_kfs_; + std::set 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 ("/svo/remote_key", 10)), + pub_frames_(nh_.advertise("/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 >( + msg->header.seq, + std::pair(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 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::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 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::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::const_iterator it=msg->keyframes.begin(); it!=msg->keyframes.end(); ++it) { + current_kfs_.insert(static_cast(*it)); + if(*it == static_cast(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; +} diff --git a/svo_ros/src/visualizer.cpp b/svo_ros/src/visualizer.cpp new file mode 100644 index 0000000..d8aa72f --- /dev/null +++ b/svo_ros/src/visualizer.cpp @@ -0,0 +1,268 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +Visualizer:: +Visualizer() : + pnh_("~"), + trace_id_(0), + img_pub_level_(vk::getParam("svo/image_publishing_level", 0)), + img_pub_nth_(vk::getParam("svo/publish_every_nth_img", 1)), + dense_pub_nth_(vk::getParam("svo/publish_every_nth_dense_input", 1)) +{ + // Init ROS Marker Publishers + pub_frames_ = pnh_.advertise("keyframes", 10); + pub_points_ = pnh_.advertise("points", 1000); + pub_pose_ = pnh_.advertise("pose",10); + pub_info_ = pnh_.advertise("info", 10); + pub_dense_ = pnh_.advertise("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& 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::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(slam.stage()); + msg_info.tracking_quality = static_cast(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_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& px_ref(slam.initFeatureTrackRefPx()); + const vector& px_cur(slam.initFeatureTrackCurPx()); + for(vector::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 frames) +{ + if(pub_points_.getNumSubscribers() > 0) + { + int ts = vk::Timer::getCurrentTime(); + for(set::iterator it=frames.begin(); it!=frames.end(); ++it) + displayKeyframeWithMps(*it, ts); + } +} + +void Visualizer:: +removeDeletedPts(const Map& map) +{ + if(pub_points_.getNumSubscribers() > 0) + { + for(list::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::max(); + double max_z = std::numeric_limits::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 diff --git a/svo_ros/src/vo_node.cpp b/svo_ros/src/vo_node.cpp new file mode 100644 index 0000000..27b7ef0 --- /dev/null +++ b/svo_ros/src/vo_node.cpp @@ -0,0 +1,181 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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("svo/publish_markers", true)), + publish_dense_input_(vk::getParam("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("svo/init_rx", 0.0), + vk::getParam("svo/init_ry", 0.0), + vk::getParam("svo/init_rz", 0.0))), + Eigen::Vector3d(vk::getParam("svo/init_tx", 0.0), + vk::getParam("svo/init_ty", 0.0), + vk::getParam("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("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; +}