diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp
index 691c13a6d4701..78f76d4f980c3 100644
--- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp
+++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp
@@ -239,19 +239,42 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent(
 void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack(
   const PointCloud2::ConstSharedPtr & input_pc, PointCloud2 & front_pc, PointCloud2 & behind_pc)
 {
-  PclPointCloud tmp_behind_pc;
-  PclPointCloud tmp_front_pc;
-  for (sensor_msgs::PointCloud2ConstIterator<float> x(*input_pc, "x"), y(*input_pc, "y"),
-       z(*input_pc, "z");
-       x != x.end(); ++x, ++y, ++z) {
+  size_t front_count = 0;
+  size_t behind_count = 0;
+
+  for (sensor_msgs::PointCloud2ConstIterator<float> x(*input_pc, "x"); x != x.end(); ++x) {
     if (*x < 0.0) {
-      tmp_behind_pc.push_back(pcl::PointXYZ(*x, *y, *z));
+      behind_count++;
     } else {
-      tmp_front_pc.push_back(pcl::PointXYZ(*x, *y, *z));
+      front_count++;
     }
   }
-  pcl::toROSMsg(tmp_front_pc, front_pc);
-  pcl::toROSMsg(tmp_behind_pc, behind_pc);
+
+  sensor_msgs::PointCloud2Modifier front_pc_modifier(front_pc);
+  sensor_msgs::PointCloud2Modifier behind_pc_modifier(behind_pc);
+  front_pc_modifier.setPointCloud2FieldsByString(1, "xyz");
+  behind_pc_modifier.setPointCloud2FieldsByString(1, "xyz");
+  front_pc_modifier.resize(front_count);
+  behind_pc_modifier.resize(behind_count);
+
+  sensor_msgs::PointCloud2Iterator<float> fr_iter(front_pc, "x");
+  sensor_msgs::PointCloud2Iterator<float> be_iter(behind_pc, "x");
+
+  for (sensor_msgs::PointCloud2ConstIterator<float> in_iter(*input_pc, "x");
+       in_iter != in_iter.end(); ++in_iter) {
+    if (*in_iter < 0.0) {
+      *be_iter = in_iter[0];
+      be_iter[1] = in_iter[1];
+      be_iter[2] = in_iter[2];
+      ++be_iter;
+    } else {
+      *fr_iter = in_iter[0];
+      fr_iter[1] = in_iter[1];
+      fr_iter[2] = in_iter[2];
+      ++fr_iter;
+    }
+  }
+
   front_pc.header = input_pc->header;
   behind_pc.header = input_pc->header;
 }
diff --git a/perception/probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml b/perception/probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml
index f9a1c4f930001..833c85a2d812e 100644
--- a/perception/probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml
+++ b/perception/probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml
@@ -12,6 +12,10 @@
         map_length_x: 150.0 # [m]
         map_length_y: 150.0 # [m]
 
+      # pointcloud donwsampler
+      downsample_input_pointcloud: true
+      downsample_voxel_size: 0.125 # [m]
+
       # each grid map parameters
       ogm_creation_config:
         height_filter:
diff --git a/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml b/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml
index 8077bdec5008e..cbf2e6c69946d 100644
--- a/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml
+++ b/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml
@@ -8,6 +8,10 @@
       min_height: -1.0
       max_height: 2.0
 
+    # pointcloud donwsampler
+    downsample_input_pointcloud: true
+    downsample_voxel_size: 0.125 # [m]
+
     enable_single_frame_mode: false
     # use sensor pointcloud to filter obstacle pointcloud
     filter_obstacle_pointcloud_by_raw_pointcloud: false
diff --git a/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp b/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp
index 46bd14b843dae..44cb3576768ef 100644
--- a/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp
+++ b/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp
@@ -116,6 +116,10 @@ bool extractCommonPointCloud(
   const sensor_msgs::msg::PointCloud2 & obstacle_pc, const sensor_msgs::msg::PointCloud2 & raw_pc,
   sensor_msgs::msg::PointCloud2 & output_obstacle_pc);
 
+bool extractApproximateCommonPointCloud(
+  const sensor_msgs::msg::PointCloud2 & obstacle_pc, const sensor_msgs::msg::PointCloud2 & raw_pc,
+  const float voxel_size, sensor_msgs::msg::PointCloud2 & output_obstacle_pc);
+
 unsigned char getApproximateOccupancyState(const unsigned char & value);
 }  // namespace utils
 
diff --git a/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py
index dccddfbe54196..efe53b487ea6a 100644
--- a/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py
+++ b/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py
@@ -92,6 +92,60 @@ def get_ogm_creation_config(total_config: dict, list_iter: int) -> dict:
     return ogm_creation_config
 
 
+# generate downsampler
+def get_downsample_filter_node(setting: dict) -> ComposableNode:
+    plugin_str = setting["plugin"]
+    voxel_size = setting["voxel_size"]
+    node_name = setting["node_name"]
+    return ComposableNode(
+        package="pointcloud_preprocessor",
+        plugin=plugin_str,
+        name=node_name,
+        remappings=[
+            ("input", setting["input_topic"]),
+            ("output", setting["output_topic"]),
+        ],
+        parameters=[
+            {
+                "voxel_size_x": voxel_size,
+                "voxel_size_y": voxel_size,
+                "voxel_size_z": voxel_size,
+            }
+        ],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+
+
+def get_downsample_preprocess_nodes(voxel_size: float, raw_pointcloud_topics: list) -> list:
+    nodes = []
+    for i in range(len(raw_pointcloud_topics)):
+        raw_settings = {
+            "plugin": "pointcloud_preprocessor::ApproximateDownsampleFilterComponent",
+            "node_name": "raw_pc_downsample_filter_" + str(i),
+            "input_topic": raw_pointcloud_topics[i],
+            "output_topic": raw_pointcloud_topics[i] + "_downsampled",
+            "voxel_size": voxel_size,
+        }
+        nodes.append(get_downsample_filter_node(raw_settings))
+    obstacle_settings = {
+        "plugin": "pointcloud_preprocessor::ApproximateDownsampleFilterComponent",
+        "node_name": "obstacle_pc_downsample_filter",
+        "input_topic": LaunchConfiguration("input/obstacle_pointcloud"),
+        "output_topic": "/perception/obstacle_segmentation/downsample/pointcloud",
+        "voxel_size": voxel_size,
+    }
+    nodes.append(get_downsample_filter_node(obstacle_settings))
+    raw_settings = {
+        "plugin": "pointcloud_preprocessor::ApproximateDownsampleFilterComponent",
+        "node_name": "obstacle_pc_downsample_filter",
+        "input_topic": LaunchConfiguration("input/obstacle_pointcloud"),
+        "output_topic": "/perception/obstacle_segmentation/downsample/pointcloud",
+        "voxel_size": voxel_size,
+    }
+    nodes.append(get_downsample_filter_node(raw_settings))
+    return nodes
+
+
 def launch_setup(context, *args, **kwargs):
     """Launch fusion based occupancy grid map creation nodes.
 
@@ -118,20 +172,39 @@ def launch_setup(context, *args, **kwargs):
         LaunchConfiguration("pointcloud_container_name").perform(context),
     )
 
+    # Down sample settings
+    downsample_input_pointcloud: bool = total_config["downsample_input_pointcloud"]
+    downsample_voxel_size: float = total_config["downsample_voxel_size"]
+
+    # get obstacle pointcloud
+    obstacle_pointcloud_topic: str = (
+        "/perception/obstacle_segmentation/downsample/pointcloud"
+        if downsample_input_pointcloud
+        else LaunchConfiguration("input/obstacle_pointcloud").perform(context)
+    )
+
     for i in range(number_of_nodes):
         # load parameter file
         ogm_creation_config = get_ogm_creation_config(total_config, i)
         ogm_creation_config["updater_type"] = LaunchConfiguration("updater_type").perform(context)
         ogm_creation_config.update(updater_config)
 
+        raw_pointcloud_topic: str = fusion_config["raw_pointcloud_topics"][i]
+        frame_name: str = raw_pointcloud_topic.split("/")[
+            -2
+        ]  # assume `/sensing/lidar/top/pointcloud` -> `top
+        if downsample_input_pointcloud:
+            raw_pointcloud_topic += "_downsampled"
+
         # generate composable node
         node = ComposableNode(
             package="probabilistic_occupancy_grid_map",
             plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode",
-            name="occupancy_grid_map_node_in_" + str(i),
+            name="occupancy_grid_map_node" + str(i),
+            namespace=frame_name,
             remappings=[
-                ("~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud")),
-                ("~/input/raw_pointcloud", fusion_config["raw_pointcloud_topics"][i]),
+                ("~/input/obstacle_pointcloud", obstacle_pointcloud_topic),
+                ("~/input/raw_pointcloud", raw_pointcloud_topic),
                 ("~/output/occupancy_grid_map", fusion_config["fusion_input_ogm_topics"][i]),
             ],
             parameters=[ogm_creation_config],
@@ -139,6 +212,12 @@ def launch_setup(context, *args, **kwargs):
         )
         gridmap_generation_composable_nodes.append(node)
 
+    if downsample_input_pointcloud:
+        downsapmle_nodes = get_downsample_preprocess_nodes(
+            downsample_voxel_size, fusion_config["raw_pointcloud_topics"]
+        )
+        gridmap_generation_composable_nodes.extend(downsapmle_nodes)
+
     # 2. launch occupancy grid map fusion node
     gridmap_fusion_node = [
         ComposableNode(
diff --git a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py
index 29435c4ea8e24..cf1ca758f3945 100644
--- a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py
+++ b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py
@@ -26,6 +26,47 @@
 import yaml
 
 
+def get_downsample_filter_node(setting: dict) -> ComposableNode:
+    plugin_str = setting["plugin"]
+    voxel_size = setting["voxel_size"]
+    node_name = setting["node_name"]
+    return ComposableNode(
+        package="pointcloud_preprocessor",
+        plugin=plugin_str,
+        name=node_name,
+        remappings=[
+            ("input", setting["input_topic"]),
+            ("output", setting["output_topic"]),
+        ],
+        parameters=[
+            {
+                "voxel_size_x": voxel_size,
+                "voxel_size_y": voxel_size,
+                "voxel_size_z": voxel_size,
+            }
+        ],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+
+
+def get_downsample_preprocess_nodes(voxel_size: float) -> list:
+    raw_settings = {
+        "plugin": "pointcloud_preprocessor::ApproximateDownsampleFilterComponent",
+        "node_name": "raw_pc_downsample_filter",
+        "input_topic": LaunchConfiguration("input/raw_pointcloud"),
+        "output_topic": "/sensing/lidar/concatenated/downsample/pointcloud",
+        "voxel_size": voxel_size,
+    }
+    obstacle_settings = {
+        "plugin": "pointcloud_preprocessor::ApproximateDownsampleFilterComponent",
+        "node_name": "obstacle_pc_downsample_filter",
+        "input_topic": LaunchConfiguration("input/obstacle_pointcloud"),
+        "output_topic": "/perception/obstacle_segmentation/downsample/pointcloud",
+        "voxel_size": voxel_size,
+    }
+    return [get_downsample_filter_node(raw_settings), get_downsample_filter_node(obstacle_settings)]
+
+
 def launch_setup(context, *args, **kwargs):
     # load parameter files
     param_file = LaunchConfiguration("param_file").perform(context)
@@ -38,7 +79,21 @@ def launch_setup(context, *args, **kwargs):
     with open(updater_param_file, "r") as f:
         occupancy_grid_map_updater_params = yaml.safe_load(f)["/**"]["ros__parameters"]
 
-    composable_nodes = [
+    # composable nodes
+    composable_nodes = []
+
+    # add downsample process
+    downsample_input_pointcloud: bool = pointcloud_based_occupancy_grid_map_node_params[
+        "downsample_input_pointcloud"
+    ]
+    if downsample_input_pointcloud:
+        voxel_grid_size: float = pointcloud_based_occupancy_grid_map_node_params[
+            "downsample_voxel_size"
+        ]
+        downsample_preprocess_nodes = get_downsample_preprocess_nodes(voxel_grid_size)
+        composable_nodes.extend(downsample_preprocess_nodes)
+
+    composable_nodes.append(
         ComposableNode(
             package="probabilistic_occupancy_grid_map",
             plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode",
@@ -46,11 +101,15 @@ def launch_setup(context, *args, **kwargs):
             remappings=[
                 (
                     "~/input/obstacle_pointcloud",
-                    LaunchConfiguration("input/obstacle_pointcloud"),
+                    LaunchConfiguration("input/obstacle_pointcloud")
+                    if not downsample_input_pointcloud
+                    else "/perception/obstacle_segmentation/downsample/pointcloud",
                 ),
                 (
                     "~/input/raw_pointcloud",
-                    LaunchConfiguration("input/raw_pointcloud"),
+                    LaunchConfiguration("input/raw_pointcloud")
+                    if not downsample_input_pointcloud
+                    else "/sensing/lidar/concatenated/downsample/pointcloud",
                 ),
                 ("~/output/occupancy_grid_map", LaunchConfiguration("output")),
             ],
@@ -60,8 +119,8 @@ def launch_setup(context, *args, **kwargs):
                 {"updater_type": LaunchConfiguration("updater_type")},
             ],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
-        ),
-    ]
+        )
+    )
 
     occupancy_grid_map_container = ComposableNodeContainer(
         name=LaunchConfiguration("individual_container_name"),
diff --git a/perception/probabilistic_occupancy_grid_map/package.xml b/perception/probabilistic_occupancy_grid_map/package.xml
index a90ae78b03597..3efed07535504 100644
--- a/perception/probabilistic_occupancy_grid_map/package.xml
+++ b/perception/probabilistic_occupancy_grid_map/package.xml
@@ -34,6 +34,7 @@
   <depend>tier4_autoware_utils</depend>
   <depend>visualization_msgs</depend>
 
+  <exec_depend>pointcloud_preprocessor</exec_depend>
   <exec_depend>pointcloud_to_laserscan</exec_depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
@@ -44,6 +45,7 @@
   <test_depend>launch_testing</test_depend>
   <test_depend>launch_testing_ament_cmake</test_depend>
   <test_depend>launch_testing_ros</test_depend>
+  <test_depend>pointcloud_preprocessor</test_depend>
   <export>
     <build_type>ament_cmake</build_type>
   </export>
diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp
index e7a51028a1159..24bc0f2f059fc 100644
--- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp
+++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp
@@ -162,9 +162,16 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw(
   // Filter obstacle pointcloud by raw pointcloud
   PointCloud2 filtered_obstacle_pc_common{};
   if (filter_obstacle_pointcloud_by_raw_pointcloud_) {
-    if (!utils::extractCommonPointCloud(
-          filtered_obstacle_pc, filtered_raw_pc, filtered_obstacle_pc_common)) {
-      filtered_obstacle_pc_common = filtered_obstacle_pc;
+    const bool use_exact_common_pointcloud = true;
+    const bool success_to_filter =
+      use_exact_common_pointcloud
+        ? utils::extractCommonPointCloud(
+            filtered_obstacle_pc, filtered_raw_pc, filtered_obstacle_pc_common)
+        : utils::extractApproximateCommonPointCloud(
+            filtered_obstacle_pc, filtered_raw_pc, 0.15f, filtered_obstacle_pc_common);
+    if (!success_to_filter) {
+      // RCLCPP_WARN_STREAM(get_logger(), "pointcloud filtering is failed in
+      // filter_obstacle_pointcloud_by_raw_pointcloud.");
     }
   } else {
     filtered_obstacle_pc_common = filtered_obstacle_pc;
@@ -190,8 +197,13 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw(
   occupancy_grid_map_ptr_->updateOrigin(
     gridmap_origin.position.x - occupancy_grid_map_ptr_->getSizeInMetersX() / 2,
     gridmap_origin.position.y - occupancy_grid_map_ptr_->getSizeInMetersY() / 2);
-  occupancy_grid_map_ptr_->updateWithPointCloud(
-    filtered_raw_pc, filtered_obstacle_pc_common, robot_pose, scan_origin);
+  // check the raw/obstacle pointcloud size
+  if (filtered_raw_pc.data.size() == 0 || filtered_obstacle_pc_common.data.size() == 0) {
+    RCLCPP_WARN_STREAM(get_logger(), "pointcloud size is 0");
+  } else {
+    occupancy_grid_map_ptr_->updateWithPointCloud(
+      filtered_raw_pc, filtered_obstacle_pc_common, robot_pose, scan_origin);
+  }
 
   if (enable_single_frame_mode_) {
     // publish
diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp
index b876004d89b0a..412a8887eaef8 100644
--- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp
+++ b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp
@@ -18,6 +18,35 @@
 
 #include <string>
 
+namespace
+{
+/**
+ * @brief Hash function for voxel keys.
+ * Utilizes prime numbers to calculate a unique hash for each voxel key.
+ */
+struct VoxelKeyHash
+{
+  std::size_t operator()(const std::array<int, 3> & k) const
+  {
+    // Primes based on the following paper: 'Investigating the Use of Primes in Hashing for
+    // Volumetric Data'.
+    return (k[0] * 73856093 ^ k[1] * 19349663 ^ k[2] * 83492791);
+  }
+};
+
+/**
+ * @brief Equality function for voxel keys.
+ * Checks if two voxel keys are equal.
+ */
+struct VoxelKeyEqual
+{
+  bool operator()(const std::array<int, 3> & a, const std::array<int, 3> & b) const
+  {
+    return a == b;
+  }
+};
+}  // namespace
+
 namespace utils
 {
 
@@ -182,6 +211,116 @@ bool extractCommonPointCloud(
   return true;
 }
 
+/**
+ * @brief extract Common Pointcloud between obstacle pc and raw pc
+ * @param obstacle_pc
+ * @param raw_pc
+ * @param voxel_size
+ * @param output_obstacle_pc
+ */
+bool extractApproximateCommonPointCloud(
+  const sensor_msgs::msg::PointCloud2 & obstacle_pc, const sensor_msgs::msg::PointCloud2 & raw_pc,
+  const float voxel_size, sensor_msgs::msg::PointCloud2 & output_obstacle_pc)
+{
+  using VoxelKey = std::array<int, 3>;
+  std::unordered_map<VoxelKey, size_t, VoxelKeyHash, VoxelKeyEqual> obstacle_voxel_map;
+  std::unordered_map<VoxelKey, size_t, VoxelKeyHash, VoxelKeyEqual> raw_voxel_map;
+
+  constexpr float large_num_offset = 100000.0;
+  const float & voxel_size_x = voxel_size;
+  const float & voxel_size_y = voxel_size;
+  const float & voxel_size_z = voxel_size;
+  const float inverse_voxel_size_x = 1.0 / voxel_size_x;
+  const float inverse_voxel_size_y = 1.0 / voxel_size_y;
+  const float inverse_voxel_size_z = 1.0 / voxel_size_z;
+
+  {
+    const int x_offset = raw_pc.fields[pcl::getFieldIndex(raw_pc, "x")].offset;
+    const int y_offset = raw_pc.fields[pcl::getFieldIndex(raw_pc, "y")].offset;
+    const int z_offset = raw_pc.fields[pcl::getFieldIndex(raw_pc, "z")].offset;
+
+    // Process each point in the point cloud
+    for (size_t global_offset = 0; global_offset + raw_pc.point_step <= raw_pc.data.size();
+         global_offset += raw_pc.point_step) {
+      const float & x = *reinterpret_cast<const float *>(&raw_pc.data[global_offset + x_offset]);
+      const float & y = *reinterpret_cast<const float *>(&raw_pc.data[global_offset + y_offset]);
+      const float & z = *reinterpret_cast<const float *>(&raw_pc.data[global_offset + z_offset]);
+
+      // The reason for adding a large value is that when converting from float to int, values
+      // around -1 to 1 are all rounded down to 0. Therefore, to prevent the numbers from becoming
+      // negative, a large value is added. It has been tuned to reduce computational costs, and
+      // deliberately avoids using round or floor functions.
+      VoxelKey key = {
+        static_cast<int>((x + large_num_offset) * inverse_voxel_size_x),
+        static_cast<int>((y + large_num_offset) * inverse_voxel_size_y),
+        static_cast<int>((z + large_num_offset) * inverse_voxel_size_z)};
+
+      if (raw_voxel_map.find(key) == raw_voxel_map.end()) {
+        raw_voxel_map[key] = global_offset;
+      }
+    }
+  }
+  {
+    const int x_offset = obstacle_pc.fields[pcl::getFieldIndex(obstacle_pc, "x")].offset;
+    const int y_offset = obstacle_pc.fields[pcl::getFieldIndex(obstacle_pc, "y")].offset;
+    const int z_offset = obstacle_pc.fields[pcl::getFieldIndex(obstacle_pc, "z")].offset;
+
+    for (size_t global_offset = 0;
+         global_offset + obstacle_pc.point_step <= obstacle_pc.data.size();
+         global_offset += obstacle_pc.point_step) {
+      const float & x =
+        *reinterpret_cast<const float *>(&obstacle_pc.data[global_offset + x_offset]);
+      const float & y =
+        *reinterpret_cast<const float *>(&obstacle_pc.data[global_offset + y_offset]);
+      const float & z =
+        *reinterpret_cast<const float *>(&obstacle_pc.data[global_offset + z_offset]);
+
+      // The reason for adding a large value is that when converting from float to int, values
+      // around -1 to 1 are all rounded down to 0. Therefore, to prevent the numbers from becoming
+      // negative, a large value is added. It has been tuned to reduce computational costs, and
+      // deliberately avoids using round or floor functions.
+      VoxelKey key = {
+        static_cast<int>((x + large_num_offset) * inverse_voxel_size_x),
+        static_cast<int>((y + large_num_offset) * inverse_voxel_size_y),
+        static_cast<int>((z + large_num_offset) * inverse_voxel_size_z)};
+
+      if (raw_voxel_map.find(key) == raw_voxel_map.end()) {
+        obstacle_voxel_map[key] = global_offset;
+      }
+    }
+
+    // Populate the output point cloud
+    size_t output_global_offset = 0;
+    output_obstacle_pc.data.resize(obstacle_voxel_map.size() * obstacle_pc.point_step);
+    for (const auto & kv : obstacle_voxel_map) {
+      std::memcpy(
+        &output_obstacle_pc.data[output_global_offset + x_offset],
+        &obstacle_pc.data[kv.second + x_offset], sizeof(float));
+      std::memcpy(
+        &output_obstacle_pc.data[output_global_offset + y_offset],
+        &obstacle_pc.data[kv.second + y_offset], sizeof(float));
+      std::memcpy(
+        &output_obstacle_pc.data[output_global_offset + z_offset],
+        &obstacle_pc.data[kv.second + z_offset], sizeof(float));
+      output_global_offset += obstacle_pc.point_step;
+    }
+
+    // Set the output point cloud metadata
+    output_obstacle_pc.header.frame_id = obstacle_pc.header.frame_id;
+    output_obstacle_pc.height = 1;
+    output_obstacle_pc.fields = obstacle_pc.fields;
+    output_obstacle_pc.is_bigendian = obstacle_pc.is_bigendian;
+    output_obstacle_pc.point_step = obstacle_pc.point_step;
+    output_obstacle_pc.is_dense = obstacle_pc.is_dense;
+    output_obstacle_pc.width = static_cast<uint32_t>(
+      output_obstacle_pc.data.size() / output_obstacle_pc.height / output_obstacle_pc.point_step);
+    output_obstacle_pc.row_step =
+      static_cast<uint32_t>(output_obstacle_pc.data.size() / output_obstacle_pc.height);
+  }
+
+  return true;
+}
+
 /**
  * @brief Convert unsigned char value to closest cost value
  * @param cost Cost value