From 02e381d9f9b13a6c622265546cd6a73577eb1962 Mon Sep 17 00:00:00 2001 From: Philip Keller Date: Thu, 28 Dec 2017 22:24:56 +0100 Subject: [PATCH 1/6] implemented rpg davis simulation for dvs, event timestamps calculated assuming linear changes --- include/gazebo_dvs_plugin/dvs_plugin.hpp | 3 + src/dvs_plugin.cpp | 103 ++++++++++++++++++----- 2 files changed, 86 insertions(+), 20 deletions(-) diff --git a/include/gazebo_dvs_plugin/dvs_plugin.hpp b/include/gazebo_dvs_plugin/dvs_plugin.hpp index 9f8e9d2..0a36cb9 100644 --- a/include/gazebo_dvs_plugin/dvs_plugin.hpp +++ b/include/gazebo_dvs_plugin/dvs_plugin.hpp @@ -79,8 +79,11 @@ namespace gazebo protected: string namespace_; private: Mat last_image; + private: Mat ref_image; + private: ros::Time received_last; private: bool has_last_image; private: float event_threshold; + private: void updateDVS(ros::Time received_current, Mat *curr_image); private: void processDelta(Mat *last_image, Mat *curr_image); private: void fillEvents(Mat *diff, int polarity, vector *events); private: void publishEvents(vector *events); diff --git a/src/dvs_plugin.cpp b/src/dvs_plugin.cpp index f0eff1e..7c7f7a5 100644 --- a/src/dvs_plugin.cpp +++ b/src/dvs_plugin.cpp @@ -155,33 +155,22 @@ namespace gazebo unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format) { + ros::Time received_current = ros::Time::now(); + #if GAZEBO_MAJOR_VERSION >= 7 _image = this->camera->ImageData(0); #else _image = this->camera->GetImageData(0); #endif - /* -#if GAZEBO_MAJOR_VERSION >= 7 -float rate = this->camera->RenderRate(); -#else -float rate = this->camera->GetRenderRate(); -#endif -if (!isfinite(rate)) -rate = 30.0; -float dt = 1.0 / rate; - */ - // convert given frame to opencv image cv::Mat input_image(_height, _width, CV_8UC3); + cv::Mat curr_image(_height, _width, CV_8UC1); + input_image.data = (uchar*)_image; - // color to grayscale - cv::Mat curr_image_rgb(_height, _width, CV_8UC3); - cvtColor(input_image, curr_image_rgb, CV_RGB2BGR); - cvtColor(curr_image_rgb, input_image, CV_BGR2GRAY); - - cv::Mat curr_image = input_image; + cvtColor(input_image, input_image, CV_RGB2BGR); + cvtColor(input_image, curr_image, CV_BGR2GRAY); /* TODO any encoding configuration should be supported try { @@ -198,11 +187,13 @@ float dt = 1.0 / rate; assert(_height == height && _width == width); if (this->has_last_image) { - this->processDelta(&this->last_image, &curr_image); + this->updateDVS(received_current, &curr_image); } else if (curr_image.size().area() > 0) { this->last_image = curr_image; + curr_image.convertTo(this->ref_image, CV_32FC1); + this->received_last = received_current; this->has_last_image = true; } else @@ -210,6 +201,78 @@ float dt = 1.0 / rate; gzwarn << "Ignoring empty image." << endl; } } + + void DvsPlugin::updateDVS(ros::Time received_current, cv::Mat *curr_image) + { + if (curr_image->size() == this->last_image.size()) + { + ros::Duration delta_t = received_current - this->received_last; + + std::vector events; + + for(int y = 0; y < curr_image->rows; y++) + { + for(int x = 0; x < curr_image->cols; x++) + { + int it = (int)this->last_image.at(y,x); // TODO check if pointer method faster than at?? + int it_dt = (int)curr_image->at(y,x); + float ref = this->ref_image.at(y,x); + int polarity = (it_dt>=it) ? (1) : (-1); + + if(fabs(it_dt-it) > 1e-6) // TODO are these float values in rpg davis version?? + { + + std::vector list_crossings; + bool all_crossings_found = false; + float current_crossing = ref; + + while(!all_crossings_found) // TODO make it directly, so that list_crossings is unnecessary! + { + current_crossing = current_crossing + polarity*(this->event_threshold); + if(polarity > 0) + { + if(current_crossing > it && current_crossing <= it_dt) + list_crossings.push_back(current_crossing); + else + all_crossings_found = true; + } + else + { + if(current_crossing < it && current_crossing >= it_dt) + list_crossings.push_back(current_crossing); + else + all_crossings_found = true; + } + } + + for(int i = 0; ireceived_last + delta_t * ((list_crossings[i]-1.0*it) / (1.0*(it_dt-it))); + + dvs_msgs::Event event_i; + event_i.x = x; + event_i.y = y; + event_i.ts = event_ts; + event_i.polarity = (polarity>0); + + events.push_back(event_i); + } + + if(list_crossings.size() > 0) + this->ref_image.at(y,x) = list_crossings.back(); + } + } + } + std::cout << " Events: " << events.size() << std::endl; + + this->publishEvents(&events); + curr_image->copyTo(this->last_image); + } + else + { + gzwarn << "Unexpected change in image size (" << this->last_image.size() << " -> " << curr_image->size() << "). Publishing no events for this frame change." << endl; + } + } void DvsPlugin::processDelta(cv::Mat *last_image, cv::Mat *curr_image) { @@ -226,9 +289,9 @@ float dt = 1.0 / rate; *last_image += pos_mask & pos_diff; *last_image -= neg_mask & neg_diff; - + std::vector events; - + this->fillEvents(&pos_mask, 0, &events); this->fillEvents(&neg_mask, 1, &events); From 5e5a9cefd592ba27c6b7e56ebcb07cd9afde5592 Mon Sep 17 00:00:00 2001 From: Philip Keller Date: Sat, 30 Dec 2017 21:23:18 +0100 Subject: [PATCH 2/6] adjusted to bgr image format, received_last update bug included --- src/dvs_plugin.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/dvs_plugin.cpp b/src/dvs_plugin.cpp index 7c7f7a5..f11270f 100644 --- a/src/dvs_plugin.cpp +++ b/src/dvs_plugin.cpp @@ -155,8 +155,8 @@ namespace gazebo unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format) { + //std::cout << "time: " << ros::Time::now() - thi received_current << std::endl; ros::Time received_current = ros::Time::now(); - #if GAZEBO_MAJOR_VERSION >= 7 _image = this->camera->ImageData(0); #else @@ -168,8 +168,9 @@ namespace gazebo cv::Mat curr_image(_height, _width, CV_8UC1); input_image.data = (uchar*)_image; + //curr_image.data = (uchar*)_image; - cvtColor(input_image, input_image, CV_RGB2BGR); + //cvtColor(input_image, input_image, CV_RGB2BGR); cvtColor(input_image, curr_image, CV_BGR2GRAY); /* TODO any encoding configuration should be supported @@ -188,6 +189,7 @@ namespace gazebo if (this->has_last_image) { this->updateDVS(received_current, &curr_image); + this->received_last = received_current; } else if (curr_image.size().area() > 0) { @@ -263,7 +265,7 @@ namespace gazebo } } } - std::cout << " Events: " << events.size() << std::endl; + //std::cout << " Events: " << events.size() << std::endl; this->publishEvents(&events); curr_image->copyTo(this->last_image); From fc07df36eac08b93ee2d4fb1a8e6c8cd27177264 Mon Sep 17 00:00:00 2001 From: Philip Keller Date: Sat, 30 Dec 2017 23:01:59 +0100 Subject: [PATCH 3/6] maybe a faster implementation of detecting events without a crossings-list --- include/gazebo_dvs_plugin/dvs_plugin.hpp | 1 + src/dvs_plugin.cpp | 76 +++++++++++------------- 2 files changed, 36 insertions(+), 41 deletions(-) diff --git a/include/gazebo_dvs_plugin/dvs_plugin.hpp b/include/gazebo_dvs_plugin/dvs_plugin.hpp index 0a36cb9..9010164 100644 --- a/include/gazebo_dvs_plugin/dvs_plugin.hpp +++ b/include/gazebo_dvs_plugin/dvs_plugin.hpp @@ -85,6 +85,7 @@ namespace gazebo private: float event_threshold; private: void updateDVS(ros::Time received_current, Mat *curr_image); private: void processDelta(Mat *last_image, Mat *curr_image); + private: void appendEvent(std::vector *events, int x, int y, ros::Time ts, bool polarity); private: void fillEvents(Mat *diff, int polarity, vector *events); private: void publishEvents(vector *events); }; diff --git a/src/dvs_plugin.cpp b/src/dvs_plugin.cpp index f11270f..80dd1c6 100644 --- a/src/dvs_plugin.cpp +++ b/src/dvs_plugin.cpp @@ -155,8 +155,8 @@ namespace gazebo unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format) { - //std::cout << "time: " << ros::Time::now() - thi received_current << std::endl; ros::Time received_current = ros::Time::now(); + //std::cout << "time: " << received_current - this->received_last << std::endl; #if GAZEBO_MAJOR_VERSION >= 7 _image = this->camera->ImageData(0); #else @@ -204,6 +204,16 @@ namespace gazebo } } + void DvsPlugin::appendEvent(std::vector *events, int x, int y, ros::Time ts, bool polarity) + { + dvs_msgs::Event event_i; + event_i.x = x; + event_i.y = y; + event_i.ts = ts; + event_i.polarity = polarity; + events->push_back(event_i); + } + void DvsPlugin::updateDVS(ros::Time received_current, cv::Mat *curr_image) { if (curr_image->size() == this->last_image.size()) @@ -218,57 +228,41 @@ namespace gazebo { int it = (int)this->last_image.at(y,x); // TODO check if pointer method faster than at?? int it_dt = (int)curr_image->at(y,x); + int it_diff = it_dt-it; float ref = this->ref_image.at(y,x); - int polarity = (it_dt>=it) ? (1) : (-1); - if(fabs(it_dt-it) > 1e-6) // TODO are these float values in rpg davis version?? + if(it_diff > 1e-6) // ON-Events { - - std::vector list_crossings; - bool all_crossings_found = false; - float current_crossing = ref; - - while(!all_crossings_found) // TODO make it directly, so that list_crossings is unnecessary! + float current_crossing = ref + (this->event_threshold); + for(; current_crossing <= it_dt; current_crossing += (this->event_threshold)) { - current_crossing = current_crossing + polarity*(this->event_threshold); - if(polarity > 0) - { - if(current_crossing > it && current_crossing <= it_dt) - list_crossings.push_back(current_crossing); - else - all_crossings_found = true; - } - else - { - if(current_crossing < it && current_crossing >= it_dt) - list_crossings.push_back(current_crossing); - else - all_crossings_found = true; - } + ros::Time event_ts = this->received_last + delta_t * ((current_crossing-(float)it) / ((float)it_diff)); + appendEvent(&events, x, y, event_ts, true); } - - for(int i = 0; ievent_threshold); + if(current_crossing > ref) + this->ref_image.at(y,x) = current_crossing; + } + else if(it_diff < -1e-6) // OFF-Events + { + float current_crossing = ref - (this->event_threshold); + for(; current_crossing >= it_dt; current_crossing -= (this->event_threshold)) { - ros::Time event_ts = this->received_last + delta_t * ((list_crossings[i]-1.0*it) / (1.0*(it_dt-it))); - - dvs_msgs::Event event_i; - event_i.x = x; - event_i.y = y; - event_i.ts = event_ts; - event_i.polarity = (polarity>0); - - events.push_back(event_i); + ros::Time event_ts = this->received_last + delta_t * ((current_crossing-(float)it) / ((float)it_diff)); + appendEvent(&events, x, y, event_ts, false); } - - if(list_crossings.size() > 0) - this->ref_image.at(y,x) = list_crossings.back(); + current_crossing += (this->event_threshold); + if(current_crossing < ref) + this->ref_image.at(y,x) = current_crossing; } + //else + //std::cout << "Pixel values numerically probably the same." << std::endl; } } - //std::cout << " Events: " << events.size() << std::endl; - + this->publishEvents(&events); - curr_image->copyTo(this->last_image); + //curr_image->copyTo(this->last_image); // necessary ?? just adjust pointer?? + this->last_image = *curr_image; } else { From 41b4497c7974945bd724a3022924a3afff687316 Mon Sep 17 00:00:00 2001 From: Philip Keller Date: Mon, 15 Jan 2018 16:36:36 +0100 Subject: [PATCH 4/6] davis simulation method --- src/dvs_plugin.cpp | 76 +++++++++++++++++++++++----------------------- 1 file changed, 38 insertions(+), 38 deletions(-) diff --git a/src/dvs_plugin.cpp b/src/dvs_plugin.cpp index 80dd1c6..3024ea9 100644 --- a/src/dvs_plugin.cpp +++ b/src/dvs_plugin.cpp @@ -166,7 +166,7 @@ namespace gazebo // convert given frame to opencv image cv::Mat input_image(_height, _width, CV_8UC3); cv::Mat curr_image(_height, _width, CV_8UC1); - + input_image.data = (uchar*)_image; //curr_image.data = (uchar*)_image; @@ -203,7 +203,7 @@ namespace gazebo gzwarn << "Ignoring empty image." << endl; } } - + void DvsPlugin::appendEvent(std::vector *events, int x, int y, ros::Time ts, bool polarity) { dvs_msgs::Event event_i; @@ -213,53 +213,53 @@ namespace gazebo event_i.polarity = polarity; events->push_back(event_i); } - + void DvsPlugin::updateDVS(ros::Time received_current, cv::Mat *curr_image) { if (curr_image->size() == this->last_image.size()) { ros::Duration delta_t = received_current - this->received_last; - + std::vector events; for(int y = 0; y < curr_image->rows; y++) { - for(int x = 0; x < curr_image->cols; x++) - { + for(int x = 0; x < curr_image->cols; x++) + { int it = (int)this->last_image.at(y,x); // TODO check if pointer method faster than at?? int it_dt = (int)curr_image->at(y,x); - int it_diff = it_dt-it; + int it_diff = it_dt-it; float ref = this->ref_image.at(y,x); - - if(it_diff > 1e-6) // ON-Events - { - float current_crossing = ref + (this->event_threshold); - for(; current_crossing <= it_dt; current_crossing += (this->event_threshold)) - { - ros::Time event_ts = this->received_last + delta_t * ((current_crossing-(float)it) / ((float)it_diff)); - appendEvent(&events, x, y, event_ts, true); - } - current_crossing -= (this->event_threshold); - if(current_crossing > ref) - this->ref_image.at(y,x) = current_crossing; - } - else if(it_diff < -1e-6) // OFF-Events - { - float current_crossing = ref - (this->event_threshold); - for(; current_crossing >= it_dt; current_crossing -= (this->event_threshold)) - { - ros::Time event_ts = this->received_last + delta_t * ((current_crossing-(float)it) / ((float)it_diff)); - appendEvent(&events, x, y, event_ts, false); - } - current_crossing += (this->event_threshold); - if(current_crossing < ref) - this->ref_image.at(y,x) = current_crossing; - } - //else - //std::cout << "Pixel values numerically probably the same." << std::endl; - } + + if(it_diff > 1e-6) // ON-Events + { + float current_crossing = ref + (this->event_threshold); + for(; current_crossing <= it_dt; current_crossing += (this->event_threshold)) + { + ros::Time event_ts = this->received_last + delta_t * ((current_crossing-(float)it) / ((float)it_diff)); + appendEvent(&events, x, y, event_ts, true); + } + current_crossing -= (this->event_threshold); + if(current_crossing > ref) + this->ref_image.at(y,x) = current_crossing; + } + else if(it_diff < -1e-6) // OFF-Events + { + float current_crossing = ref - (this->event_threshold); + for(; current_crossing >= it_dt; current_crossing -= (this->event_threshold)) + { + ros::Time event_ts = this->received_last + delta_t * ((current_crossing-(float)it) / ((float)it_diff)); + appendEvent(&events, x, y, event_ts, false); + } + current_crossing += (this->event_threshold); + if(current_crossing < ref) + this->ref_image.at(y,x) = current_crossing; + } + //else + //std::cout << "Pixel values numerically probably the same." << std::endl; + } } - + this->publishEvents(&events); //curr_image->copyTo(this->last_image); // necessary ?? just adjust pointer?? this->last_image = *curr_image; @@ -285,9 +285,9 @@ namespace gazebo *last_image += pos_mask & pos_diff; *last_image -= neg_mask & neg_diff; - + std::vector events; - + this->fillEvents(&pos_mask, 0, &events); this->fillEvents(&neg_mask, 1, &events); From 82cf774d799024062d7641bb85eac240c87f1ddf Mon Sep 17 00:00:00 2001 From: Philip Keller Date: Mon, 15 Jan 2018 17:23:03 +0100 Subject: [PATCH 5/6] timing performance check --- src/dvs_plugin.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/dvs_plugin.cpp b/src/dvs_plugin.cpp index 3024ea9..24ceff2 100644 --- a/src/dvs_plugin.cpp +++ b/src/dvs_plugin.cpp @@ -156,7 +156,7 @@ namespace gazebo const std::string &_format) { ros::Time received_current = ros::Time::now(); - //std::cout << "time: " << received_current - this->received_last << std::endl; + std::cout << "time between frames: " << (received_current - this->received_last).toSec()*1000 << std::endl; #if GAZEBO_MAJOR_VERSION >= 7 _image = this->camera->ImageData(0); #else @@ -202,6 +202,8 @@ namespace gazebo { gzwarn << "Ignoring empty image." << endl; } + + std::cout << "Time: " << (ros::Time::now() - received_current).toSec()*1000 << std::endl; } void DvsPlugin::appendEvent(std::vector *events, int x, int y, ros::Time ts, bool polarity) From 462c3aefc9cc654021354ca7a2ef9ff7e4358b90 Mon Sep 17 00:00:00 2001 From: Philip Keller Date: Thu, 24 May 2018 16:31:56 +0200 Subject: [PATCH 6/6] less prints, fill_events functionality --- src/dvs_plugin.cpp | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/src/dvs_plugin.cpp b/src/dvs_plugin.cpp index 24ceff2..a2de78f 100644 --- a/src/dvs_plugin.cpp +++ b/src/dvs_plugin.cpp @@ -156,7 +156,6 @@ namespace gazebo const std::string &_format) { ros::Time received_current = ros::Time::now(); - std::cout << "time between frames: " << (received_current - this->received_last).toSec()*1000 << std::endl; #if GAZEBO_MAJOR_VERSION >= 7 _image = this->camera->ImageData(0); #else @@ -202,8 +201,6 @@ namespace gazebo { gzwarn << "Ignoring empty image." << endl; } - - std::cout << "Time: " << (ros::Time::now() - received_current).toSec()*1000 << std::endl; } void DvsPlugin::appendEvent(std::vector *events, int x, int y, ros::Time ts, bool polarity) @@ -218,6 +215,23 @@ namespace gazebo void DvsPlugin::updateDVS(ros::Time received_current, cv::Mat *curr_image) { + bool fill_events; + ros::param::param("/fill_events", fill_events, false); + if (fill_events) + { + std::vector events; + ros::Time ts = ros::Time::now(); + for(int y = 0; y < curr_image->rows; y++) + { + for(int x = 0; x < curr_image->cols; x++) + { + appendEvent(&events, x, y, ts, true); + } + } + this->publishEvents(&events); + return; + } + if (curr_image->size() == this->last_image.size()) { ros::Duration delta_t = received_current - this->received_last;