Added invalid point checking to RGBD and Depth Camera plugins to set is_dense flag accordingly (#575) (#578)

(cherry picked from commit e1c5bd6)

Signed-off-by: Dyst-0 <69257845+Dyst-0@users.noreply.github.com>
Co-authored-by: Dyst-0 <69257845+Dyst-0@users.noreply.github.com>
Co-authored-by: Ian Chen <ichen@openrobotics.org>

Author: 3 people

src/DepthCameraSensor.cc

@@ -624,7 +624,6 @@ bool DepthCameraSensor::Update(
     // Set the time stamp
     *this->dataPtr->pointMsg.mutable_header()->mutable_stamp() =
       msgs::Convert(_now);
-    this->dataPtr->pointMsg.set_is_dense(true);
 
     if (!this->dataPtr->xyzBuffer)
       this->dataPtr->xyzBuffer = new float[width*height*3];

src/PointCloudUtil.cc

@@ -37,6 +37,9 @@ void PointCloudUtil::FillMsg(msgs::PointCloudPacked &_msg,
   msgBuffer->resize(_msg.row_step() * _msg.height());
   char *msgBufferIndex = msgBuffer->data();
 
+  // Set Pointcloud as dense. Change if invalid points are found.
+  bool isDense { true };
+
   // For depth calculation from image
   double fl = width / (2.0 * std::tan(_hfov.Radian() / 2.0));
 
@@ -54,6 +57,10 @@ void PointCloudUtil::FillMsg(msgs::PointCloudPacked &_msg,
       // Current point depth
       float depth = _depthData[j * width + i];
 
+      // Validate Depth/Radius and update pointcloud density flag
+      if (isDense)
+        isDense = !(gz::math::isnan(depth) || std::isinf(depth));
+
       float yAngle = 0.0;
       if (fl > 0 && width > 1)
         yAngle = std::atan2(0.5 * (width - 1) - i, fl);
@@ -87,6 +94,7 @@ void PointCloudUtil::FillMsg(msgs::PointCloudPacked &_msg,
       msgBufferIndex += _msg.point_step();
     }
   }
+  _msg.set_is_dense(isDense);
 }
 
 //////////////////////////////////////////////////
@@ -100,6 +108,9 @@ void PointCloudUtil::FillMsg(msgs::PointCloudPacked &_msg,
   msgBuffer->resize(_msg.row_step() * _msg.height());
   char *msgBufferIndex = msgBuffer->data();
 
+  // Set Pointcloud as dense. Change if invalid points are found.
+  bool isDense { true };
+
   // Iterate over scan and populate point cloud
   for (uint32_t j = 0; j < height; ++j)
   {
@@ -111,6 +122,14 @@ void PointCloudUtil::FillMsg(msgs::PointCloudPacked &_msg,
       float y = _xyzData[index + 1];
       float z = _xyzData[index + 2];
 
+      // Validate Depth/Radius and update pointcloud density flag
+      if (isDense)
+      {
+        isDense = !(gz::math::isnan(x) || std::isinf(x) ||
+                    gz::math::isnan(y) || std::isinf(y) ||
+                    gz::math::isnan(z) || std::isinf(z));
+      }
+
       int fieldIndex = 0;
       *reinterpret_cast<float*>(msgBufferIndex +
           _msg.field(fieldIndex++).offset()) = x;
@@ -142,6 +161,7 @@ void PointCloudUtil::FillMsg(msgs::PointCloudPacked &_msg,
       msgBufferIndex += _msg.point_step();
     }
   }
+  _msg.set_is_dense(isDense);
 }
 
 //////////////////////////////////////////////////
@@ -157,6 +177,9 @@ void PointCloudUtil::FillMsg(msgs::PointCloudPacked &_msg,
   msgBuffer->resize(_msg.row_step() * _msg.height());
   char *msgBufferIndex = msgBuffer->data();
 
+  // Set Pointcloud as dense. Change if invalid points are found.
+  bool isDense { true };
+
   // Iterate over scan and populate point cloud
   for (uint32_t j = 0; j < height; ++j)
   {
@@ -168,6 +191,14 @@ void PointCloudUtil::FillMsg(msgs::PointCloudPacked &_msg,
       float x = _pointCloudData[pcIndex];
       float y = _pointCloudData[pcIndex + 1];
       float z = _pointCloudData[pcIndex + 2];
+      // Validate Depth/Radius and update pointcloud density flag
+      if (isDense)
+      {
+        isDense = !(gz::math::isnan(x) || std::isinf(x) ||
+                    gz::math::isnan(y) || std::isinf(y) ||
+                    gz::math::isnan(z) || std::isinf(z));
+      }
+
       float rgba = _pointCloudData[pcIndex + 3];
 
       int fieldIndex = 0;
@@ -218,6 +249,7 @@ void PointCloudUtil::FillMsg(msgs::PointCloudPacked &_msg,
       }
     }
   }
+  _msg.set_is_dense(isDense);
 }
 
 

src/RgbdCameraSensor.cc

@@ -559,7 +559,6 @@ bool RgbdCameraSensor::Update(const std::chrono::steady_clock::duration &_now)
       // Set the time stamp
       *this->dataPtr->pointMsg.mutable_header()->mutable_stamp() =
         msgs::Convert(_now);
-      this->dataPtr->pointMsg.set_is_dense(true);
 
       if ((this->dataPtr->hasDepthNearClip || this->dataPtr->hasDepthFarClip)
           && this->dataPtr->depthBuffer)