Support for trail filters (#50)

* Added support for Metavision SDK trail filters (Andreas Ziegler) --------- Co-authored-by: Bernd Pfrommer <[email protected]>
ros-event-camera · Aug 13, 2024 · f7a4fb6 · f7a4fb6
1 parent 219b696
commit f7a4fb6
Show file tree

Hide file tree

Showing 6 changed files with 79 additions and 7 deletions.
diff --git a/README.md b/README.md
@@ -169,6 +169,10 @@ Parameters:
 - ``erc_rate``: event rate control rate (Gen4 sensor) events/sec. Default: 100000000.
 - ``mipi_frame_period``:: mipi frame period in usec. Only available on some sensors.
     Tune this to get faster callback rates from the SDK to the ROS driver. For instance 1008 will give a callback every millisecond. Risk of data corruption when set too low! Default: -1 (not set).
+- ``trail_filter``: enable/disable event trail filter. Default: False.
+- ``trail_filter_type``: type of trail filter. Allowed values: ``trail``, ``stc_cut_trail``, ``stc_keep_trail``.
+  Default: ``trail``. See Metavision SDK documentation.
+- ``trail_filter_threshold``: Filter threshold, see MetavisionSDK documentation. Default: 5000.
 - ``sync_mode``: Used to synchronize the time stamps across multiple
   cameras (tested for only 2). The cameras must be connected via a
   sync cable, and two separate ROS driver nodes are started, see

diff --git a/include/metavision_driver/logging.h b/include/metavision_driver/logging.h
@@ -76,13 +76,6 @@
     SS << __VA_ARGS__;                                     \
     throw(std::runtime_error(SS.str()));                   \
   }
-#define BOMB_OUT_CERR(...)                 \
-  {                                        \
-    std::cerr << __VA_ARGS__ << std::endl; \
-    std::stringstream SS;                  \
-    SS << __VA_ARGS__;                     \
-    throw(std::runtime_error(SS.str()));   \
-  }
 #define LOG_INFO_NAMED(...)                                           \
   {                                                                   \
     RCLCPP_INFO_STREAM(rclcpp::get_logger(loggerName_), __VA_ARGS__); \
@@ -175,4 +168,12 @@
 
 #endif  // USING_ROS_1
 
+#define BOMB_OUT_CERR(...)                 \
+  {                                        \
+    std::cerr << __VA_ARGS__ << std::endl; \
+    std::stringstream SS;                  \
+    SS << __VA_ARGS__;                     \
+    throw(std::runtime_error(SS.str()));   \
+  }
+
 #endif  // METAVISION_DRIVER__LOGGING_H_
diff --git a/include/metavision_driver/metavision_wrapper.h b/include/metavision_driver/metavision_wrapper.h
@@ -60,6 +60,13 @@ class MetavisionWrapper
     size_t maxQueueSize{0};
   };
 
+  struct TrailFilter
+  {
+    bool enabled;
+    std::string type;
+    uint32_t threshold;
+  };
+
   typedef std::map<std::string, std::map<std::string, int>> HardwarePinConfig;
 
   explicit MetavisionWrapper(const std::string & loggerName);
@@ -112,6 +119,7 @@ class MetavisionWrapper
     ercRate_ = rate;
   }
   void setMIPIFramePeriod(int usec) { mipiFramePeriod_ = usec; }
+  void setTrailFilter(const std::string & type, const uint32_t threshold, const bool state);
 
   bool triggerActive() const
   {
@@ -139,6 +147,7 @@ class MetavisionWrapper
     const std::string & mode_in, const std::string & mode_out, const int period,
     const double duty_cycle);
   void configureEventRateController(const std::string & mode, const int rate);
+  void activateTrailFilter();
   void configureMIPIFramePeriod(int usec, const std::string & sensorName);
   void printStatistics();
   // ------------ variables
@@ -168,6 +177,7 @@ class MetavisionWrapper
   HardwarePinConfig hardwarePinConfig_;
   std::string ercMode_;
   int ercRate_;
+  TrailFilter trailFilter_;
   int mipiFramePeriod_{-1};
   std::string loggerName_{"driver"};
   std::vector<int> roi_;

diff --git a/launch/driver_node.launch.py b/launch/driver_node.launch.py
@@ -50,6 +50,9 @@ def launch_setup(context, *args, **kwargs):
                 "serial": LaunchConfig("serial"),
                 "erc_mode": "enabled",
                 "erc_rate": 100000000,
+                "trail_filter": False,
+                "trail_filter_type": "stc_trail_cut",
+                "trail_filter_threshold": 5000,
                 # 'roi': [0, 0, 100, 100],
                 # valid: 'external', 'loopback', 'disabled'
                 "trigger_in_mode": "external",

diff --git a/src/driver_ros2.cpp b/src/driver_ros2.cpp
@@ -317,6 +317,19 @@ void DriverROS2::configureWrapper(const std::string & name)
   this->get_parameter_or("sync_mode", syncMode, std::string("standalone"));
   wrapper_->setSyncMode(syncMode);
   LOG_INFO("sync mode: " << syncMode);
+  bool trailFilter;
+  this->get_parameter_or("trail_filter", trailFilter, false);
+  std::string trailFilterType;
+  this->get_parameter_or("trail_filter_type", trailFilterType, std::string("trail"));
+  int trailFilterThreshold;
+  this->get_parameter_or("trail_filter_threshold", trailFilterThreshold, 0);
+  if (trailFilter) {
+    LOG_INFO(
+      "Using tail filter in " << trailFilterType << " mode with threshold "
+                              << trailFilterThreshold);
+    wrapper_->setTrailFilter(
+      trailFilterType, static_cast<uint32_t>(trailFilterThreshold), trailFilter);
+  }
   std::vector<int64_t> roi_long;
   this->get_parameter_or("roi", roi_long, std::vector<int64_t>());
   std::vector<int> r(roi_long.begin(), roi_long.end());

diff --git a/src/metavision_wrapper.cpp b/src/metavision_wrapper.cpp
@@ -15,6 +15,8 @@
 
 #include "metavision_driver/metavision_wrapper.h"
 
+#include "metavision_driver/logging.h"
+
 #ifdef USING_METAVISION_3
 #include <metavision/hal/facilities/i_device_control.h>
 #include <metavision/hal/facilities/i_erc.h>
@@ -23,6 +25,7 @@
 #include <metavision/hal/facilities/i_erc_module.h>
 #endif
 
+#include <metavision/hal/facilities/i_event_trail_filter_module.h>
 #include <metavision/hal/facilities/i_hw_identification.h>
 #include <metavision/hal/facilities/i_hw_register.h>
 #include <metavision/hal/facilities/i_plugin_software_info.h>
@@ -62,6 +65,11 @@ static const std::map<std::string, Metavision::I_TriggerIn::Channel> channelMap
 static const std::map<std::string, uint32_t> sensorToMIPIAddress = {
   {"IMX636", 0xB028}, {"Gen3.1", 0x1508}};
 
+static const std::map<std::string, Metavision::I_EventTrailFilterModule::Type> trailFilterMap = {
+  {"trail", Metavision::I_EventTrailFilterModule::Type::TRAIL},
+  {"stc_cut_trail", Metavision::I_EventTrailFilterModule::Type::STC_CUT_TRAIL},
+  {"stc_keep_trail", Metavision::I_EventTrailFilterModule::Type::STC_KEEP_TRAIL}};
+
 static std::string to_lower(const std::string upper)
 {
   std::string lower(upper);
@@ -119,6 +127,14 @@ int MetavisionWrapper::setBias(const std::string & name, int val)
   return (now);
 }
 
+void MetavisionWrapper::setTrailFilter(
+  const std::string & type, const uint32_t threshold, const bool state)
+{
+  trailFilter_.enabled = state;
+  trailFilter_.type = type;
+  trailFilter_.threshold = threshold;
+}
+
 bool MetavisionWrapper::initialize(bool useMultithreading, const std::string & biasFile)
 {
   biasFile_ = biasFile;
@@ -428,8 +444,33 @@ void MetavisionWrapper::setDecodingEvents(bool decodeEvents)
   }
 }
 
+void MetavisionWrapper::activateTrailFilter()
+{
+  Metavision::I_EventTrailFilterModule * i_trail_filter =
+    cam_.get_device().get_facility<Metavision::I_EventTrailFilterModule>();
+
+  const auto it = trailFilterMap.find(trailFilter_.type);
+  if (it == trailFilterMap.end()) {
+    BOMB_OUT_CERR("unknown trail filter type " << trailFilter_.type);
+  }
+
+  // Set filter type
+  if (!i_trail_filter->set_type(it->second)) {
+    LOG_WARN_NAMED("cannot set type of trail filter!")
+  }
+  if (!i_trail_filter->set_threshold(trailFilter_.threshold)) {
+    LOG_WARN_NAMED("cannot set threshold of trail filter!")
+  }
+
+  i_trail_filter->enable(trailFilter_.enabled);
+}
+
 bool MetavisionWrapper::startCamera(CallbackHandler * h)
 {
+  if (trailFilter_.enabled) {
+    activateTrailFilter();
+  }
+
   try {
     callbackHandler_ = h;
     if (useMultithreading_) {