feat(map): add pointcloud divider and pointcloud merger (#100)

* Added point cloud merger Signed-off-by: Anh Nguyen <[email protected]> * Update README and remove unused svg file Signed-off-by: Anh Nguyen <[email protected]> * style(pre-commit): autofix * Fix pre-commit Signed-off-by: Anh Nguyen <[email protected]> * style(pre-commit): autofix * Fix spell check Signed-off-by: Anh Nguyen <[email protected]> * style(pre-commit): autofix * Fix pre-commit ci Signed-off-by: Anh Nguyen <[email protected]> * Fix pre-commit ci Signed-off-by: Anh Nguyen <[email protected]> * Fix pre-commit ci Signed-off-by: Anh Nguyen <[email protected]> * Add autoware_ prefix to nodes Signed-off-by: Anh Nguyen <[email protected]> * style(pre-commit): autofix * Add autoware_ prefix to pointcloud merger and divider; refactor code; rearrange files Signed-off-by: Anh Nguyen <[email protected]> * Follow autoware directory structure Signed-off-by: Anh Nguyen <[email protected]> * Move parameters to yaml files, create schema json files, and apply clang format Signed-off-by: Anh Nguyen <[email protected]> * Fix an error while reading PCD file Signed-off-by: Anh Nguyen <[email protected]> * style(pre-commit): autofix * Fix pre-commit ci Signed-off-by: Anh Nguyen <[email protected]> * Fix pre-commit-optional Signed-off-by: Anh Nguyen <[email protected]> * style(pre-commit): autofix * Fix merger's schema file Signed-off-by: Anh Nguyen <[email protected]> * style(pre-commit): autofix * Fixed review comments Signed-off-by: Anh Nguyen <[email protected]> * style(pre-commit): autofix * Fixed spell check Signed-off-by: Anh Nguyen <[email protected]> --------- Signed-off-by: Anh Nguyen <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yamato Ando <[email protected]>
autowarefoundation · Sep 4, 2024 · 7564237 · 7564237
1 parent 584be31
commit 7564237
Show file tree

Hide file tree

Showing 34 changed files with 3,848 additions and 0 deletions.
diff --git a/map/autoware_pointcloud_divider/CMakeLists.txt b/map/autoware_pointcloud_divider/CMakeLists.txt
@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_pointcloud_divider)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+ament_auto_find_build_dependencies()
+
+# Enable support for C++17
+if (${CMAKE_VERSION} VERSION_LESS "3.1.0")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17")
+else ()
+  set(CMAKE_CXX_STANDARD 17)
+endif ()
+
+# Find packages
+find_package(yaml-cpp REQUIRED)
+find_package(PCL REQUIRED COMPONENTS common io filters)
+
+include_directories(include)
+
+# Add divider library
+ament_auto_add_library(${PROJECT_NAME} SHARED src/pointcloud_divider_node.cpp src/voxel_grid_filter.cpp src/pcd_divider.cpp)
+target_link_libraries(${PROJECT_NAME} yaml-cpp ${PCL_LIBRARIES})
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::pointcloud_divider::PointCloudDivider"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+install(TARGETS ${PROJECT_NAME}
+        EXPORT ${PROJECT_NAME}
+        RUNTIME DESTINATION bin
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        INCLUDES DESTINATION include
+        )
+
+ament_auto_package(INSTALL_TO_SHARE launch config)
diff --git a/map/autoware_pointcloud_divider/LICENSE b/map/autoware_pointcloud_divider/LICENSE
@@ -0,0 +1,28 @@
+BSD 3-Clause License
+
+Copyright (c) 2023, MAP IV
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its
+   contributors may be used to endorse or promote products derived from
+   this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/map/autoware_pointcloud_divider/README.md b/map/autoware_pointcloud_divider/README.md
@@ -0,0 +1,100 @@
+# autoware_pointcloud_divider
+
+This is a tool for processing pcd files, and it can perform the following functions:
+
+- Dividing point clouds
+- Downsampling point clouds
+- Generating metadata to efficiently handle the divided point clouds
+
+## Supported Data Format
+
+**Currently, only `pcl::PointXYZ` and `pcl::PointXYZI` are supported. Any PCD will be loaded as those two types.**
+
+This tool can be used with files that have data fields other than `XYZI` (e.g., `XYZRGB`) and files that only contain `XYZ`.
+
+- Data fields other than `XYZI` are ignored during loading.
+- When loading `XYZ`-only data, the `intensity` field is assigned 0.
+
+## Installation
+
+```bash
+cd <PATH_TO_pilot-auto.*> # OR <PATH_TO_autoware>
+cd src/
+git clone [email protected]:autowarefoundation/autoware_tools.git
+cd ..
+colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-skip-building-tests --symlink-install --packages-up-to autoware_pointcloud_divider
+```
+
+## Usage
+
+- Select directory, process all files found with `find $INPUT_DIR -name "*.pcd"`.
+
+  ```bash
+  ros2 launch autoware_pointcloud_divider pointcloud_divider.launch.xml input_pcd_or_dir:=<INPUT_DIR> output_pcd_dir:=<OUTPUT_DIR> prefix:=<PREFIX>
+  ```
+
+  | Name       | Description                           |
+  | ---------- | ------------------------------------- |
+  | INPUT_DIR  | Directory that contains all PCD files |
+  | OUTPUT_DIR | Output directory name                 |
+  | PREFIX     | Prefix of output PCD file name        |
+
+`INPUT_DIR` and `OUTPUT_DIR` should be specified as **absolute paths**.
+
+NOTE: The folder `OUTPUT_DIR` is auto generated. If it already exists, all files within that folder will be deleted before the tool runs. Hence, users should backup the important files in that folder if necessary.
+
+### Parameters
+
+{{ json_to_markdown("map/autoware_pointcloud_divider/schema/pointcloud_divider.schema.json") }}
+
+How the point cloud is processed.
+
+![node_diagram](docs/pcd_divider.drawio.svg)
+
+How the PCD file is named
+
+![node_diagram](docs/output_file_name_pattern.drawio.svg)
+
+### Parameter example
+
+1. Dividing point clouds without downsampling
+
+   ```yaml
+   use_large_grid: false
+   leaf_size: -1.0 # any negative number
+   grid_size_x: 20
+   grid_size_y: 20
+   ```
+
+2. Dividing and downsampling point clouds
+
+   ```yaml
+   use_large_grid: false
+   leaf_size: 0.2
+   grid_size_x: 20
+   grid_size_y: 20
+   ```
+
+## Metadata YAML Format
+
+The metadata file should be named `metadata.yaml`. It contains the following fields:
+
+- `x_resolution`: The resolution along the X-axis.
+- `y_resolution`: The resolution along the Y-axis.
+
+Additionally, the file contains entries for individual point cloud files (`.pcd` files) and their corresponding grid coordinates. The key is the file name, and the value is a list containing the X and Y coordinates of the lower-left corner of the grid cell associated with that file. The grid cell's boundaries can be calculated using the `x_resolution` and `y_resolution` values.
+
+For example:
+
+```yaml
+x_resolution: 100.0
+y_resolution: 150.0
+A.pcd: [1200, 2500] # -> 1200 <= x <= 1300, 2500 <= y <= 2650
+B.pcd: [1300, 2500] # -> 1300 <= x <= 1400, 2500 <= y <= 2650
+C.pcd: [1200, 2650] # -> 1200 <= x <= 1300, 2650 <= y <= 2800
+D.pcd: [1400, 2650] # -> 1400 <= x <= 1500, 2650 <= y <= 2800
+```
+
+## LICENSE
+
+Parts of files grid_info.hpp, pcd_divider.hpp, and pcd_divider.cpp are copied from [MapIV's pointcloud_divider](https://github.com/MapIV/pointcloud_divider) and are under [BSD-3-Clauses](LICENSE) license. The remaining code are under [Apache License 2.0](../../LICENSE)
diff --git a/map/autoware_pointcloud_divider/config/pointcloud_divider.param.yaml b/map/autoware_pointcloud_divider/config/pointcloud_divider.param.yaml
@@ -0,0 +1,10 @@
+/**:
+  ros__parameters:
+    use_large_grid: false
+    leaf_size: -0.1
+    grid_size_x: 20.0
+    grid_size_y: 20.0
+    input_pcd_or_dir: $(var input_pcd_or_dir) # Path to the folder containing the input PCD files
+    output_pcd_dir: $(var output_pcd_dir) # Path to the folder containing the segmented PCD files
+    prefix: $(var prefix) # Prefix for the name of the output PCD files
+    point_type: "point_xyzi"
diff --git a/map/autoware_pointcloud_divider/docs/output_file_name_pattern.drawio.svg b/map/autoware_pointcloud_divider/docs/output_file_name_pattern.drawio.svg
diff --git a/map/autoware_pointcloud_divider/docs/pcd_divider.drawio.svg b/map/autoware_pointcloud_divider/docs/pcd_divider.drawio.svg
diff --git a/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/centroid.hpp b/map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/centroid.hpp
@@ -0,0 +1,116 @@
+// Copyright 2024 Autoware Foundation
+//
+// 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_DIVIDER__CENTROID_HPP_
+#define AUTOWARE__POINTCLOUD_DIVIDER__CENTROID_HPP_
+
+#include "utility.hpp"
+
+#include <pcl/point_types.h>
+
+#include <iostream>
+
+namespace autoware::pointcloud_divider
+{
+
+template <typename PointT>
+void accumulate(const PointT & p, const PointT & first_p, PointT & acc_diff);
+template <typename PointT>
+void compute_centroid(
+  const PointT & acc_diff, const PointT & first_p, size_t point_num, PointT & centroid);
+
+template <>
+void accumulate(const pcl::PointXYZ & p, const pcl::PointXYZ & first_p, pcl::PointXYZ & acc_diff)
+{
+  acc_diff.x += p.x - first_p.x;
+  acc_diff.y += p.y - first_p.y;
+  acc_diff.z += p.z - first_p.z;
+}
+
+template <>
+void accumulate(const pcl::PointXYZI & p, const pcl::PointXYZI & first_p, pcl::PointXYZI & acc_diff)
+{
+  acc_diff.x += p.x - first_p.x;
+  acc_diff.y += p.y - first_p.y;
+  acc_diff.z += p.z - first_p.z;
+  acc_diff.intensity += p.intensity - first_p.intensity;
+}
+
+template <>
+void compute_centroid(
+  const pcl::PointXYZ & acc_diff, const pcl::PointXYZ & first_p, size_t point_num,
+  pcl::PointXYZ & centroid)
+{
+  double double_point_num = static_cast<double>(point_num);
+
+  centroid.x = acc_diff.x / double_point_num + first_p.x;
+  centroid.y = acc_diff.y / double_point_num + first_p.y;
+  centroid.z = acc_diff.z / double_point_num + first_p.z;
+}
+
+template <>
+void compute_centroid(
+  const pcl::PointXYZI & acc_diff, const pcl::PointXYZI & first_p, size_t point_num,
+  pcl::PointXYZI & centroid)
+{
+  double double_point_num = static_cast<double>(point_num);
+
+  centroid.x = acc_diff.x / double_point_num + first_p.x;
+  centroid.y = acc_diff.y / double_point_num + first_p.y;
+  centroid.z = acc_diff.z / double_point_num + first_p.z;
+  centroid.intensity = acc_diff.intensity / double_point_num + first_p.intensity;
+}
+
+template <typename PointT>
+struct Centroid
+{
+  Centroid()
+  {
+    point_num_ = 0;
+    util::zero_point(acc_diff_);
+    util::zero_point(first_point_);
+  }
+
+  void add(const PointT & p)
+  {
+    if (point_num_ == 0) {
+      first_point_ = p;
+    } else {
+      accumulate(p, first_point_, acc_diff_);
+    }
+
+    ++point_num_;
+  }
+
+  PointT get()
+  {
+    PointT centroid;
+
+    if (point_num_ > 0) {
+      compute_centroid(acc_diff_, first_point_, point_num_, centroid);
+    } else {
+      std::cerr << "Error: there is no point in the centroid group!" << std::endl;
+      exit(EXIT_FAILURE);
+    }
+
+    return centroid;
+  }
+
+  PointT acc_diff_, first_point_;
+  size_t point_num_;
+};
+
+}  // namespace autoware::pointcloud_divider
+
+#endif  // AUTOWARE__POINTCLOUD_DIVIDER__CENTROID_HPP_