Set xyz values of individual points in the PointCloud to NaNs when ra…

…nge is zero

CHANGELOG.rst

@@ -13,7 +13,8 @@ Changelog
   * address an issue where LaserScan appeared different on FW prior to 2.4
 * [BUGFIX]: LaserScan does not work when using dual mode
 * [BUGFIX]: Implement lock free ring buffer with throttling to avoid generating partial frames
-* add support for FUSA udp profile ``Point_FUSA_RNG15_RFL8_NIR8_DUAL``
+* add support for FUSA udp profile ``Point_FUSA_RNG15_RFL8_NIR8_DUAL``.
+* Set xyz values of individual points in the PointCloud to NaNs when range is zero.
 
 
 ouster_ros v0.10.0

src/impl/cartesian.h

@@ -0,0 +1,64 @@
+#pragma once
+
+#include <ouster/lidar_scan.h>
+
+namespace ouster {
+
+// TODO: move to the sdk client
+
+/**
+ * This is the same function as the cartesianT method defined in the client but
+ * allows the user choose a specific value for range values of zero.
+ *
+ * @param[in, out] points The resulting point cloud, should be pre-allocated and
+ * have the same dimensions as the direction array.
+ * @param[in] range a range image in the same format as the RANGE field of a
+ * LidarScan.
+ * @param[in] direction the direction of an xyz lut.
+ * @param[in] offset the offset of an xyz lut.
+ * @param[in] invalid the value to assign of an xyz lut.
+ *
+ * @return Cartesian points where ith row is a 3D point which corresponds
+ *         to ith pixel in LidarScan where i = row * w + col.
+ */
+template <typename T>
+void cartesianT(PointsT<T>& points,
+                const Eigen::Ref<const img_t<uint32_t>>& range,
+                const PointsT<T>& direction, const PointsT<T>& offset,
+                T invalid) {
+    assert(points.rows() == direction.rows() &&
+           "points & direction row count mismatch");
+    assert(points.rows() == offset.rows() &&
+           "points & offset row count mismatch");
+    assert(points.rows() == range.size() &&
+           "points and range image size mismatch");
+
+    const auto pts = points.data();
+    const auto* const rng = range.data();
+    const auto* const dir = direction.data();
+    const auto* const ofs = offset.data();
+
+    const auto N = range.size();
+    const auto col_x = 0 * N;  // 1st column of points (x)
+    const auto col_y = 1 * N;  // 2nd column of points (y)
+    const auto col_z = 2 * N;  // 3rd column of points (z)
+
+#ifdef __OUSTER_UTILIZE_OPENMP__
+#pragma omp parallel for schedule(static)
+#endif
+    for (auto i = 0; i < N; ++i) {
+        const auto r = rng[i];
+        const auto idx_x = col_x + i;
+        const auto idx_y = col_y + i;
+        const auto idx_z = col_z + i;
+        if (r == 0) {
+            pts[idx_x] = pts[idx_y] = pts[idx_z] = invalid;
+        } else {
+            pts[idx_x] = r * dir[idx_x] + ofs[idx_x];
+            pts[idx_y] = r * dir[idx_y] + ofs[idx_y];
+            pts[idx_z] = r * dir[idx_z] + ofs[idx_z];
+        }
+    }
+}
+
+}  // namespace ouster

src/point_cloud_processor.h

@@ -16,6 +16,7 @@
 
 #include "point_cloud_compose.h"
 #include "lidar_packet_handler.h"
+#include "impl/cartesian.h"
 
 namespace ouster_ros {
 
@@ -78,7 +79,8 @@ class PointCloudProcessor {
         for (int i = 0; i < static_cast<int>(pc_msgs.size()); ++i) {
             auto range_channel = static_cast<sensor::ChanField>(sensor::ChanField::RANGE + i);
             auto range = lidar_scan.field<uint32_t>(range_channel);
-            ouster::cartesianT(points, range, lut_direction, lut_offset);
+            ouster::cartesianT(points, range, lut_direction, lut_offset,
+                               std::numeric_limits<float>::quiet_NaN());
 
             scan_to_cloud_fn(cloud, points, scan_ts, lidar_scan,
                                         pixel_shift_by_row, i);