PointCloud mask

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 ====

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>

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>

src/os_cloud_nodelet.cpp

@@ -213,11 +213,13 @@ class OusterCloud : public nodelet::Nodelet {
                 throw std::runtime_error("invalid v_reduction value!");
             }
 
+            auto mask_path = pnh.param("mask_path", std::string{});
+
             processors.push_back(
                 PointCloudProcessorFactory::create_point_cloud_processor(
                     point_type, info, tf_bcast.point_cloud_frame_id(),
                     tf_bcast.apply_lidar_to_sensor_transform(), organized,
-                    destagger, min_range, max_range, v_reduction,
+                    destagger, min_range, max_range, v_reduction, mask_path,
                     [this](PointCloudProcessor_OutputType msgs) {
                         for (size_t i = 0; i < msgs.size(); ++i) {
                             if (msgs[i]->header.stamp > last_msg_ts)

src/os_driver_nodelet.cpp

@@ -159,11 +159,13 @@ class OusterDriver : public OusterSensor {
                 throw std::runtime_error("invalid v_reduction value!");
             }
 
+            auto mask_path = pnh.param("mask_path", std::string{});
+
             processors.push_back(
                 PointCloudProcessorFactory::create_point_cloud_processor(
                     point_type, info, tf_bcast.point_cloud_frame_id(),
                     tf_bcast.apply_lidar_to_sensor_transform(), organized,
-                    destagger, min_range, max_range, v_reduction,
+                    destagger, min_range, max_range, v_reduction, mask_path,
                     [this](PointCloudProcessor_OutputType msgs) {
                         for (size_t i = 0; i < msgs.size(); ++i)
                             lidar_pubs[i].publish(*msgs[i]);

src/point_cloud_processor.h

@@ -18,6 +18,10 @@
 #include "lidar_packet_handler.h"
 #include "impl/cartesian.h"
 
+#include <opencv2/opencv.hpp>
+#include <opencv2/core/eigen.hpp>
+
+
 namespace ouster_ros {
 
 // Moved out of PointCloudProcessor to avoid type templatization
@@ -40,6 +44,7 @@ class PointCloudProcessor {
                         const std::string& frame_id,
                         bool apply_lidar_to_sensor_transform,
                         uint32_t min_range, uint32_t max_range, int rows_step,
+                        const std::string& mask_path,
                         ScanToCloudFn scan_to_cloud_fn_,
                         PointCloudProcessor_PostProcessingFn post_processing_fn_)
         : frame(frame_id),
@@ -66,6 +71,17 @@ class PointCloudProcessor {
         lut_direction = xyz_lut.direction.cast<float>();
         lut_offset = xyz_lut.offset.cast<float>();
         points = ouster::PointsF(lut_direction.rows(), lut_offset.cols());
+
+        if (!mask_path.empty()) {
+            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);
+            }
+            Eigen::MatrixXi eigen_img(image.rows, image.cols);
+            cv::cv2eigen(image, eigen_img);
+            mask = eigen_img.cast<uint32_t>() / 255;
+        }
     }
 
    private:
@@ -82,7 +98,8 @@ class PointCloudProcessor {
         for (int i = 0; i < static_cast<int>(pc_msgs.size()); ++i) {
             auto range_ch = static_cast<sensor::ChanField>(sensor::ChanField::RANGE + i);
             auto range = lidar_scan.field<uint32_t>(range_ch);
-            ouster::cartesianT(points, range, lut_direction, lut_offset,
+            auto range_masked = mask.rows() == range.rows() && mask.cols() == range.cols() ? range * mask : range;
+            ouster::cartesianT(points, range_masked, lut_direction, lut_offset,
                                min_range_, max_range_,
                                std::numeric_limits<float>::quiet_NaN());
 
@@ -102,11 +119,12 @@ class PointCloudProcessor {
                                      const std::string& frame,
                                      bool apply_lidar_to_sensor_transform,
                                      uint32_t min_range, uint32_t max_range,
-                                     int rows_step, ScanToCloudFn scan_to_cloud_fn_,
+                                     int rows_step, const std::string& mask_path,
+                                     ScanToCloudFn scan_to_cloud_fn_,
                                      PointCloudProcessor_PostProcessingFn post_processing_fn) {
         auto handler = std::make_shared<PointCloudProcessor>(
             info, frame, apply_lidar_to_sensor_transform,
-            min_range, max_range, rows_step,
+            min_range, max_range, rows_step, mask_path,
             scan_to_cloud_fn_, post_processing_fn);
 
         return [handler](const ouster::LidarScan& lidar_scan, uint64_t scan_ts,
@@ -132,6 +150,8 @@ class PointCloudProcessor {
     PointCloudProcessor_OutputType pc_msgs;
     ScanToCloudFn scan_to_cloud_fn;
     PointCloudProcessor_PostProcessingFn post_processing_fn;
+
+    ouster::img_t<uint32_t> mask;
 };
 
 }  // namespace ouster_ros

src/point_cloud_processor_factory.h

@@ -118,12 +118,13 @@ class PointCloudProcessorFactory {
         bool apply_lidar_to_sensor_transform,
         bool organized, bool destagger,
         uint32_t min_range, uint32_t max_range, int rows_step,
+        const std::string& mask_path,
         PointCloudProcessor_PostProcessingFn post_processing_fn) {
         auto scan_to_cloud_fn = make_scan_to_cloud_fn<PointT>(
             info, organized, destagger, rows_step);
         return PointCloudProcessor<PointT>::create(
             info, frame, apply_lidar_to_sensor_transform,
-            min_range, max_range, rows_step,
+            min_range, max_range, rows_step, mask_path,
             scan_to_cloud_fn, post_processing_fn);
     }
 
@@ -144,37 +145,38 @@ class PointCloudProcessorFactory {
         const std::string& frame, bool apply_lidar_to_sensor_transform,
         bool organized, bool destagger,
         uint32_t min_range, uint32_t max_range, int rows_step,
+        const std::string& mask_path,
         PointCloudProcessor_PostProcessingFn post_processing_fn) {
         if (point_type == "native") {
             switch (info.format.udp_profile_lidar) {
                 case UDPProfileLidar::PROFILE_LIDAR_LEGACY:
                     return make_point_cloud_processor<Point_LEGACY>(
                         info, frame, apply_lidar_to_sensor_transform,
                         organized, destagger, min_range, max_range, rows_step,
-                        post_processing_fn);
+                        mask_path, post_processing_fn);
                 case UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL:
                     return make_point_cloud_processor<
                         Point_RNG19_RFL8_SIG16_NIR16_DUAL>(
                         info, frame, apply_lidar_to_sensor_transform,
                         organized, destagger, min_range, max_range, rows_step,
-                        post_processing_fn);
+                        mask_path, post_processing_fn);
                 case UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16:
                     return make_point_cloud_processor<
                         Point_RNG19_RFL8_SIG16_NIR16>(
                         info, frame, apply_lidar_to_sensor_transform,
                         organized, destagger, min_range, max_range, rows_step,
-                        post_processing_fn);
+                        mask_path, post_processing_fn);
                 case UDPProfileLidar::PROFILE_RNG15_RFL8_NIR8:
                     return make_point_cloud_processor<Point_RNG15_RFL8_NIR8>(
                         info, frame, apply_lidar_to_sensor_transform,
                         organized, destagger, min_range, max_range, rows_step,
-                        post_processing_fn);
+                        mask_path, post_processing_fn);
                 case UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL:
                     return make_point_cloud_processor<
                         Point_FUSA_RNG15_RFL8_NIR8_DUAL>(
                         info, frame, apply_lidar_to_sensor_transform,
                         organized, destagger, min_range, max_range, rows_step,
-                        post_processing_fn);
+                        mask_path, post_processing_fn);
                 default:
                     // TODO: implement fallback?
                     throw std::runtime_error("unsupported udp_profile_lidar");
@@ -183,27 +185,27 @@ class PointCloudProcessorFactory {
             return make_point_cloud_processor<pcl::PointXYZ>(
                 info, frame, apply_lidar_to_sensor_transform,
                 organized, destagger, min_range, max_range, rows_step,
-                post_processing_fn);
+                mask_path, post_processing_fn);
         } else if (point_type == "xyzi") {
             return make_point_cloud_processor<pcl::PointXYZI>(
                 info, frame, apply_lidar_to_sensor_transform,
                 organized, destagger, min_range, max_range, rows_step,
-                post_processing_fn);
+                mask_path, post_processing_fn);
         } else if (point_type == "o_xyzi") {
             return make_point_cloud_processor<ouster_ros::PointXYZI>(
                 info, frame, apply_lidar_to_sensor_transform,
                 organized, destagger, min_range, max_range, rows_step,
-                post_processing_fn);
+                mask_path, post_processing_fn);
         } else if (point_type == "xyzir") {
             return make_point_cloud_processor<PointXYZIR>(
                 info, frame, apply_lidar_to_sensor_transform,
                 organized, destagger, min_range, max_range, rows_step,
-                post_processing_fn);
+                mask_path, post_processing_fn);
         } else if (point_type == "original") {
             return make_point_cloud_processor<ouster_ros::Point>(
                 info, frame, apply_lidar_to_sensor_transform,
                 organized, destagger, min_range, max_range, rows_step,
-                post_processing_fn);
+                mask_path, post_processing_fn);
         }
 
         throw std::runtime_error(