diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py
index 43cbe884..b78aeb45 100644
--- a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py
+++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py
@@ -455,7 +455,7 @@ def input_pointcloud(
         """
         raw_points = cp.asarray(raw_points, dtype=self.data_type)
         additional_channels = channels[3:]
-        raw_points = raw_points[~cp.isnan(raw_points).any(axis=1)]
+        raw_points = raw_points[~cp.isnan(raw_points[:, :3]).any(axis=1)]
         self.update_map_with_kernel(
             raw_points,
             additional_channels,