├── README.md └── corner_event_detector ├── CMakeLists.txt ├── cfg └── viewers.perspective ├── include └── corner_event_detector │ ├── detector.h │ ├── distinct_queue.h │ ├── fast_detector.h │ ├── fixed_distinct_queue.h │ ├── harris_detector.h │ ├── local_event_queues.h │ └── timer.h ├── launch ├── bag.launch └── davis_live.launch ├── package.xml └── src ├── detector.cpp ├── distinct_queue.cpp ├── fast_detector.cpp ├── fixed_distinct_queue.cpp ├── harris_detector.cpp └── node.cpp /README.md: -------------------------------------------------------------------------------- 1 | # Fast Event-based Corner Detection 2 | Inspired by frame-based pre-processing techniques that reduce an image to a set of features, which are typically the input to higher-level algorithms, we propose a method to reduce an event stream to a *corner event* stream. 3 | Our goal is twofold: extract relevant tracking information (corners do not suffer from the aperture problem) and decrease the event rate for later processing stages. 4 | Our event-based corner detector is very efficient due to its design principle, which consists of working on the Surface of Active Events (a map with the timestamp of the latest event at each pixel) using only comparison operations. 5 | Our method asynchronously processes event by event with very low latency. 6 | Our implementation is capable of processing a million events per second on a single core (less than a micro-second per event) and reduces the event rate by a factor of 10 to 20. 7 | 8 | ![corners_screenshot](https://user-images.githubusercontent.com/670994/33448948-9fd5c3fa-d607-11e7-8be3-d57fa93391ea.png) 9 | Left: image with *all* events, right: image with only corner events. Event color depicts polarity (i.e., the sign of the brightness change). 10 | 11 | [![YouTube video](http://rpg.ifi.uzh.ch/img/papers/BMVC17_Mueggler_thumb_play.jpg)](http://www.youtube.com/watch?v=tgvM4ELesgI) 12 | 13 | This code also contains the Spatially-Adaptive Harris Method used for comparison. 14 | For more details, please read our [BMVC'17 paper](http://rpg.ifi.uzh.ch/docs/BMVC17_Mueggler.pdf) or have look at the [poster](http://rpg.ifi.uzh.ch/docs/BMVC17_Mueggler_poster.pdf). 15 | 16 | ## Publication 17 | 18 | If you use this code in an academic context, please cite the following [BMVC'17 publication](http://rpg.ifi.uzh.ch/docs/BMVC17_Mueggler.pdf): 19 | 20 | E. Mueggler, C. Bartolozzi, D. Scaramuzza: 21 | **Fast Event-based Corner Detection.** 22 | British Machine Vision Conference (BMVC), London, 2017. 23 | 24 | @inproceedings{Mueggler17BMVC, 25 | author = {Mueggler, Elias and Bartolozzi, Chiara and Scaramuzza, Davide}, 26 | title = {Fast Event-based Corner Detection}, 27 | booktitle = {British Machine Vision Conference (BMVC)}, 28 | year = {2017} 29 | } 30 | 31 | # Disclaimer and License 32 | This code has been tested with ROS kinetic on Ubuntu 16.04. 33 | This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed. 34 | The source code is released under a GNU General Public License (GPL). 35 | For a commercial license, please contact the [Davide Scaramuzza](http://rpg.ifi.uzh.ch/people_scaramuzza.html) 36 | 37 | # Instructions 38 | ## Installation 39 | 1. Install the [DVS/DAVIS ROS driver](https://github.com/uzh-rpg/rpg_dvs_ros) (you only need the `dvs_msgs` and `dvs_renderer` packages). 40 | 2. Clone the repository to your ROS workspace 41 | ``` 42 | git clone https://github.com/uzh-rpg/rpg_corner_events 43 | ``` 44 | 3. Build it using the following command: 45 | ``` 46 | roscd corner_event_detector 47 | catkin build --this 48 | ``` 49 | 50 | ## Using a Dataset 51 | To get a bag file from the [Event-Camera Dataset](http://rpg.ifi.uzh.ch/davis_data.html): 52 | ``` 53 | wget http://rpg.ifi.uzh.ch/datasets/davis/shapes_6dof.bag 54 | ``` 55 | 56 | Run the detector and visualization launch file: 57 | ``` 58 | roslaunch corner_event_detector bag.launch 59 | ``` 60 | 61 | In a separate terminal, run a bag file, e.g.: 62 | ``` 63 | rosbag play shapes_6dof.bag 64 | ``` 65 | 66 | ## Using the DAVIS Event Camera (Live Mode) 67 | Please run the file: 68 | ``` 69 | roslaunch corner_event_detector davis_live.launch 70 | ``` 71 | 72 | -------------------------------------------------------------------------------- /corner_event_detector/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(corner_event_detector) 3 | 4 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") 6 | 7 | find_package(catkin_simple REQUIRED) 8 | catkin_simple() 9 | 10 | list(APPEND SOURCE_FILES 11 | src/detector.cpp 12 | src/harris_detector.cpp 13 | src/fast_detector.cpp 14 | src/distinct_queue.cpp 15 | src/fixed_distinct_queue.cpp 16 | ) 17 | 18 | list(APPEND HEADER_FILES 19 | include/corner_event_detector/detector.h 20 | include/corner_event_detector/harris_detector.h 21 | include/corner_event_detector/fast_detector.h 22 | include/corner_event_detector/local_event_queues.h 23 | include/corner_event_detector/distinct_queue.h 24 | include/corner_event_detector/fixed_distinct_queue.h 25 | ) 26 | 27 | cs_add_executable(detector ${SOURCE_FILES} src/node.cpp) 28 | 29 | cs_export() 30 | -------------------------------------------------------------------------------- /corner_event_detector/cfg/viewers.perspective: -------------------------------------------------------------------------------- 1 | { 2 | "keys": {}, 3 | "groups": { 4 | "pluginmanager": { 5 | "keys": { 6 | "running-plugins": { 7 | "type": "repr", 8 | "repr": "{u'rqt_image_view/ImageView': [2, 1]}" 9 | } 10 | }, 11 | "groups": { 12 | "plugin__rqt_plot__Plot__1": { 13 | "keys": {}, 14 | "groups": { 15 | "dock_widget__DataPlotWidget": { 16 | "keys": { 17 | "dockable": { 18 | "type": "repr", 19 | "repr": "u'true'" 20 | }, 21 | "parent": { 22 | "type": "repr", 23 | "repr": "None" 24 | }, 25 | "dock_widget_title": { 26 | "type": "repr", 27 | "repr": "u'MatPlot'" 28 | } 29 | }, 30 | "groups": {} 31 | }, 32 | "plugin": { 33 | "keys": { 34 | "autoscroll": { 35 | "type": "repr", 36 | "repr": "True" 37 | }, 38 | "plot_type": { 39 | "type": "repr", 40 | "repr": "1" 41 | }, 42 | "topics": { 43 | "type": "repr", 44 | "repr": "[u'/dvs/imu/angular_velocity/z', u'/dvs/imu/angular_velocity/y', u'/dvs/imu/angular_velocity/x']" 45 | }, 46 | "y_limits": { 47 | "type": "repr", 48 | "repr": "[-0.40160465909443543, 1.7773333160836877]" 49 | }, 50 | "x_limits": { 51 | "type": "repr", 52 | "repr": "[-13282.286030936195, -13281.248074054718]" 53 | } 54 | }, 55 | "groups": {} 56 | } 57 | } 58 | }, 59 | "plugin__rqt_plot__Plot__2": { 60 | "keys": {}, 61 | "groups": { 62 | "dock_widget__DataPlotWidget": { 63 | "keys": { 64 | "dockable": { 65 | "type": "repr", 66 | "repr": "u'true'" 67 | }, 68 | "parent": { 69 | "type": "repr", 70 | "repr": "None" 71 | }, 72 | "dock_widget_title": { 73 | "type": "repr", 74 | "repr": "u'MatPlot (2)'" 75 | } 76 | }, 77 | "groups": {} 78 | }, 79 | "plugin": { 80 | "keys": { 81 | "autoscroll": { 82 | "type": "repr", 83 | "repr": "True" 84 | }, 85 | "plot_type": { 86 | "type": "repr", 87 | "repr": "1" 88 | }, 89 | "topics": { 90 | "type": "repr", 91 | "repr": "[u'/dvs/imu/linear_acceleration/z', u'/dvs/imu/linear_acceleration/x', u'/dvs/imu/linear_acceleration/y']" 92 | }, 93 | "y_limits": { 94 | "type": "repr", 95 | "repr": "[-20.35244349131424, 14.270764048414978]" 96 | }, 97 | "x_limits": { 98 | "type": "repr", 99 | "repr": "[-13290.056386633185, -13289.843425989151]" 100 | } 101 | }, 102 | "groups": {} 103 | } 104 | } 105 | }, 106 | "plugin__rqt_image_view__ImageView__2": { 107 | "keys": {}, 108 | "groups": { 109 | "dock_widget__ImageViewWidget": { 110 | "keys": { 111 | "dockable": { 112 | "type": "repr", 113 | "repr": "True" 114 | }, 115 | "parent": { 116 | "type": "repr", 117 | "repr": "None" 118 | }, 119 | "dock_widget_title": { 120 | "type": "repr", 121 | "repr": "u'Image View (2)'" 122 | } 123 | }, 124 | "groups": {} 125 | }, 126 | "plugin": { 127 | "keys": { 128 | "max_range": { 129 | "type": "repr", 130 | "repr": "10.0" 131 | }, 132 | "mouse_pub_topic": { 133 | "type": "repr", 134 | "repr": "u'/dvs_rendering_corners_mouse_left'" 135 | }, 136 | "toolbar_hidden": { 137 | "type": "repr", 138 | "repr": "False" 139 | }, 140 | "zoom1": { 141 | "type": "repr", 142 | "repr": "False" 143 | }, 144 | "dynamic_range": { 145 | "type": "repr", 146 | "repr": "False" 147 | }, 148 | "topic": { 149 | "type": "repr", 150 | "repr": "u'/dvs_rendering_corners'" 151 | }, 152 | "publish_click_location": { 153 | "type": "repr", 154 | "repr": "False" 155 | } 156 | }, 157 | "groups": {} 158 | } 159 | } 160 | }, 161 | "plugin__rqt_topic__TopicPlugin__1": { 162 | "keys": {}, 163 | "groups": { 164 | "plugin": { 165 | "keys": { 166 | "tree_widget_header_state": { 167 | "type": "repr(QByteArray.hex)", 168 | "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000000000010000000000000000010000000000000000000000000000000000000228000000050101000100000000000000000500000064ffffffff0000008100000003000000050000008800000001000000030000008d00000001000000030000007200000001000000030000003d0000000100000003000000640000000100000003000003e800')", 169 | "pretty-print": " d r = d " 170 | } 171 | }, 172 | "groups": {} 173 | }, 174 | "dock_widget__TopicWidget": { 175 | "keys": { 176 | "dockable": { 177 | "type": "repr", 178 | "repr": "u'true'" 179 | }, 180 | "parent": { 181 | "type": "repr", 182 | "repr": "None" 183 | }, 184 | "dock_widget_title": { 185 | "type": "repr", 186 | "repr": "u'Topic Monitor'" 187 | } 188 | }, 189 | "groups": {} 190 | } 191 | } 192 | }, 193 | "plugin__rqt_image_view__ImageView__1": { 194 | "keys": {}, 195 | "groups": { 196 | "dock_widget__ImageViewWidget": { 197 | "keys": { 198 | "dockable": { 199 | "type": "repr", 200 | "repr": "True" 201 | }, 202 | "parent": { 203 | "type": "repr", 204 | "repr": "None" 205 | }, 206 | "dock_widget_title": { 207 | "type": "repr", 208 | "repr": "u'Image View'" 209 | } 210 | }, 211 | "groups": {} 212 | }, 213 | "plugin": { 214 | "keys": { 215 | "max_range": { 216 | "type": "repr", 217 | "repr": "10.0" 218 | }, 219 | "mouse_pub_topic": { 220 | "type": "repr", 221 | "repr": "u'/dvs_rendering_mouse_left'" 222 | }, 223 | "toolbar_hidden": { 224 | "type": "repr", 225 | "repr": "False" 226 | }, 227 | "zoom1": { 228 | "type": "repr", 229 | "repr": "False" 230 | }, 231 | "dynamic_range": { 232 | "type": "repr", 233 | "repr": "False" 234 | }, 235 | "topic": { 236 | "type": "repr", 237 | "repr": "u'/dvs_rendering'" 238 | }, 239 | "publish_click_location": { 240 | "type": "repr", 241 | "repr": "False" 242 | } 243 | }, 244 | "groups": {} 245 | } 246 | } 247 | } 248 | } 249 | }, 250 | "mainwindow": { 251 | "keys": { 252 | "geometry": { 253 | "type": "repr(QByteArray.hex)", 254 | "repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb00020000000007800000001800000eff000004af000007800000001800000eff000004af00000001000000000780')", 255 | "pretty-print": " " 256 | }, 257 | "state": { 258 | "type": "repr(QByteArray.hex)", 259 | "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000fd00000001000000030000078000000482fc0100000005fb0000004c007200710074005f0074006f007000690063005f005f0054006f0070006900630050006c007500670069006e005f005f0031005f005f0054006f0070006900630057006900640067006500740100000000000001090000000000000000fb00000042007200710074005f0070006c006f0074005f005f0050006c006f0074005f005f0031005f005f00440061007400610050006c006f007400570069006400670065007401000000000000038e0000000000000000fb00000042007200710074005f0070006c006f0074005f005f0050006c006f0074005f005f0032005f005f00440061007400610050006c006f00740057006900640067006500740100000000000007800000000000000000fb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0031005f005f0049006d00610067006500560069006500770057006900640067006500740100000000000003910000017700fffffffb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0032005f005f0049006d00610067006500560069006500770057006900640067006500740100000397000003e90000017700ffffff000007800000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", 260 | "pretty-print": " Lrqt_topic__TopicPlugin__1__TopicWidget Brqt_plot__Plot__1__DataPlotWidget Brqt_plot__Plot__2__DataPlotWidget Zrqt_image_view__ImageView__1__ImageViewWidget Zrqt_image_view__ImageView__2__ImageViewWidget 6MinimizedDockWidgetsToolbar " 261 | } 262 | }, 263 | "groups": { 264 | "toolbar_areas": { 265 | "keys": { 266 | "MinimizedDockWidgetsToolbar": { 267 | "type": "repr", 268 | "repr": "8" 269 | } 270 | }, 271 | "groups": {} 272 | } 273 | } 274 | } 275 | } 276 | } -------------------------------------------------------------------------------- /corner_event_detector/include/corner_event_detector/detector.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "corner_event_detector/timer.h" 7 | 8 | namespace corner_event_detector 9 | { 10 | 11 | class Detector 12 | { 13 | public: 14 | Detector(bool connect = true); 15 | virtual ~Detector(); 16 | 17 | // check if event 18 | virtual bool isFeature(const dvs_msgs::Event &e) = 0; 19 | 20 | protected: 21 | std::string detector_name_; 22 | 23 | private: 24 | // interface 25 | ros::NodeHandle nh_; 26 | ros::Publisher feature_pub_; 27 | ros::Subscriber event_sub_; 28 | void eventCallback(const dvs_msgs::EventArray::ConstPtr &msg); 29 | 30 | // statistics 31 | double total_time_; 32 | int total_events_, total_corners_; 33 | }; 34 | 35 | 36 | } // namespace 37 | -------------------------------------------------------------------------------- /corner_event_detector/include/corner_event_detector/distinct_queue.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "corner_event_detector/local_event_queues.h" 7 | #include "corner_event_detector/fixed_distinct_queue.h" 8 | 9 | namespace corner_event_detector 10 | { 11 | 12 | class DistinctQueue : public LocalEventQueues 13 | { 14 | public: 15 | DistinctQueue(int window_size, int queue_size, bool use_polarity); 16 | virtual ~DistinctQueue(); 17 | 18 | void newEvent(int x, int y, bool pol=false); 19 | bool isFull(int x, int y, bool pol=false) const; 20 | Eigen::MatrixXi getPatch(int x, int y, bool pol=false); 21 | 22 | private: 23 | // data structure 24 | std::vector queues_; 25 | 26 | // helper function 27 | int getIndex(int x, int y, bool polarity) const; 28 | 29 | // constants 30 | static const int sensor_width_ = 240; 31 | static const int sensor_height_ = 180; 32 | }; 33 | 34 | } // namespace 35 | -------------------------------------------------------------------------------- /corner_event_detector/include/corner_event_detector/fast_detector.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "corner_event_detector/detector.h" 11 | 12 | namespace corner_event_detector 13 | { 14 | 15 | class FastDetector : public Detector 16 | { 17 | public: 18 | FastDetector(bool connect = true); 19 | virtual ~FastDetector(); 20 | 21 | bool isFeature(const dvs_msgs::Event &e); 22 | 23 | private: 24 | // SAE 25 | Eigen::MatrixXd sae_[2]; 26 | 27 | // pixels on circle 28 | int circle3_[16][2]; 29 | int circle4_[20][2]; 30 | 31 | // parameters 32 | static const int sensor_width_ = 240; 33 | static const int sensor_height_ = 180; 34 | }; 35 | 36 | 37 | } // namespace 38 | -------------------------------------------------------------------------------- /corner_event_detector/include/corner_event_detector/fixed_distinct_queue.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace corner_event_detector 8 | { 9 | 10 | class FixedDistinctQueue 11 | { 12 | public: 13 | FixedDistinctQueue(int window, int queue); 14 | 15 | bool isFull() const; 16 | 17 | void addNew(int x, int y); 18 | Eigen::MatrixXi getWindow() const; 19 | 20 | private: 21 | // contains index of queue element if occupied, negative value otherwise 22 | Eigen::MatrixXi window_; 23 | // contains one event 24 | struct QueueEvent 25 | { 26 | int prev, next; 27 | int x, y; 28 | }; 29 | std::vector queue_; 30 | int first_, last_; 31 | int queue_max_; 32 | }; 33 | 34 | 35 | } // namespace 36 | -------------------------------------------------------------------------------- /corner_event_detector/include/corner_event_detector/harris_detector.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include "corner_event_detector/detector.h" 9 | 10 | #include "corner_event_detector/local_event_queues.h" 11 | #include "corner_event_detector/distinct_queue.h" 12 | 13 | namespace corner_event_detector 14 | { 15 | 16 | class HarrisDetector : public Detector 17 | { 18 | public: 19 | HarrisDetector(bool connect = true); 20 | virtual ~HarrisDetector(); 21 | 22 | bool isFeature(const dvs_msgs::Event &e); 23 | double getLastScore() const { 24 | return last_score_; 25 | } 26 | 27 | private: 28 | // methods 29 | void updateQueue(const int x, const int y, const dvs_msgs::Event &e); 30 | double getHarrisScore(int x, int y, bool polarity); 31 | 32 | // queues 33 | LocalEventQueues* queues_; 34 | 35 | // parameters 36 | int queue_size_; 37 | int window_size_; 38 | int kernel_size_; 39 | static const int sensor_width_ = 240; 40 | static const int sensor_height_ = 180; 41 | double harris_threshold_; 42 | 43 | double last_score_; 44 | 45 | // kernels 46 | Eigen::MatrixXd Gx_, h_; 47 | int factorial(int n) const; 48 | int pasc(int k, int n) const; 49 | }; 50 | 51 | 52 | } // namespace 53 | -------------------------------------------------------------------------------- /corner_event_detector/include/corner_event_detector/local_event_queues.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace corner_event_detector 6 | { 7 | 8 | class LocalEventQueues 9 | { 10 | public: 11 | LocalEventQueues(int window_size, int queue_size) 12 | : window_size_(window_size), queue_size_(queue_size) {} 13 | 14 | virtual void newEvent(int x, int y, bool pol) = 0; 15 | virtual bool isFull(int x, int y, bool pol) const = 0; 16 | virtual Eigen::MatrixXi getPatch(int x, int y, bool pol) = 0; 17 | 18 | protected: 19 | int window_size_; 20 | int queue_size_; 21 | }; 22 | 23 | } // namespace 24 | -------------------------------------------------------------------------------- /corner_event_detector/include/corner_event_detector/timer.h: -------------------------------------------------------------------------------- 1 | #ifndef UTILS_TIMER_H_ 2 | #define UTILS_TIMER_H_ 3 | 4 | // Source: https://gist.github.com/artivis/53a621d65a676723c0e87b6faadaeda8 5 | 6 | #include 7 | 8 | namespace utils 9 | { 10 | namespace time 11 | { 12 | using std::chrono::hours; 13 | using std::chrono::minutes; 14 | using std::chrono::seconds; 15 | using std::chrono::milliseconds; 16 | using std::chrono::microseconds; 17 | using std::chrono::nanoseconds; 18 | 19 | namespace details 20 | { 21 | template 22 | using time_point = std::chrono::time_point<_Clock, _Dur>; 23 | 24 | using default_clock_t = std::chrono::_V2::high_resolution_clock; 25 | } // namespace details 26 | 27 | /** 28 | * @brief Timer. A tic-toc timer. 29 | * 30 | * Mesure the elapsed time between construction - or tic() - 31 | * and toc(). The elapsed time is expressed in unit. 32 | * 33 | * @param unit. The time unit. 34 | * @see unit 35 | */ 36 | template 37 | class Timer 38 | { 39 | public: 40 | 41 | /** 42 | * @brief Timer. Launch the timer. 43 | */ 44 | Timer(): start_(now()) { } 45 | 46 | /** 47 | * @brief ~Timer. Default desctructor. 48 | */ 49 | ~Timer() = default; 50 | 51 | /** 52 | * @brief tic. Reset the timer. 53 | */ 54 | void tic() 55 | { 56 | start_ = now(); 57 | } 58 | 59 | /** 60 | * @brief toc. Return this elapsed time since construction or last tic(). 61 | * @return double. The elapsed time. 62 | * @see tic() 63 | */ 64 | template 65 | T toc() 66 | { 67 | return static_cast(cast_d(now() - start_).count()); 68 | } 69 | 70 | protected: 71 | 72 | details::time_point start_; 73 | 74 | template 75 | auto cast_d(Args&&... args) -> 76 | decltype(std::chrono::duration_cast(std::forward(args)...)) 77 | { 78 | return std::chrono::duration_cast(std::forward(args)...); 79 | } 80 | 81 | template 82 | auto cast(Args&&... args) -> 83 | decltype(std::chrono::time_point_cast(std::forward(args)...)) 84 | { 85 | return std::chrono::time_point_cast(std::forward(args)...); 86 | } 87 | 88 | auto now() -> 89 | decltype(std::declval>().cast(details::default_clock_t::now())) 90 | { 91 | return cast(std::chrono::system_clock::now()); 92 | } 93 | }; 94 | 95 | using timer_secs = Timer; 96 | using timer_msecs = Timer; 97 | using timer_usecs = Timer; 98 | using timer_nsecs = Timer; 99 | 100 | } // namespace time 101 | } // namespace utils 102 | #endif // UTILS_TIMER_H_ 103 | 104 | /* 105 | 106 | Usage : 107 | 108 | utils::timer_usecs timer; 109 | 110 | // do stuff 111 | 112 | auto elapsed_time_usecs = timer.toc(); 113 | 114 | */ 115 | -------------------------------------------------------------------------------- /corner_event_detector/launch/bag.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /corner_event_detector/launch/davis_live.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /corner_event_detector/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | corner_event_detector 4 | 0.0.0 5 | The corner_event_detector package 6 | Elias Mueggler 7 | GNU GPL 8 | 9 | catkin 10 | catkin_simple 11 | 12 | roscpp 13 | dvs_msgs 14 | eigen_catkin 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /corner_event_detector/src/detector.cpp: -------------------------------------------------------------------------------- 1 | #include "corner_event_detector/detector.h" 2 | 3 | namespace corner_event_detector 4 | { 5 | 6 | Detector::Detector(bool detect) 7 | : total_time_(0.), total_events_(0), total_corners_(0) 8 | { 9 | // interface 10 | if (detect) 11 | { 12 | feature_pub_ = nh_.advertise("/feature_events", 1); 13 | event_sub_ = nh_.subscribe("/dvs/events", 0, &Detector::eventCallback, this); 14 | } 15 | } 16 | 17 | Detector::~Detector() 18 | { 19 | // print overall statistics 20 | std::cout << "Statistics for " << detector_name_ << std::endl 21 | << " Total time [ns]: " << total_time_ << std::endl 22 | << " Total number of events: " << total_events_ << std::endl 23 | << " Total number of corners: " << total_corners_ << std::endl 24 | << " Time/event [ns]: " << total_time_/(double) total_events_ << std::endl 25 | << " Events/s: " << total_events_/total_time_*1e9 << std::endl 26 | << " Reduction (%): " << (1.-total_corners_/(double)total_events_)*100 27 | << std::endl; 28 | } 29 | 30 | void Detector::eventCallback(const dvs_msgs::EventArray::ConstPtr &msg) 31 | { 32 | dvs_msgs::EventArray feature_msg; 33 | feature_msg.header = msg->header; 34 | feature_msg.width = msg->width; 35 | feature_msg.height = msg->height; 36 | 37 | utils::time::Timer timer; 38 | for (const auto e : msg->events) 39 | { 40 | if (isFeature(e)) 41 | { 42 | feature_msg.events.push_back(e); 43 | } 44 | } 45 | const auto elapsed_time_nsecs = timer.toc(); 46 | 47 | // global stats 48 | total_time_ += elapsed_time_nsecs; 49 | total_events_ += msg->events.size(); 50 | total_corners_ += feature_msg.events.size(); 51 | 52 | // publish feature events 53 | feature_pub_.publish(feature_msg); 54 | 55 | // stats 56 | const int num_events = msg->events.size(); 57 | if (num_events > 0) 58 | { 59 | const int num_features = feature_msg.events.size(); 60 | const float reduction_rate = 100.*(1.-num_features/(float) num_events); 61 | const float reduction_factor = num_events/(float) num_features; 62 | const float events_per_second = float(num_events)/(elapsed_time_nsecs/1e9); 63 | const float ns_per_event = elapsed_time_nsecs/float(num_events); 64 | ROS_INFO("%s reduction rate: %.3f%% (%.0fx). Speed: %.0f e/s / %.0f ns/e.", 65 | detector_name_.c_str(), reduction_rate, reduction_factor, 66 | events_per_second, ns_per_event); 67 | } 68 | else 69 | { 70 | ROS_INFO("%s reduction rate: No events.", detector_name_.c_str()); 71 | } 72 | } 73 | 74 | } // namespace 75 | -------------------------------------------------------------------------------- /corner_event_detector/src/distinct_queue.cpp: -------------------------------------------------------------------------------- 1 | #include "corner_event_detector/distinct_queue.h" 2 | 3 | namespace corner_event_detector 4 | { 5 | 6 | DistinctQueue::DistinctQueue(int window_size, int queue_size, bool use_polarity) : 7 | LocalEventQueues(window_size, queue_size) 8 | { 9 | // create queues 10 | const int polarities = use_polarity ? 2 : 1; 11 | const int num_queues = sensor_width_*sensor_height_ * polarities; 12 | 13 | queues_ = std::vector 14 | (num_queues, FixedDistinctQueue(2*window_size+1, queue_size)); 15 | } 16 | 17 | DistinctQueue::~DistinctQueue() 18 | { 19 | } 20 | 21 | bool DistinctQueue::isFull(int x, int y, bool pol) const 22 | { 23 | return queues_[getIndex(x, y, pol)].isFull(); 24 | } 25 | 26 | void DistinctQueue::newEvent(int x, int y, bool pol) 27 | { 28 | // update neighboring pixels 29 | for (int dx=-window_size_; dx<=window_size_; dx++) 30 | { 31 | for (int dy=-window_size_; dy<=window_size_; dy++) 32 | { 33 | // in limits? 34 | if (x+dx<0 or x+dx>=sensor_width_ or y+dy<0 or y+dy>=sensor_height_) 35 | { 36 | continue; 37 | } 38 | 39 | // update pixel's queue 40 | queues_[getIndex(x+dx, y+dy, pol)].addNew(window_size_+dx, 41 | window_size_+dy); 42 | } 43 | } 44 | } 45 | 46 | Eigen::MatrixXi DistinctQueue::getPatch(int x, int y, bool pol) 47 | { 48 | return queues_[getIndex(x, y, pol)].getWindow(); 49 | } 50 | 51 | int DistinctQueue::getIndex(int x, int y, bool polarity) const 52 | { 53 | int polarity_offset = polarity ? sensor_height_*sensor_width_ : 0; 54 | return y*sensor_width_ + x + polarity_offset; 55 | } 56 | 57 | } // namespace 58 | -------------------------------------------------------------------------------- /corner_event_detector/src/fast_detector.cpp: -------------------------------------------------------------------------------- 1 | #include "corner_event_detector/fast_detector.h" 2 | 3 | namespace corner_event_detector 4 | { 5 | 6 | FastDetector::FastDetector(bool connect) 7 | : Detector(connect), 8 | circle3_ {{0, 3}, {1, 3}, {2, 2}, {3, 1}, 9 | {3, 0}, {3, -1}, {2, -2}, {1, -3}, 10 | {0, -3}, {-1, -3}, {-2, -2}, {-3, -1}, 11 | {-3, 0}, {-3, 1}, {-2, 2}, {-1, 3}}, 12 | circle4_ {{0, 4}, {1, 4}, {2, 3}, {3, 2}, 13 | {4, 1}, {4, 0}, {4, -1}, {3, -2}, 14 | {2, -3}, {1, -4}, {0, -4}, {-1, -4}, 15 | {-2, -3}, {-3, -2}, {-4, -1}, {-4, 0}, 16 | {-4, 1}, {-3, 2}, {-2, 3}, {-1, 4}} 17 | { 18 | detector_name_ = "FAST"; 19 | 20 | // allocate SAE matrices 21 | sae_[0] = Eigen::MatrixXd::Zero(sensor_height_, sensor_width_); 22 | sae_[1] = Eigen::MatrixXd::Zero(sensor_height_, sensor_width_); 23 | } 24 | 25 | FastDetector::~FastDetector() 26 | { 27 | } 28 | 29 | bool FastDetector::isFeature(const dvs_msgs::Event &e) 30 | { 31 | // update SAE 32 | const int pol = e.polarity ? 1 : 0; 33 | sae_[pol](e.x, e.y) = e.ts.toSec(); 34 | 35 | const int max_scale = 1; 36 | 37 | // only check if not too close to border 38 | const int cs = max_scale*4; 39 | if (e.x < cs || e.x >= sensor_width_-cs || 40 | e.y < cs || e.y >= sensor_height_-cs) 41 | { 42 | return false; 43 | } 44 | 45 | bool found_streak = false; 46 | 47 | for (int i=0; i<16; i++) 48 | { 49 | for (int streak_size = 3; streak_size<=6; streak_size++) 50 | { 51 | // check that streak event is larger than neighbor 52 | if (sae_[pol](e.x+circle3_[i][0], e.y+circle3_[i][1]) < sae_[pol](e.x+circle3_[(i-1+16)%16][0], e.y+circle3_[(i-1+16)%16][1])) 53 | continue; 54 | 55 | // check that streak event is larger than neighbor 56 | if (sae_[pol](e.x+circle3_[(i+streak_size-1)%16][0], e.y+circle3_[(i+streak_size-1)%16][1]) < sae_[pol](e.x+circle3_[(i+streak_size)%16][0], e.y+circle3_[(i+streak_size)%16][1])) 57 | continue; 58 | 59 | double min_t = sae_[pol](e.x+circle3_[i][0], e.y+circle3_[i][1]); 60 | for (int j=1; j= min_t) 73 | { 74 | did_break = true; 75 | break; 76 | } 77 | } 78 | 79 | if (!did_break) 80 | { 81 | found_streak = true; 82 | break; 83 | } 84 | 85 | } 86 | if (found_streak) 87 | { 88 | break; 89 | } 90 | } 91 | 92 | if (found_streak) 93 | { 94 | found_streak = false; 95 | for (int i=0; i<20; i++) 96 | { 97 | for (int streak_size = 4; streak_size<=8; streak_size++) 98 | { 99 | // check that first event is larger than neighbor 100 | if (sae_[pol](e.x+circle4_[i][0], e.y+circle4_[i][1]) < sae_[pol](e.x+circle4_[(i-1+20)%20][0], e.y+circle4_[(i-1+20)%20][1])) 101 | continue; 102 | 103 | // check that streak event is larger than neighbor 104 | if (sae_[pol](e.x+circle4_[(i+streak_size-1)%20][0], e.y+circle4_[(i+streak_size-1)%20][1]) < sae_[pol](e.x+circle4_[(i+streak_size)%20][0], e.y+circle4_[(i+streak_size)%20][1])) 105 | continue; 106 | 107 | double min_t = sae_[pol](e.x+circle4_[i][0], e.y+circle4_[i][1]); 108 | for (int j=1; j= min_t) 120 | { 121 | did_break = true; 122 | break; 123 | } 124 | } 125 | 126 | if (!did_break) 127 | { 128 | found_streak = true; 129 | break; 130 | } 131 | } 132 | if (found_streak) 133 | { 134 | break; 135 | } 136 | } 137 | } 138 | 139 | return found_streak; 140 | } 141 | 142 | } // namespace 143 | -------------------------------------------------------------------------------- /corner_event_detector/src/fixed_distinct_queue.cpp: -------------------------------------------------------------------------------- 1 | #include "corner_event_detector/fixed_distinct_queue.h" 2 | 3 | namespace corner_event_detector 4 | { 5 | 6 | FixedDistinctQueue::FixedDistinctQueue(int window, int queue) : 7 | first_(-1), last_(-1), queue_max_(queue) 8 | { 9 | window_ = Eigen::MatrixXi::Constant(window, window, -1); 10 | queue_.reserve(queue_max_); 11 | } 12 | 13 | bool FixedDistinctQueue::isFull() const 14 | { 15 | return (queue_.size() >= queue_max_); 16 | } 17 | 18 | void FixedDistinctQueue::addNew(int x, int y) 19 | { 20 | // queue full? 21 | if (queue_.size() < queue_max_) 22 | { 23 | if (window_(x, y) < 0) 24 | { 25 | // first element? 26 | if (queue_.empty()) 27 | { 28 | first_ = 0; 29 | last_ = 0; 30 | 31 | QueueEvent qe; 32 | qe.prev = -1; 33 | qe.next = -1; 34 | qe.x = x; 35 | qe.y = y; 36 | queue_.push_back(qe); 37 | 38 | window_(x, y) = 0; 39 | } 40 | else 41 | { 42 | // add new element 43 | QueueEvent qe; 44 | qe.prev = -1; 45 | qe.next = first_; 46 | qe.x = x; 47 | qe.y = y; 48 | queue_.push_back(qe); 49 | 50 | const int place = queue_.size() - 1; 51 | queue_[first_].prev = place; 52 | first_ = place; 53 | 54 | window_(x, y) = place; 55 | } 56 | } 57 | else 58 | { 59 | // link neighbors of old event in queue 60 | const int place = window_(x, y); 61 | 62 | if (queue_[place].next >= 0 && queue_[place].prev >= 0) 63 | { 64 | queue_[queue_[place].prev].next = queue_[place].next; 65 | queue_[queue_[place].next].prev = queue_[place].prev; 66 | } 67 | 68 | // relink first and last 69 | if (place == last_) 70 | { 71 | if (queue_[place].prev >= 0) 72 | { 73 | last_ = queue_[place].prev; 74 | queue_[queue_[place].prev].next = -1; 75 | } 76 | } 77 | queue_[first_].prev = place; 78 | 79 | queue_[place].prev = -1; 80 | if (first_ != place) 81 | { 82 | queue_[place].next = first_; 83 | } 84 | 85 | first_ = place; 86 | } 87 | } 88 | else 89 | { 90 | // is window empty at location 91 | if (window_(x, y) < 0) 92 | { 93 | // update window 94 | window_(queue_[last_].x, queue_[last_].y) = -1; 95 | window_(x, y) = last_; 96 | 97 | // update queue 98 | queue_[queue_[last_].prev].next = -1; 99 | queue_[last_].x = x; 100 | queue_[last_].y = y; 101 | queue_[last_].next = first_; 102 | const int second_last = queue_[last_].prev; 103 | queue_[last_].prev = -1; 104 | queue_[first_].prev = last_; 105 | first_ = last_; 106 | last_ = second_last; 107 | } 108 | else 109 | { 110 | const int place = window_(x, y); 111 | if (place != first_) 112 | { 113 | // update window 114 | window_(x, y) = place; 115 | 116 | // update queue 117 | if (queue_[place].prev != -1) 118 | { 119 | queue_[queue_[place].prev].next = queue_[place].next; 120 | } 121 | if (queue_[place].next != -1) 122 | { 123 | queue_[queue_[place].next].prev = queue_[place].prev; 124 | } 125 | 126 | if (place == last_) 127 | { 128 | last_ = queue_[last_].prev; 129 | } 130 | 131 | queue_[place].prev = -1; 132 | queue_[place].next = first_; 133 | queue_[first_].prev = place; 134 | 135 | first_ = place; 136 | } 137 | } 138 | } 139 | } 140 | 141 | Eigen::MatrixXi FixedDistinctQueue::getWindow() const 142 | { 143 | Eigen::MatrixXi patch = window_; 144 | for (int x = 0; xnewEvent(e.x, e.y, e.polarity); 53 | 54 | // check if queue is full 55 | double score = harris_threshold_ - 10.; 56 | if (queues_->isFull(e.x, e.y, e.polarity)) 57 | { 58 | // check if current event is a feature 59 | score = getHarrisScore(e.x, e.y, e.polarity); 60 | 61 | last_score_ = score; 62 | } 63 | 64 | return (score > harris_threshold_); 65 | } 66 | 67 | double HarrisDetector::getHarrisScore(int img_x, int img_y, bool polarity) 68 | { 69 | // do not consider border 70 | if (img_xsensor_width_-window_size_ or 71 | img_ysensor_height_-window_size_) 72 | { 73 | // something below the threshold 74 | return harris_threshold_ - 10.; 75 | } 76 | 77 | const Eigen::MatrixXi local_frame = queues_->getPatch(img_x, img_y, polarity); 78 | 79 | const int l = 2*window_size_+2-kernel_size_; 80 | Eigen::MatrixXd dx = Eigen::MatrixXd::Zero(l, l); 81 | Eigen::MatrixXd dy = Eigen::MatrixXd::Zero(l, l); 82 | // Eigen::MatrixXd dxy = Eigen::MatrixXd::Zero(l, l); 83 | for (int x=0; x 1) 118 | { 119 | return n * factorial(n - 1); 120 | } 121 | else 122 | { 123 | return 1; 124 | } 125 | } 126 | 127 | int HarrisDetector::pasc(int k, int n) const 128 | { 129 | if (k>=0 && k<=n) 130 | { 131 | return factorial(n)/(factorial(n-k)*factorial(k)); 132 | } 133 | else 134 | { 135 | return 0; 136 | } 137 | } 138 | 139 | } // namespace 140 | -------------------------------------------------------------------------------- /corner_event_detector/src/node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "corner_event_detector/detector.h" 4 | #include "corner_event_detector/harris_detector.h" 5 | #include "corner_event_detector/fast_detector.h" 6 | 7 | int main(int argc, char* argv[]) 8 | { 9 | ros::init(argc, argv, "corner_event_detector"); 10 | 11 | // load parameter 12 | std::string feature_type; 13 | ros::param::param("~feature_type", feature_type, "harris"); 14 | 15 | // create feature detecotr 16 | corner_event_detector::Detector* detector; 17 | if (feature_type == "harris") 18 | { 19 | ROS_INFO("Using Harris detector."); 20 | detector = new corner_event_detector::HarrisDetector; 21 | } 22 | else if (feature_type == "fast") 23 | { 24 | ROS_INFO("Using fast detector."); 25 | detector = new corner_event_detector::FastDetector; 26 | } 27 | else 28 | { 29 | ROS_ERROR("Feature type '%s' is unknown.", feature_type.c_str()); 30 | return 1; 31 | } 32 | 33 | // run 34 | ros::spin(); 35 | 36 | delete detector; 37 | 38 | return 0; 39 | } 40 | --------------------------------------------------------------------------------