PointCloud mask

CHANGELOG.rst

@@ -18,6 +18,8 @@ Changelog
   version ``pcl::PointXYZI`` for bandwith sensitive applications.
 * Introduce a new param ``v_reduction`` that allows reducing the number of beams count of the published
   point cloud.
+* Introduce a new capability to suppress certain range measurements of the point cloud by providing
+  a mask image to the driver through the ``mask_path`` launch file argument.
 
 
 ouster_ros v0.13.0

CMakeLists.txt

@@ -12,6 +12,7 @@ find_package(PCL REQUIRED COMPONENTS common)
 find_package(tf2_eigen REQUIRED)
 find_package(CURL REQUIRED)
 find_package(Boost REQUIRED)
+find_package(OpenCV REQUIRED)
 
 find_package(
   catkin REQUIRED
@@ -54,6 +55,7 @@ catkin_package(
     geometry_msgs
   DEPENDS
     EIGEN3
+    OpenCV
 )
 
 # ==== Libraries ====
@@ -71,7 +73,10 @@ find_package(OusterSDK REQUIRED)
 set(BUILD_SHARED_LIBS ${_SAVE_BUILD_SHARED_LIBS})
 
 # catkin adds all include dirs to a single variable, don't try to use targets
-include_directories(${_ouster_ros_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
+include_directories(
+  ${_ouster_ros_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS})
 
 # use only MPL-licensed parts of eigen
 add_definitions(-DEIGEN_MPL2_ONLY)
@@ -100,7 +105,10 @@ add_dependencies(ouster_ros ${PROJECT_NAME}_gencpp)
 
 # ==== Executables ====
 add_library(${PROJECT_NAME}_nodelets ${NODELET_SRC})
-target_link_libraries(${PROJECT_NAME}_nodelets ouster_ros ${catkin_LIBRARIES})
+target_link_libraries(
+  ${PROJECT_NAME}_nodelets ouster_ros
+  ${catkin_LIBRARIES}
+  ${OpenCV_LIBRARIES})
 add_dependencies(${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gencpp)
 
 # ==== Test ====

include/ouster_ros/os_ros.h

@@ -19,6 +19,8 @@
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/LaserScan.h>
 #include <sensor_msgs/PointCloud2.h>
+#include <opencv2/opencv.hpp>
+#include <opencv2/core/eigen.hpp>
 
 #include <chrono>
 #include <string>
@@ -132,7 +134,7 @@ Telemetry lidar_packet_to_telemetry_msg(
 
 
 namespace impl {
-sensor::ChanField suitable_return(sensor::ChanField input_field, bool second);
+sensor::ChanField scan_return(sensor::ChanField input_field, bool second);
 
 struct read_and_cast {
     template <typename T, typename U>
@@ -183,6 +185,32 @@ uint64_t ulround(T value) {
     return static_cast<uint64_t>(rounded_value);
 }
 
+void warn_mask_resized(int image_cols, int image_rows,
+                       int scan_height, int scan_width);
+
+template <typename pixel_type>
+ouster::img_t<pixel_type> load_mask(const std::string& mask_path,
+                                    size_t height, size_t width) {
+    if (mask_path.empty()) return ouster::img_t<pixel_type>();
+
+    cv::Mat image = cv::imread(mask_path, cv::IMREAD_GRAYSCALE);
+    if (image.empty()) {
+        throw std::runtime_error("Failed to load mask image from path: " + mask_path);
+    }
+
+    if (image.rows != static_cast<int>(height) || image.cols != static_cast<int>(width)) {
+        warn_mask_resized(image.cols, image.rows, static_cast<int>(height), static_cast<int>(width));
+        cv::Mat resized;
+        cv::resize(image, resized, cv::Size(width, height), 0, 0, cv::INTER_NEAREST);
+        image = resized;
+    }
+    Eigen::MatrixXi eigen_img(image.rows, image.cols);
+    cv::cv2eigen(image, eigen_img);
+    Eigen::MatrixXi zero_image = Eigen::MatrixXi::Zero(eigen_img.rows(), eigen_img.cols());
+    Eigen::MatrixXi ones_image = Eigen::MatrixXi::Ones(eigen_img.rows(), eigen_img.cols());
+    return (eigen_img.array() == 0.0).select(zero_image, ones_image).cast<pixel_type>();
+}
+
 } // namespace impl
 
 }  // namespace ouster_ros

launch/common.launch

@@ -57,6 +57,8 @@
 
   <arg name="v_reduction" doc="vertical beam reduction; available options: {1, 2, 4, 8, 16}"/>
 
+  <arg name="mask_path" doc="path to an image file that will be used to mask parts of the pointcloud"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_cloud_node"
       output="screen" required="true"
@@ -81,6 +83,7 @@
       <param name="~/min_range" value="$(arg min_range)"/>
       <param name="~/max_range" value="$(arg max_range)"/>
       <param name="~/v_reduction" value="$(arg v_reduction)"/>
+      <param name="~/mask_path" value="$(arg mask_path)"/>
       <param name="~/min_scan_valid_columns_ratio"
         value="$(arg min_scan_valid_columns_ratio)"/>
     </node>
@@ -91,6 +94,7 @@
       output="screen" required="true"
       args="load ouster_ros/OusterImage os_nodelet_mgr $(arg _no_bond)">
       <param name="~/proc_mask" value="$(arg proc_mask)"/>
+      <param name="~/mask_path" value="$(arg mask_path)"/>
     </node>
   </group>
 

launch/driver.launch

@@ -106,6 +106,9 @@
   <arg name="v_reduction" default="1"
     doc="vertical beam reduction; available options: {1, 2, 4, 8, 16}"/>
 
+  <arg name="mask_path" default=""
+    doc="path to an image file that will be used to mask parts of the pointcloud"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -147,6 +150,7 @@
       <param name="~/min_range" value="$(arg min_range)"/>
       <param name="~/max_range" value="$(arg max_range)"/>
       <param name="~/v_reduction" value="$(arg v_reduction)"/>
+      <param name="~/mask_path" value="$(arg mask_path)"/>
       <param name="~/min_scan_valid_columns_ratio"
         value="$(arg min_scan_valid_columns_ratio)"/>
     </node>

launch/record.launch

@@ -106,6 +106,9 @@
   <arg name="v_reduction" default="1"
     doc="vertical beam reduction; available options: {1, 2, 4, 8, 16}"/>
 
+  <arg name="mask_path" default=""
+    doc="path to an image file that will be used to mask parts of the pointcloud"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -162,6 +165,7 @@
     <arg name="min_range" value="$(arg min_range)"/>
     <arg name="max_range" value="$(arg max_range)"/>
     <arg name="v_reduction" value="$(arg v_reduction)"/>
+    <arg name="~/mask_path" value="$(arg mask_path)"/>
     <arg name="min_scan_valid_columns_ratio"
         value="$(arg min_scan_valid_columns_ratio)"/>
   </include>

launch/replay.launch

@@ -84,6 +84,9 @@
   <arg name="v_reduction" default="1"
     doc="vertical beam reduction; available options: {1, 2, 4, 8, 16}"/>
 
+  <arg name="mask_path" default=""
+    doc="path to an image file that will be used to mask parts of the pointcloud"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -124,6 +127,7 @@
     <arg name="min_range" value="$(arg min_range)"/>
     <arg name="max_range" value="$(arg max_range)"/>
     <arg name="v_reduction" value="$(arg v_reduction)"/>
+    <arg name="mask_path" value="$(arg mask_path)"/>
     <arg name="min_scan_valid_columns_ratio"
         value="$(arg min_scan_valid_columns_ratio)"/>
   </include>

launch/replay_pcap.launch

@@ -78,6 +78,9 @@
   <arg name="v_reduction" default="1"
     doc="vertical beam reduction; available options: {1, 2, 4, 8, 16}"/>
 
+  <arg name="mask_path" default=""
+    doc="path to an image file that will be used to mask parts of the pointcloud"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -119,6 +122,7 @@
     <arg name="min_range" value="$(arg min_range)"/>
     <arg name="max_range" value="$(arg max_range)"/>
     <arg name="v_reduction" value="$(arg v_reduction)"/>
+    <arg name="mask_path" value="$(arg mask_path)"/>
     <arg name="min_scan_valid_columns_ratio"
         value="$(arg min_scan_valid_columns_ratio)"/>
   </include>

launch/sensor.launch

@@ -114,6 +114,9 @@
   <arg name="v_reduction" default="1"
     doc="vertical beam reduction; available options: {1, 2, 4, 8, 16}"/>
 
+  <arg name="mask_path" default=""
+    doc="path to an image file that will be used to mask parts of the pointcloud"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -170,6 +173,7 @@
     <arg name="min_range" value="$(arg min_range)"/>
     <arg name="max_range" value="$(arg max_range)"/>
     <arg name="v_reduction" value="$(arg v_reduction)"/>
+    <arg name="mask_path" value="$(arg mask_path)"/>
     <arg name="min_scan_valid_columns_ratio"
         value="$(arg min_scan_valid_columns_ratio)"/>
   </include>

launch/sensor_mtp.launch

@@ -118,6 +118,9 @@
   <arg name="v_reduction" default="1"
     doc="vertical beam reduction; available options: {1, 2, 4, 8, 16}"/>
 
+  <arg name="mask_path" default=""
+    doc="path to an image file that will be used to mask parts of the pointcloud"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -176,6 +179,7 @@
     <arg name="min_range" value="$(arg min_range)"/>
     <arg name="max_range" value="$(arg max_range)"/>
     <arg name="v_reduction" value="$(arg v_reduction)"/>
+    <arg name="mask_path" value="$(arg mask_path)"/>
     <arg name="min_scan_valid_columns_ratio"
         value="$(arg min_scan_valid_columns_ratio)"/>
   </include>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>ouster_ros</name>
-  <version>0.13.6</version>
+  <version>0.13.7</version>
   <description>Ouster ROS driver</description>
   <maintainer email="oss@ouster.io">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>
@@ -14,6 +14,7 @@
   <depend>tf2_ros</depend>
   <depend>pcl_ros</depend>
   <depend>pcl_conversions</depend>
+  <depend>cv_bridge</depend>
 
   <build_depend>boost</build_depend>
   <build_depend>nodelet</build_depend>