- Add 'general/region_of_interest' parameter

CHANGELOG.rst

@@ -6,6 +6,7 @@ CHANGELOG
 - Change the parameter 'general/resolution' to 'general/grab_resolution' and replace numeric values with strings
 - Add 'general.svo_realtime' parameter
 - Change 'general/verbose' to 'general/sdk_verbose'
+- Add 'general/region_of_interest' parameter
 
 2023-09-10
 ----------

zed_nodelets/src/tools/include/sl_tools.h

@@ -72,6 +72,19 @@ bool isZED2OrZED2i(sl::MODEL camModel);
  */
 bool isZEDX(sl::MODEL camModel);
 
+/*! \brief Creates an sl::Mat containing a ROI from a polygon
+ *  \param poly the ROI polygon. Coordinates must be normalized from 0.0 to 1.0
+ *  \param out_roi the `sl::Mat` containing the ROI
+ */
+bool generateROI(const std::vector<sl::float2> & poly, sl::Mat & out_roi);
+
+/*! \brief Parse a vector of vector of floats from a string.
+ *  \param input
+ *  \param error_return
+ *  Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...] */
+std::vector<std::vector<float>> parseStringVector(
+  const std::string & input, std::string & error_return);
+
 /*! \brief sl::Mat to ros message conversion
  * \param imgMsgPtr : the image topic message to publish
  * \param img : the image to publish

zed_nodelets/src/tools/src/sl_tools.cpp

@@ -351,6 +351,139 @@ std::vector<std::string> split_string(const std::string& s, char seperator)
   return output;
 }
 
+inline bool contains(std::vector<sl::float2> & poly, sl::float2 test)
+{
+  int i, j;
+  bool c = false;
+  const int nvert = poly.size();
+  for (i = 0, j = nvert - 1; i < nvert; j = i++) {
+    if (
+      ((poly[i].y > test.y) != (poly[j].y > test.y)) &&
+      (test.x <
+      (poly[j].x - poly[i].x) * (test.y - poly[i].y) / (poly[j].y - poly[i].y) + poly[i].x))
+    {
+      c = !c;
+    }
+  }
+  return c;
+}
+
+bool generateROI(const std::vector<sl::float2> & poly, sl::Mat & out_roi)
+{
+  if (poly.size() < 3) {
+    out_roi = sl::Mat();
+    return false;
+  }
+
+  // Set each pixel to valid
+  // std::cerr << "Setting ROI mask to full valid" << std::endl;
+  out_roi.setTo<sl::uchar1>(255, sl::MEM::CPU);
+
+  // ----> De-normalize coordinates
+  size_t w = out_roi.getWidth();
+  size_t h = out_roi.getHeight();
+
+  // std::cerr << "De-normalize coordinates" << std::endl;
+  // std::cerr << "Image resolution: " << w << "x" << h << std::endl;
+  std::vector<sl::float2> poly_img;
+  size_t idx = 0;
+  for (auto & it : poly) {
+    sl::float2 pt;
+    pt.x = it.x * w;
+    pt.y = it.y * h;
+
+    if (pt.x >= w) {
+      pt.x = (w - 1);
+    }
+    if (pt.y >= h) {
+      pt.y = (h - 1);
+    }
+
+    poly_img.push_back(pt);
+
+    ++idx;
+  }
+  // <---- De-normalize coordinates
+
+  // ----> Unset ROI pixels outside the polygon
+  //std::cerr << "Unset ROI pixels outside the polygon" << std::endl;
+  //std::cerr << "Set mask" << std::endl;
+  for (int v = 0; v < h; v++) {
+    for (int u = 0; u < w; u++) {
+      if (!contains(poly_img, sl::float2(u, v))) {
+        out_roi.setValue<sl::uchar1>(u, v, 0, sl::MEM::CPU);
+      }
+    }
+  }
+  // std::cerr << "Mask ready" << std::endl;
+  // std::cerr << "ROI resolution: " << w << "x" << h << std::endl;
+  // <---- Unset ROI pixels outside the polygon
+
+  return true;
+}
+
+std::vector<std::vector<float>> parseStringVector(
+  const std::string & input, std::string & error_return)
+{
+  std::vector<std::vector<float>> result;
+
+  std::stringstream input_ss(input);
+  int depth = 0;
+  std::vector<float> current_vector;
+  while (!!input_ss && !input_ss.eof()) {
+    switch (input_ss.peek()) {
+      case EOF:
+        break;
+      case '[':
+        depth++;
+        if (depth > 2) {
+          error_return = "Array depth greater than 2";
+          return result;
+        }
+        input_ss.get();
+        current_vector.clear();
+        break;
+      case ']':
+        depth--;
+        if (depth < 0) {
+          error_return = "More close ] than open [";
+          return result;
+        }
+        input_ss.get();
+        if (depth == 1) {
+          result.push_back(current_vector);
+        }
+        break;
+      case ',':
+      case ' ':
+      case '\t':
+        input_ss.get();
+        break;
+      default:  // All other characters should be part of the numbers.
+        if (depth != 2) {
+          std::stringstream err_ss;
+          err_ss << "Numbers at depth other than 2. Char was '" << char(input_ss.peek()) << "'.";
+          error_return = err_ss.str();
+          return result;
+        }
+        float value;
+        input_ss >> value;
+        if (!!input_ss) {
+          current_vector.push_back(value);
+        }
+        break;
+    }
+  }
+
+  if (depth != 0) {
+    error_return = "Unterminated vector string.";
+  } else {
+    error_return = "";
+  }
+
+  return result;
+}
+
 CSmartMean::CSmartMean(int winSize)
 {
   mValCount = 0;

zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp

@@ -249,6 +249,13 @@ class ZEDWrapperNodelet : public nodelet::Nodelet {
    */
     void checkResolFps();
 
+    // ----> Region of Interest
+    std::string getRoiParam(std::string paramName, std::vector<std::vector<float>> & outVal);
+    std::string parseRoiPoly(
+        const std::vector<std::vector<float>> & in_poly, std::vector<sl::float2> & out_poly);
+    void resetRoi();
+    // <---- Region of Interest
+
     /*! \brief Callback to handle dynamic reconfigure events in ROS
    */
     void callback_dynamicReconf(zed_nodelets::ZedConfig& config, uint32_t level);
@@ -562,6 +569,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet {
     double mPathPubRate;
     int mPathMaxCount;
     int mSdkVerbose=1;
+    std::vector<std::vector<float>> mRoiParam;
     bool mSvoMode = false;
     double mCamMinDepth;
     double mCamMaxDepth;

zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp

@@ -707,15 +707,16 @@ void ZEDWrapperNodelet::readParameters()
     if (tmp_sn > 0) {
         mZedSerialNumber = static_cast<int>(tmp_sn);
         NODELET_INFO_STREAM(" * Serial number\t\t-> " << mZedSerialNumber);
-    }    
-    // <---- General
-
+    }
 
+    std::string parsed_str = getRoiParam("general/region_of_interest", mRoiParam);
+    NODELET_INFO_STREAM(" * Region of interest\t\t-> " << parsed_str.c_str());
+    // <---- General
 
-    // ----> Video    
+    // ----> Video
     //NODELET_INFO_STREAM("*** VIDEO PARAMETERS ***");
     // Note: there are no "static" video parameters
-    // <---- Video    
+    // <---- Video
 
     // -----> Depth
     NODELET_INFO_STREAM("*** DEPTH PARAMETERS ***");
@@ -802,8 +803,8 @@ void ZEDWrapperNodelet::readParameters()
         mNhNs.getParam("pos_tracking/area_memory_db_path", mAreaMemDbPath);
         mAreaMemDbPath = sl_tools::resolveFilePath(mAreaMemDbPath);
         NODELET_INFO_STREAM(" * Odometry DB path\t\t-> " << mAreaMemDbPath.c_str());
-
-        mNhNs.param<bool>("pos_tracking/save_area_memory_db_on_exit", mSaveAreaMapOnClosing,AUTO_WB false);
+        
+        mNhNs.param<bool>("pos_tracking/save_area_memory_db_on_exit", mSaveAreaMapOnClosing, false);
         NODELET_INFO_STREAM(" * Save Area Memory on closing\t-> " << (mSaveAreaMapOnClosing ? "ENABLED": "DISABLED"));
         mNhNs.param<bool>("pos_tracking/area_memory", mAreaMemory, false);
         NODELET_INFO_STREAM(" * Area Memory\t\t\t-> " << (mAreaMemory ? "ENABLED": "DISABLED"));
@@ -1064,6 +1065,91 @@ void ZEDWrapperNodelet::readParameters()
     // <---- Dynamic
 }
 
+std::string ZEDWrapperNodelet::getRoiParam(std::string paramName, std::vector<std::vector<float>> & outVal)
+{
+  outVal.clear();
+
+  std::string roi_param_str = "";
+
+  mNhNs.getParam("general/region_of_interest", roi_param_str);
+
+  if (roi_param_str.empty()) {
+    return std::string();
+  }
+
+  std::string error;
+  outVal = sl_tools::parseStringVector(roi_param_str, error);
+
+  if (error != "") {
+    NODELET_WARN_STREAM("Error parsing " << paramName << " parameter: " << error.c_str());
+    NODELET_WARN_STREAM("   " << paramName << " string was " << roi_param_str.c_str());
+
+    outVal.clear();
+  }
+
+  return roi_param_str;
+}
+
+std::string ZEDWrapperNodelet::parseRoiPoly(
+    const std::vector<std::vector<float>> & in_poly, 
+    std::vector<sl::float2> & out_poly)
+{
+  out_poly.clear();
+
+  std::string ss;
+  ss = "[";
+
+  size_t poly_size = in_poly.size();
+
+  if (poly_size < 3) {
+    if (poly_size != 0) {
+      NODELET_WARN_STREAM("A vector with " <<
+          poly_size <<
+          " points is not enough to create a polygon to set a Region "
+          "of Interest.");
+      return std::string();
+    }
+  } else {
+    for (size_t i; i < poly_size; ++i) {
+      if (in_poly[i].size() != 2) {
+        NODELET_WARN_STREAM("The component with index '" << i <<
+            "' of the ROI vector "
+            "has not the correct size.");
+        out_poly.clear();
+        return std::string();
+      } else if (in_poly[i][0] < 0.0 || in_poly[i][1] < 0.0 || in_poly[i][0] > 1.0 ||
+        in_poly[i][1] > 1.0)
+      {
+        NODELET_WARN_STREAM("The component with index '" << i <<
+            "' of the ROI vector "
+            "is not a "
+            "valid normalized point: [" <<
+            in_poly[i][0] << "," << in_poly[i][1] <<
+            "].");
+        out_poly.clear();
+        return std::string();
+      } else {
+        sl::float2 pt;
+        pt.x = in_poly[i][0];
+        pt.y = in_poly[i][1];
+        out_poly.push_back(pt);
+        ss += "[";
+        ss += std::to_string(pt.x);
+        ss += ",";
+        ss += std::to_string(pt.y);
+        ss += "]";
+      }
+
+      if (i != poly_size - 1) {
+        ss += ",";
+      }
+    }
+  }
+  ss += "]";
+
+  return ss;
+}
+
 void ZEDWrapperNodelet::checkResolFps()
 {
     switch (mCamResol) {
@@ -3270,6 +3356,29 @@ void ZEDWrapperNodelet::device_poll_thread_func()
     mMatResol = sl::Resolution(pub_w, pub_h);
     NODELET_DEBUG_STREAM("Publishing frame size: " << mMatResol.width << "x" << mMatResol.height);
 
+    // ----> Set Region of Interest
+    if (!mRoiParam.empty()) {
+        NODELET_INFO("*** Setting ROI ***");
+        sl::Resolution resol(mCamWidth, mCamHeight);
+        std::vector<sl::float2> sl_poly;
+        std::string log_msg = parseRoiPoly(mRoiParam, sl_poly);
+
+        // Create ROI mask
+        sl::Mat roi_mask(resol, sl::MAT_TYPE::U8_C1, sl::MEM::CPU);
+
+        if (!sl_tools::generateROI(sl_poly, roi_mask)) {
+        NODELET_WARN(" * Error generating the region of interest image mask.");
+        } else {
+        sl::ERROR_CODE err = mZed.setRegionOfInterest(roi_mask);
+        if (err != sl::ERROR_CODE::SUCCESS) {
+            NODELET_WARN_STREAM(" * Error while setting ZED SDK region of interest: " << sl::toString(err).c_str());
+        } else {
+            NODELET_INFO(" * Region of Interest correctly set.");
+        }
+        }
+    }
+    // <---- Set Region of Interest
+
     // Create and fill the camera information messages
     fillCamInfo(mZed, mLeftCamInfoMsg, mRightCamInfoMsg, mLeftCamOptFrameId, mRightCamOptFrameId);
     fillCamInfo(mZed, mLeftCamInfoRawMsg, mRightCamInfoRawMsg, mLeftCamOptFrameId, mRightCamOptFrameId, true);

zed_wrapper/params/common.yaml

@@ -32,6 +32,11 @@ general:
     pub_downscale_factor:       2.0                             # rescale factor used to rescale image before publishing when 'pub_resolution' is 'CUSTOM'
     pub_frame_rate:             15.0                            # frequency of publishing of video and depth data (see SDK API "InitParameters::grab_compute_capping_fps")
     svo_realtime:               true                            # if true the input SVO will be played trying to respect the original framerate eventually skipping frames, otherwise every frame will be processed respecting the `pub_frame_rate` setting
+    #region_of_interest:         '[]'                            # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent.
+    region_of_interest:        '[[0.25,0.33],[0.75,0.33],[0.75,0.5],[0.5,0.75],[0.25,0.5]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent.
+    #region_of_interest:        '[[0.25,0.25],[0.75,0.25],[0.75,0.75],[0.25,0.75]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent.
+    #region_of_interest:        '[[0.5,0.25],[0.75,0.5],[0.5,0.75],[0.25,0.5]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent.
+
 
 #video: