add invalid_depth param (ros-perception#943)

Add option to set all invalid depth pixels to a specified value, typically the maximum range.

 * Updates convertDepth parameter name and optimizes use of the parameter.
 * Updates PointCloudXYZ, PointCloudXYZI, and PointCloudXYZRGB with new invalid_depth parameter

.gitignore

@@ -1,3 +1,3 @@
 *pyc
-.vscode/settings.json
+.vscode/
 */doc/generated

depth_image_proc/doc/components.rst

@@ -116,6 +116,8 @@ Parameters
    for the depth topic subscriber.
  * **queue_size** (int, default: 5): Size of message queue for synchronizing
    subscribed topics.
+ * **invalid_depth** (double, default: 0.0): Value used for replacing invalid depth
+   values (if 0.0 the parameter has no effect).
 
 depth_image_proc::PointCloudXyzRadialNode
 -----------------------------------------
@@ -165,6 +167,8 @@ Parameters
    the intensity image subscriber.
  * **queue_size** (int, default: 5): Size of message queue for synchronizing
    subscribed topics.
+ * **invalid_depth** (double, default: 0.0): Value used for replacing invalid depth
+   values (if 0.0 the parameter has no effect).
 
 depth_image_proc::PointCloudXyziRadialNode
 ------------------------------------------
@@ -235,6 +239,8 @@ Parameters
  * **exact_sync** (bool, default: False): Whether to use exact synchronizer.
  * **queue_size** (int, default: 5): Size of message queue for synchronizing
    subscribed topics.
+ * **invalid_depth** (double, default: 0.0): Value used for replacing invalid depth
+   values (if 0.0 the parameter has no effect).
 
 depth_image_proc::PointCloudXyzrgbRadialNode
 --------------------------------------------

depth_image_proc/include/depth_image_proc/conversions.hpp

@@ -51,9 +51,9 @@ namespace depth_image_proc
 template<typename T>
 void convertDepth(
   const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg,
-  sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg,
+  const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg,
   const image_geometry::PinholeCameraModel & model,
-  double range_max = 0.0)
+  double invalid_depth = 0.0)
 {
   // Use correct principal point from calibration
   float center_x = model.cx();
@@ -65,19 +65,26 @@ void convertDepth(
   float constant_y = unit_scaling / model.fy();
   float bad_point = std::numeric_limits<float>::quiet_NaN();
 
+  // ensure that the computation only happens in case we have a default depth
+  T invalid_depth_cvt = T(0);
+  bool use_invalid_depth = invalid_depth != 0.0;
+  if (use_invalid_depth) {
+    invalid_depth_cvt = DepthTraits<T>::fromMeters(invalid_depth);
+  }
   sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
   sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
   sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");
+
   const T * depth_row = reinterpret_cast<const T *>(&depth_msg->data[0]);
-  int row_step = depth_msg->step / sizeof(T);
-  for (int v = 0; v < static_cast<int>(cloud_msg->height); ++v, depth_row += row_step) {
-    for (int u = 0; u < static_cast<int>(cloud_msg->width); ++u, ++iter_x, ++iter_y, ++iter_z) {
+  uint32_t row_step = depth_msg->step / sizeof(T);
+  for (uint32_t v = 0; v < cloud_msg->height; ++v, depth_row += row_step) {
+    for (uint32_t u = 0; u < cloud_msg->width; ++u, ++iter_x, ++iter_y, ++iter_z) {
       T depth = depth_row[u];
 
       // Missing points denoted by NaNs
       if (!DepthTraits<T>::valid(depth)) {
-        if (range_max != 0.0) {
-          depth = DepthTraits<T>::fromMeters(range_max);
+        if (use_invalid_depth) {
+          depth = invalid_depth_cvt;
         } else {
           *iter_x = *iter_y = *iter_z = bad_point;
           continue;
@@ -96,8 +103,8 @@ void convertDepth(
 template<typename T>
 void convertDepthRadial(
   const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg,
-  sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg,
-  cv::Mat & transform)
+  const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg,
+  const cv::Mat & transform)
 {
   // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
   float bad_point = std::numeric_limits<float>::quiet_NaN();
@@ -129,7 +136,7 @@ void convertDepthRadial(
 template<typename T>
 void convertIntensity(
   const sensor_msgs::msg::Image::ConstSharedPtr & intensity_msg,
-  sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg)
+  const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg)
 {
   sensor_msgs::PointCloud2Iterator<float> iter_i(*cloud_msg, "intensity");
   const T * inten_row = reinterpret_cast<const T *>(&intensity_msg->data[0]);
@@ -145,7 +152,7 @@ void convertIntensity(
 // Handles RGB8, BGR8, and MONO8
 void convertRgb(
   const sensor_msgs::msg::Image::ConstSharedPtr & rgb_msg,
-  sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg,
+  sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg,
   int red_offset, int green_offset, int blue_offset, int color_step);
 
 cv::Mat initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial);

depth_image_proc/include/depth_image_proc/point_cloud_xyz.hpp

@@ -67,6 +67,9 @@ class PointCloudXyzNode : public rclcpp::Node
   image_transport::CameraSubscriber sub_depth_;
   int queue_size_;
 
+  // Parameters
+  double invalid_depth_;
+
   // Publications
   std::mutex connect_mutex_;
   rclcpp::Publisher<PointCloud2>::SharedPtr pub_point_cloud_;

depth_image_proc/include/depth_image_proc/point_cloud_xyzi.hpp

@@ -71,6 +71,9 @@ class PointCloudXyziNode : public rclcpp::Node
   using Synchronizer = message_filters::Synchronizer<SyncPolicy>;
   std::shared_ptr<Synchronizer> sync_;
 
+  // parameters
+  float invalid_depth_;
+
   // Publications
   std::mutex connect_mutex_;
   rclcpp::Publisher<PointCloud>::SharedPtr pub_point_cloud_;

depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb.hpp

@@ -75,6 +75,9 @@ class PointCloudXyzrgbNode : public rclcpp::Node
   std::shared_ptr<Synchronizer> sync_;
   std::shared_ptr<ExactSynchronizer> exact_sync_;
 
+  // parameters
+  float invalid_depth_;
+
   // Publications
   std::mutex connect_mutex_;
   rclcpp::Publisher<PointCloud2>::SharedPtr pub_point_cloud_;

depth_image_proc/src/conversions.cpp

@@ -79,7 +79,7 @@ cv::Mat initMatrix(
 
 void convertRgb(
   const sensor_msgs::msg::Image::ConstSharedPtr & rgb_msg,
-  sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg,
+  const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg,
   int red_offset, int green_offset, int blue_offset, int color_step)
 {
   sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(*cloud_msg, "r");
@@ -101,8 +101,8 @@ void convertRgb(
 // force template instantiation
 template void convertDepth<uint16_t>(
   const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg,
-  sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg,
+  const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg,
   const image_geometry::PinholeCameraModel & model,
-  double range_max);
+  double invalid_depth);
 
 }  // namespace depth_image_proc

depth_image_proc/src/point_cloud_xyz.cpp

@@ -58,6 +58,9 @@ PointCloudXyzNode::PointCloudXyzNode(const rclcpp::NodeOptions & options)
   // Read parameters
   queue_size_ = this->declare_parameter<int>("queue_size", 5);
 
+  // values used for invalid points for pcd conversion
+  invalid_depth_ = this->declare_parameter<double>("invalid_depth", 0.0);
+
   // Create publisher with connect callback
   rclcpp::PublisherOptions pub_options;
   pub_options.event_callbacks.matched_callback =
@@ -94,7 +97,7 @@ void PointCloudXyzNode::depthCb(
   const Image::ConstSharedPtr & depth_msg,
   const CameraInfo::ConstSharedPtr & info_msg)
 {
-  auto cloud_msg = std::make_shared<PointCloud2>();
+  const PointCloud2::SharedPtr cloud_msg = std::make_shared<PointCloud2>();
   cloud_msg->header = depth_msg->header;
   cloud_msg->height = depth_msg->height;
   cloud_msg->width = depth_msg->width;
@@ -109,9 +112,9 @@ void PointCloudXyzNode::depthCb(
 
   // Convert Depth Image to Pointcloud
   if (depth_msg->encoding == enc::TYPE_16UC1 || depth_msg->encoding == enc::MONO16) {
-    convertDepth<uint16_t>(depth_msg, cloud_msg, model_);
+    convertDepth<uint16_t>(depth_msg, cloud_msg, model_, invalid_depth_);
   } else if (depth_msg->encoding == enc::TYPE_32FC1) {
-    convertDepth<float>(depth_msg, cloud_msg, model_);
+    convertDepth<float>(depth_msg, cloud_msg, model_, invalid_depth_);
   } else {
     RCLCPP_ERROR(
       get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());

depth_image_proc/src/point_cloud_xyzi.cpp

@@ -58,6 +58,9 @@ PointCloudXyziNode::PointCloudXyziNode(const rclcpp::NodeOptions & options)
   this->declare_parameter<std::string>("image_transport", "raw");
   this->declare_parameter<std::string>("depth_image_transport", "raw");
 
+  // value used for invalid points for pcd conversion
+  invalid_depth_ = this->declare_parameter<double>("invalid_depth", 0.0);
+
   // Read parameters
   int queue_size = this->declare_parameter<int>("queue_size", 5);
 
@@ -202,9 +205,9 @@ void PointCloudXyziNode::imageCb(
 
   // Convert Depth Image to Pointcloud
   if (depth_msg->encoding == enc::TYPE_16UC1) {
-    convertDepth<uint16_t>(depth_msg, cloud_msg, model_);
+    convertDepth<uint16_t>(depth_msg, cloud_msg, model_, invalid_depth_);
   } else if (depth_msg->encoding == enc::TYPE_32FC1) {
-    convertDepth<float>(depth_msg, cloud_msg, model_);
+    convertDepth<float>(depth_msg, cloud_msg, model_, invalid_depth_);
   } else {
     RCLCPP_ERROR(
       get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());

depth_image_proc/src/point_cloud_xyzrgb.cpp

@@ -55,6 +55,9 @@ PointCloudXyzrgbNode::PointCloudXyzrgbNode(const rclcpp::NodeOptions & options)
   this->declare_parameter<std::string>("image_transport", "raw");
   this->declare_parameter<std::string>("depth_image_transport", "raw");
 
+  // value used for invalid points for pcd conversion
+  invalid_depth_ = this->declare_parameter<double>("invalid_depth", 0.0);
+
   // Read parameters
   int queue_size = this->declare_parameter<int>("queue_size", 5);
   bool use_exact_sync = this->declare_parameter<bool>("exact_sync", false);
@@ -256,9 +259,9 @@ void PointCloudXyzrgbNode::imageCb(
 
   // Convert Depth Image to Pointcloud
   if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) {
-    convertDepth<uint16_t>(depth_msg, cloud_msg, model_);
+    convertDepth<uint16_t>(depth_msg, cloud_msg, model_, invalid_depth_);
   } else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) {
-    convertDepth<float>(depth_msg, cloud_msg, model_);
+    convertDepth<float>(depth_msg, cloud_msg, model_, invalid_depth_);
   } else {
     RCLCPP_ERROR(
       get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());