feat(autoware_pointcloud_preprocessor): add pointcloud_densifier package

sensing/autoware_pointcloud_preprocessor/CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(Sophus REQUIRED)
 find_package(Boost REQUIRED)
 find_package(PCL REQUIRED)
 find_package(CGAL REQUIRED COMPONENTS Core)
+find_package(tf2_sensor_msgs REQUIRED)
 
 include_directories(
   include
@@ -90,6 +91,8 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED
   src/polygon_remover/polygon_remover.cpp
   src/vector_map_filter/vector_map_inside_area_filter_node.cpp
   src/utility/geometry.cpp
+  src/pointcloud_densifier/pointcloud_densifier_node.cpp
+  src/pointcloud_densifier/occupancy_grid.cpp
 )
 
 target_link_libraries(pointcloud_preprocessor_filter
@@ -99,6 +102,7 @@ target_link_libraries(pointcloud_preprocessor_filter
   ${OpenCV_LIBRARIES}
   ${Sophus_LIBRARIES}
   ${PCL_LIBRARIES}
+  ${tf2_sensor_msgs_LIBRARIES}
 )
 
 # ========== Time synchronizer ==========
@@ -206,6 +210,17 @@ rclcpp_components_register_node(pointcloud_preprocessor_filter
   PLUGIN "autoware::pointcloud_preprocessor::VectorMapInsideAreaFilterComponent"
   EXECUTABLE vector_map_inside_area_filter_node)
 
+# ========== Pointcloud Densifier ==========
+rclcpp_components_register_node(pointcloud_preprocessor_filter
+  PLUGIN "autoware::pointcloud_preprocessor::PointCloudDensifierNode"
+  EXECUTABLE pointcloud_densifier_node)
+
+# Make sure launch directory is installed
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
 install(
   TARGETS pointcloud_preprocessor_filter_base EXPORT export_${PROJECT_NAME}
   ARCHIVE DESTINATION lib

sensing/autoware_pointcloud_preprocessor/README.md

@@ -9,6 +9,7 @@ The `autoware_pointcloud_preprocessor` is a package that includes the following
 - concatenating pointclouds
 - correcting distortion
 - downsampling
+- densifying pointclouds
 
 ## Inner-workings / Algorithms
 
@@ -23,6 +24,7 @@ Detail description of each filter's algorithm is in the following links.
 | outlier_filter                | remove points caused by hardware problems, rain drops and small insects as a noise | [link](docs/outlier-filter.md)                |
 | passthrough_filter            | remove points on the outside of a range in given field (e.g. x, y, z, intensity)   | [link](docs/passthrough-filter.md)            |
 | pointcloud_accumulator        | accumulate pointclouds for a given amount of time                                  | [link](docs/pointcloud-accumulator.md)        |
+| pointcloud_densifier          | enhance sparse point clouds by using information from previous frames              | [link](docs/pointcloud-densifier.md)          |
 | vector_map_filter             | remove points on the outside of lane by using vector map                           | [link](docs/vector-map-filter.md)             |
 | vector_map_inside_area_filter | remove points inside of vector map area that has given type by parameter           | [link](docs/vector-map-inside-area-filter.md) |
 

sensing/autoware_pointcloud_preprocessor/config/pointcloud_densifier.param.yaml

@@ -0,0 +1,8 @@
+/**:
+  ros__parameters:
+    num_previous_frames: 1
+    x_min: 80.0
+    x_max: 230.0
+    y_min: -20.0
+    y_max: 20.0
+    grid_resolution: 0.3

sensing/autoware_pointcloud_preprocessor/docs/pointcloud-densifier.md

@@ -0,0 +1,62 @@
+# PointCloud Densifier
+
+## Purpose
+
+The `pointcloud_densifier` enhances sparse point cloud data by leveraging information from previous LiDAR frames,
+creating a denser representation especially for long-range points. This is particularly useful for improving perception
+of distant objects where LiDAR data tends to be sparse.
+
+## Inner-workings / Algorithm
+
+The algorithm works as follows:
+
+1. **ROI Filtering**: First filters the input point cloud to only keep points in a specific region of interest (ROI),
+   typically focused on the distant area in front of the vehicle.
+
+2. **Occupancy Grid Creation**: Creates a 2D occupancy grid from the filtered points to track which areas contain valid
+   points in the current frame.
+
+3. **Previous Frame Integration**: Transforms points from previous frames into the current frame's coordinate system
+   using TF transformations.
+
+4. **Selective Point Addition**: Adds points from previous frames only if they fall into grid cells that are occupied
+   in the current frame. This ensures that only relevant points are added, avoiding ghost points from dynamic objects.
+
+5. **Combined Output**: Returns a combined point cloud that includes both the current frame's points and selected
+   points from previous frames.
+
+## Inputs / Outputs
+
+### Input
+
+| Name    | Type                            | Description       |
+| ------- | ------------------------------- | ----------------- |
+| `input` | `sensor_msgs::msg::PointCloud2` | Input point cloud |
+
+### Output
+
+| Name     | Type                            | Description                  |
+| -------- | ------------------------------- | ---------------------------- |
+| `output` | `sensor_msgs::msg::PointCloud2` | Densified point cloud output |
+
+## Parameters
+
+| Name                  | Type   | Default Value | Description                            |
+| --------------------- | ------ | ------------- | -------------------------------------- |
+| `num_previous_frames` | int    | 1             | Number of previous frames to consider  |
+| `x_min`               | double | 80.0          | Minimum x coordinate of ROI in meters  |
+| `x_max`               | double | 200.0         | Maximum x coordinate of ROI in meters  |
+| `y_min`               | double | -20.0         | Minimum y coordinate of ROI in meters  |
+| `y_max`               | double | 20.0          | Maximum y coordinate of ROI in meters  |
+| `grid_resolution`     | double | 0.3           | Resolution of occupancy grid in meters |
+
+## Assumptions / Known limits
+
+- The filter assumes that the TF tree contains valid transformations between coordinate frames from previous point clouds to the current frame.
+- Performance depends on the number of previous frames used - more frames increase density but also processing time.
+- The filter performs best on static elements in the scene, as dynamic objects may create artifacts if they move between frames.
+- The accuracy of the densification depends on the quality of the TF transformations and ego-vehicle motion estimation.
+
+## Usage
+
+The pointcloud_densifier can be launched using:

sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/occupancy_grid.hpp

@@ -0,0 +1,48 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__POINTCLOUD_DENSIFIER__OCCUPANCY_GRID_HPP_
+#define AUTOWARE__POINTCLOUD_PREPROCESSOR__POINTCLOUD_DENSIFIER__OCCUPANCY_GRID_HPP_
+
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "sensor_msgs/point_cloud2_iterator.hpp"
+
+#include <vector>
+
+namespace autoware::pointcloud_preprocessor
+{
+
+class OccupancyGrid
+{
+public:
+  OccupancyGrid(double x_min, double x_max, double y_min, double y_max, double resolution);
+  void updateOccupancy(const sensor_msgs::msg::PointCloud2 & cloud);
+  bool isOccupied(double x, double y) const;
+
+private:
+  double x_min_;
+  double x_max_;
+  double y_min_;
+  double y_max_;
+  double resolution_;
+  size_t cols_;
+  size_t rows_;
+  std::vector<bool> grid_;
+
+  size_t index(size_t row, size_t col) const;
+};
+
+}  // namespace autoware::pointcloud_preprocessor
+
+#endif  // AUTOWARE__POINTCLOUD_PREPROCESSOR__POINTCLOUD_DENSIFIER__OCCUPANCY_GRID_HPP_

sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/pointcloud_densifier_node.hpp

@@ -0,0 +1,80 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__POINTCLOUD_DENSIFIER__POINTCLOUD_DENSIFIER_NODE_HPP_
+#define AUTOWARE__POINTCLOUD_PREPROCESSOR__POINTCLOUD_DENSIFIER__POINTCLOUD_DENSIFIER_NODE_HPP_
+
+#include "autoware/pointcloud_preprocessor/filter.hpp"
+#include "autoware/pointcloud_preprocessor/pointcloud_densifier/occupancy_grid.hpp"
+#include "autoware/pointcloud_preprocessor/transform_info.hpp"
+
+#include <deque>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace autoware::pointcloud_preprocessor
+{
+
+class PointCloudDensifierNode : public Filter
+{
+protected:
+  void filter(
+    const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override;
+
+  void faster_filter(
+    const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output,
+    const TransformInfo & transform_info) override;
+
+private:
+  sensor_msgs::msg::PointCloud2::SharedPtr filterPointCloudByROI(
+    const PointCloud2ConstPtr & input_cloud, const IndicesPtr & indices = nullptr);
+
+  void transformAndMergePreviousClouds(
+    const PointCloud2ConstPtr & current_msg, const OccupancyGrid & occupancy_grid,
+    PointCloud2 & combined_cloud);
+
+  void storeCurrentCloud(const sensor_msgs::msg::PointCloud2::SharedPtr & filtered_cloud);
+
+  bool isValidTransform(const Eigen::Matrix4d & transform) const;
+
+  struct DensifierParam
+  {
+    int num_previous_frames{1};
+    double x_min{80.0};
+    double x_max{200.0};
+    double y_min{-20.0};
+    double y_max{20.0};
+    double grid_resolution{0.3};
+  } param_;
+
+  std::unique_ptr<autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<autoware_utils::DebugPublisher> debug_publisher_;
+
+  std::deque<sensor_msgs::msg::PointCloud2::SharedPtr> previous_pointclouds_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+  rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector<rclcpp::Parameter> & p);
+
+public:
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+  explicit PointCloudDensifierNode(const rclcpp::NodeOptions & options);
+};
+
+}  // namespace autoware::pointcloud_preprocessor
+
+#endif  // AUTOWARE__POINTCLOUD_PREPROCESSOR__POINTCLOUD_DENSIFIER__POINTCLOUD_DENSIFIER_NODE_HPP_

sensing/autoware_pointcloud_preprocessor/launch/pointcloud_densifier.launch.py

@@ -0,0 +1,72 @@
+# Copyright 2025 TIER IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+import launch
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+    """Launch the pointcloud densifier node."""
+    pkg_prefix = get_package_share_directory("autoware_pointcloud_preprocessor")
+    config_file = os.path.join(pkg_prefix, "config/pointcloud_densifier.param.yaml")
+
+    input_topic = DeclareLaunchArgument(
+        "input_topic",
+        default_value="/sensing/lidar/concatenated/pointcloud",
+        description="Input pointcloud topic",
+    )
+
+    output_topic = DeclareLaunchArgument(
+        "output_topic",
+        default_value="/sensing/lidar/densified/pointcloud",
+        description="Output pointcloud topic",
+    )
+
+    # Create the composable node
+    component = ComposableNode(
+        package="autoware_pointcloud_preprocessor",
+        plugin="autoware::pointcloud_preprocessor::PointCloudDensifierNode",
+        name="pointcloud_densifier",
+        remappings=[
+            ("input", LaunchConfiguration("input_topic")),
+            ("output", LaunchConfiguration("output_topic")),
+        ],
+        parameters=[config_file],
+    )
+
+    # Create container
+    container = ComposableNodeContainer(
+        name="pointcloud_densifier_container",
+        namespace="",
+        package="rclcpp_components",
+        executable="component_container",
+        composable_node_descriptions=[component],
+        output="screen",
+    )
+
+    return launch.LaunchDescription(
+        [
+            # Launch arguments
+            input_topic,
+            output_topic,
+            # Nodes
+            container,
+        ]
+    )

sensing/autoware_pointcloud_preprocessor/package.xml

@@ -14,6 +14,7 @@
   <maintainer email="david.wong@tier4.jp">David Wong</maintainer>
   <maintainer email="melike@leodrive.ai">Melike Tanrikulu</maintainer>
   <maintainer email="max.schmeller@tier4.jp">Max Schmeller</maintainer>
+  <maintainer email="kaan.colak.dev@gmail.com">Kaan Colak</maintainer>
   <license>Apache License 2.0</license>
   <!-- For polygon_remover, vector_map_filter -->
   <license>LGPLv3</license>

sensing/autoware_pointcloud_preprocessor/schema/pointcloud_densifier_node.schema.json

@@ -0,0 +1,41 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for PointCloud Densifier Node",
+  "type": "object",
+  "properties": {
+    "num_previous_frames": {
+      "type": "integer",
+      "description": "Number of previous frames to use for densification",
+      "default": 1,
+      "minimum": 0
+    },
+    "x_min": {
+      "type": "number",
+      "description": "Minimum X coordinate (in meters) of the region of interest",
+      "default": 80.0
+    },
+    "x_max": {
+      "type": "number",
+      "description": "Maximum X coordinate (in meters) of the region of interest",
+      "default": 230.0
+    },
+    "y_min": {
+      "type": "number",
+      "description": "Minimum Y coordinate (in meters) of the region of interest",
+      "default": -20.0
+    },
+    "y_max": {
+      "type": "number",
+      "description": "Maximum Y coordinate (in meters) of the region of interest",
+      "default": 20.0
+    },
+    "grid_resolution": {
+      "type": "number",
+      "description": "Resolution (in meters) of the occupancy grid used for point selection",
+      "default": 0.3,
+      "exclusiveMinimum": 0
+    }
+  },
+  "required": ["num_previous_frames", "x_min", "x_max", "y_min", "y_max", "grid_resolution"],
+  "additionalProperties": false
+}

sensing/autoware_pointcloud_preprocessor/src/filter.cpp

@@ -140,7 +140,8 @@ void autoware::pointcloud_preprocessor::Filter::subscribe(const std::string & fi
   // each time a child class supports the faster version.
   // When all the child classes support the faster version, this workaround is deleted.
   std::set<std::string> supported_nodes = {
-    "CropBoxFilter", "RingOutlierFilter", "VoxelGridDownsampleFilter", "ScanGroundFilter"};
+    "CropBoxFilter", "RingOutlierFilter", "VoxelGridDownsampleFilter", "ScanGroundFilter",
+    "PointCloudDensifier"};
   auto callback = supported_nodes.find(filter_name) != supported_nodes.end()
                     ? &Filter::faster_input_indices_callback
                     : &Filter::input_indices_callback;