diff --git a/CMakeLists.txt b/CMakeLists.txt index 124b8c9e0..3987f6a9e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,8 @@ endif() set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(CMAKE_C_EXTENSIONS OFF) set(CMAKE_CXX_EXTENSIONS OFF) +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD_REQUIRED ON) option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON) set(CONTEXT_DIR event-driven) set(EVENTDRIVEN_LIBRARY event-driven) @@ -68,6 +70,13 @@ if(prophesee_core_FOUND) message(STATUS "Found prophesee_core: (found version ${prophesee_core_VERSION})") endif() +find_package(libcaer) + +find_package(dv-processing QUIET) +if(dv-processing_FOUND) + message(STATUS "Found dv-processing: (found version ${dv-processing_VERSION})") +endif() + #build the library diff --git a/Dockerfile_Ubuntu2004 b/Dockerfile_Ubuntu2004 index 99aec6100..96ec37974 100644 --- a/Dockerfile_Ubuntu2004 +++ b/Dockerfile_Ubuntu2004 @@ -5,7 +5,7 @@ ENV DEBIAN_FRONTEND=noninteractive ARG CODE_DIR=/usr/local/src -#basic environment +# Basic environment RUN apt update && apt install -y \ ca-certificates \ build-essential \ @@ -47,6 +47,26 @@ RUN apt install -y \ libopencv-dev \ metavision-sdk +# dv-processing +# Add toolchain PPA and install gcc-13/g++-13 +RUN apt update && \ + apt install -y software-properties-common && \ + add-apt-repository ppa:ubuntu-toolchain-r/test && \ + apt update && \ + apt install -y gcc-13 g++-13 + +# Add inivation PPA and install dv-processing dependencies +RUN add-apt-repository ppa:inivation-ppa/inivation && \ + apt-get update && \ + apt-get install -y \ + boost-inivation \ + libcaer-dev \ + libfmt-dev \ + liblz4-dev \ + libzstd-dev \ + libssl-dev && \ + apt-get -y install dv-processing + # YCM ARG YCM_VERSION=v0.15.2 RUN cd $CODE_DIR && \ @@ -68,7 +88,6 @@ RUN cd $CODE_DIR && \ EXPOSE 10000/tcp 10000/udp RUN yarp check - # event-driven ARG ED_VERSION=main RUN cd $CODE_DIR &&\ diff --git a/README.md b/README.md index 3df430593..c43ea2d75 100644 --- a/README.md +++ b/README.md @@ -41,6 +41,7 @@ Event-driven libraries provide basic functionality for handling events in a YARP * [**calibration**](https://github.com/robotology/event-driven/tree/ev2-dev/cpp_tools/calibration) - estimating the camera intrinsic parameters * [**vPreProcess**](https://github.com/robotology/event-driven/tree/ev2-dev/cpp_tools/vPreProcess) - splitting different event-types into separate event-streams, performing filtering, and simple augmentations (flipping etc.) * [**atis-bridge**](https://github.com/robotology/event-driven/tree/ev2-dev/cpp_tools/atis3-bridge) - bridge between the Prophesee ATIS cameras and YARP + * [**dv-bridge**](https://github.com/robotology/event-driven/tree/ev2-dev/cpp_tools/dv-bridge) - bridge between the Inivation DVXplorer Micro camera and YARP * [**zynqGrabber**](https://github.com/robotology/event-driven/tree/ev2-dev/cpp_tools/zynqGrabber) - bridge between zynq-based FPGA sensor interface and YARP ## Applications diff --git a/cpp_tools/CMakeLists.txt b/cpp_tools/CMakeLists.txt index d981e6e37..dc8b2dc3d 100755 --- a/cpp_tools/CMakeLists.txt +++ b/cpp_tools/CMakeLists.txt @@ -15,3 +15,8 @@ endif() if(prophesee_core_FOUND OR MetavisionSDK_FOUND) add_subdirectory(atis3-bridge) endif() + +if(dv-processing_FOUND) + add_subdirectory(dv-bridge) +endif() + diff --git a/cpp_tools/dv-bridge/CMakeLists.txt b/cpp_tools/dv-bridge/CMakeLists.txt new file mode 100644 index 000000000..d67a58c70 --- /dev/null +++ b/cpp_tools/dv-bridge/CMakeLists.txt @@ -0,0 +1,14 @@ + +project(dv-bridge-sdk) + +add_executable(${PROJECT_NAME} dv-bridge-sdk.cpp) + +target_link_libraries(${PROJECT_NAME} PRIVATE YARP::YARP_OS + YARP::YARP_init + ev::event-driven + dv::processing + dl) + +target_compile_definitions(${PROJECT_NAME} PRIVATE $<$:ENABLE_TS=1>) + +install(TARGETS ${PROJECT_NAME} DESTINATION ${CMAKE_INSTALL_BINDIR}) diff --git a/cpp_tools/dv-bridge/README.MD b/cpp_tools/dv-bridge/README.MD new file mode 100644 index 000000000..0e7caf42f --- /dev/null +++ b/cpp_tools/dv-bridge/README.MD @@ -0,0 +1,7 @@ +# dv-bridge + +A bridge application between Inivation's DVXplorer Micro camera SDK and YARP + +### Usage + +`dv-bridge-sdk` diff --git a/cpp_tools/dv-bridge/dv-bridge-sdk.cpp b/cpp_tools/dv-bridge/dv-bridge-sdk.cpp new file mode 100644 index 000000000..53e9649d2 --- /dev/null +++ b/cpp_tools/dv-bridge/dv-bridge-sdk.cpp @@ -0,0 +1,294 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "event-driven/core.h" +#include "event-driven/vis.h" + +using namespace ev; +using namespace yarp::os; + + +class dvbridge : public RFModule, public Thread { + +private: + + yarp::os::Port output_port; + Stamp yarpstamp; + + dv::io::camera::DVXplorerM cam{}; // create and open the DVXplorer Micro camera + std::optional resolution = cam.getEventResolution(); + bool bgaFilter{false}; + int bgaDuration{2000}; + + + int counterPackets{0}; + int counterEvents{0}; + static constexpr double period{0.2}; + bool recordMode{false}; + + ev::vNoiseFilter nf; + + std::mutex m; + std::condition_variable signal; + std::vector< ev::packet > buffer; + int bufferSize{0}; + int bufferUsed{0}; + int bSel{0}; + double limit{-1}; + static constexpr void switch_buffer(int &buf_i) {buf_i = (buf_i + 1) % 2;}; + double clock_time{0}; + +public: + + bool configure(yarp::os::ResourceFinder& rf) override + { + + if(rf.check("h") || rf.check("help")) { + + yInfo() << "Bridge to push Inivation DVXplorer Micro camera events to YARP."; + yInfo() << "Usage: dvbridge [options]"; + yInfo() << "Options:"; + yInfo() << "\t--threshold \t: (optional) Set threshold intensity variation for event output (default: 9, range 0-17)"; + yInfo() << "\t--bgafilter \t: (optional) Enable background activity filter with duration in microseconds (default: 2000)"; + yInfo() << "\t--filter \t: (optional) Enable event-driven temporal noise filter (seconds per event per pixel)"; + yInfo() << "\t--limit \t: (optional) Maximum events per second in buffer (in millions, default: unlimited)"; + yInfo() << "\t--name \t: (optional) Module name used to name the YARP ports (default: /dv)"; + yInfo() << "\t--help \t\t: Show this help message and exit"; + yInfo() << ""; + yInfo() << "Example:"; + yInfo() << "\tdv-bridge-sdk --threshold 10 --bgafilter 3000 --filter 0.01 --limit 2 --name /mydv"; + return false; + } + + if(!yarp::os::Network::checkNetwork(2.0)) { + std::cout << "Could not connect to YARP" << std::endl; + return false; + } + + + limit = rf.check("limit", Value(-1)).asFloat64(); + if(limit < 0) limit = DBL_MAX; + else limit *= 1e6; + + buffer.emplace_back(); + buffer.emplace_back(); + + if (rf.check("threshold")) { + int threshold = rf.check("threshold", Value(9)).asInt8(); + cam.setContrastThresholdOn(threshold); + cam.setContrastThresholdOff(threshold); + yInfo() << "[THRESHOLD] The DVXplorer Micro threshold is set to"<< threshold; + } + + if (rf.check("bgafilter")) { + bgaFilter = true; + bgaDuration = rf.check("bgafilter", Value(2000)).asInt16(); + yInfo() << "[BGA_FILTER] The DVXplorer background activity filter duration is"<< bgaDuration; + } + + double nf_param = 0.0; + if(rf.check("filter")) nf_param = rf.find("filter").asFloat64(); + if(rf.check("f")) nf_param = rf.find("f").asFloat64(); + if(nf_param > 0.0) + { + yInfo() << "[FILTER] ON. Maximum 1 event per pixel per" << nf_param << "seconds"; + nf.initialise(resolution->width, resolution->height); + nf.use_temporal_filter(nf_param); + } + + //set the module name used to name ports + setName((rf.check("name", Value("/dv")).asString()).c_str()); + + + //automatically assign port numbers + std::stringstream ss; + ss.str(""); ss << getName() << "/AE:o"; + if(yarp::os::Network::exists(ss.str())) { + int port_number = 1; + do { + port_number++; + ss.str(""); ss << getName() << "-" << port_number << "/AE:o"; + } while(yarp::os::Network::exists(ss.str())); + } + + if(!output_port.open(ss.str())) { + yError() << "Could not open output port"; + return false; + } + + if(!cam.isRunning()) { + yError() << "Could not start the camera"; + return false; + } + + clock_time = yarp::os::Time::now(); + + return Thread::start(); + } + + double getPeriod() override + { + return period; //period of synchronous thread + } + + bool interruptModule() override + { + //if the module is asked to stop ask the asynchronous thread to stop + return Thread::stop(); + } + + void onStop() override + { + output_port.close(); + signal.notify_one(); + } + + //synchronous thread + bool updateModule() override + { + //perform synchronisation between CPU and camera clock + double oos = yarp::os::Time::now() - clock_time; + static int i = 0; + if(i++ < 5) { + clock_time += oos*0.5; + } else { + clock_time += oos*0.005; + } + + //output some nice info + if(i % (int)(1/period) == 0) { + yInfo() << counterPackets << "packets and" + << (counterEvents * 0.001) << "k events sent per second"; + counterPackets = counterEvents = 0; + } + + if(!cam.isRunning()) + Thread::stop(); + + return Thread::isRunning(); + } + + void fill_buffer(const auto &events) + { + std::unique_lock lk(m); + //fill up the buffer that will be sent over the port in the other thread + AE ae; + if (nf.active()) { + for (const auto& ev: events) { + if(nf.check(ev.x(), ev.y(), ev.polarity(), ev.timestamp() * 0.000001)) { + ae.x = ev.x(); ae.y = ev.y(); ae.p = ev.polarity(); + buffer[bSel].push_back(ae); + } + } + } else { + for (const auto& ev: events) { + ae.x = ev.x(); ae.y = ev.y(); ae.p = ev.polarity(); + buffer[bSel].push_back(ae); + } + } + dv::Duration dur = events.duration(); + buffer[bSel].duration(static_cast(dur.count())*1e-6 + buffer[bSel].duration()); + lk.unlock(); + signal.notify_one(); + + } + + + + //asynchronous thread run forever + void run() override + { + const dv::Duration backgroundActivityDuration = dv::Duration(bgaDuration); + dv::noise::BackgroundActivityNoiseFilter<> BGAFilter(resolution.value(),backgroundActivityDuration); + while(cam.isRunning()) { + // wait for data and then switch buffers so the callback can keep + // filling the second buffer while we are sending + + if (auto events = cam.getNextEventBatch(); events.has_value()) { + if (bgaFilter){ + BGAFilter.accept(*events); + dv::EventStore BGAFiltered = BGAFilter.generateEvents(); + if (BGAFiltered.isEmpty()) { + continue; // skip empty batches + } + fill_buffer(BGAFiltered); + } + else { + fill_buffer(*events); + } + ev::packet ¤t_buffer = buffer[bSel]; + std::unique_lock lk(m); + signal.wait(lk, [this] { return buffer[bSel].size() > 0 || Thread::isStopping(); }); + switch_buffer(bSel); + buffer[bSel].duration(0.0); + lk.unlock(); + + if(Thread::isStopping()) break; + + // send the data in the first buffer + clock_time += current_buffer.duration(); + yarpstamp.update(clock_time); + if(current_buffer.size() / current_buffer.duration() < limit) + { + output_port.setEnvelope(yarpstamp); + output_port.write(current_buffer); + } + counterPackets++; + counterEvents += current_buffer.size(); + current_buffer.clear(); + } + } + } + + void make_plot(double seconds) + { + static const int NX = 40; + static const int NY = 50; + static const int xx = 10; + static const int yx = 1; + + static cv::Mat img(2*NY*yx, NX*xx, CV_8U); + + static std::list buffer; + double ms = seconds*1000; + if(ms > NY) ms = NY; if(ms < -NY) ms = -NY; + buffer.push_back((int)((NY - ms)*yx)); + while(buffer.size() > NX) buffer.pop_front(); + + img.setTo(0); int i = 1; + auto p1 = buffer.begin(); auto p2 = buffer.begin(); p2++; + for(; p2 != buffer.end(); p1++, p2++, i++) { + cv::line(img, {xx*(i-1), *p1}, {xx*i, *p2}, 255, 1); + } + + cv::line(img, {0, NY*yx}, {NX*xx, NY*yx}, 128); + std::stringstream ss; + ss.str(""); ss << NY << "ms"; + cv::putText(img, ss.str(), {2, 20}, cv::FONT_HERSHEY_PLAIN, 1.0, 255); + ss.str(""); ss << -NY << "ms"; + cv::putText(img, ss.str(), {2, img.rows - 2}, cv::FONT_HERSHEY_PLAIN, 1.0, 255); + + cv::imshow("clock synch", img); + cv::waitKey(10); + + } +}; + +int main(int argc, char * argv[]) +{ + + /* prepare and configure the resource finder */ + yarp::os::ResourceFinder rf; + rf.configure( argc, argv ); + + /* create the module */ + dvbridge instance; + return instance.runModule(rf); +} \ No newline at end of file