diff --git a/map/autoware_pointcloud_map_validator/README.md b/map/autoware_pointcloud_map_validator/README.md new file mode 100644 index 00000000..4713b2b8 --- /dev/null +++ b/map/autoware_pointcloud_map_validator/README.md @@ -0,0 +1,46 @@ +# Autoware Pointcloud Map Validator + +Analyze and visualize point cloud resolution and point-to-point distances. + +This script provides comprehensive analysis tools for point cloud data, including: + +- Computing resolution (average point-to-point distance) using k-nearest neighbors +- Visualizing point clouds with color-coded distances +- Generating distance distribution histograms +- Supporting both single PCD files and directories of PCD files + +```yaml +Usage: + ros2 run autoware_pointcloud_map_validator autoware_pointcloud_checker [options] + # or + python pointcloud_check.py [options] + +Arguments: + input_file Path to a PCD file or directory containing PCD files + +Options: + --neighbors, -k Number of nearest neighbors to consider (default: 3) + --threshold Distance threshold for visualization (default: 0.2) + --cache Enable caching of computed distances to file + --output, -o Output path for saving colored point cloud + --histogram Show histogram of point-to-point distances + +Key Features: + +- Automatic merging of multiple point clouds when processing a directory +- Color-coded visualization (blue for distances below threshold, yellow for above) +- Statistical analysis including min, max, median, and standard deviation +- Interactive 3D visualization +- Distance distribution histogram with statistical annotations +- Distance computation result caching support for large point clouds + +Example Usage: + # Analyze a single PCD file with visualization and statistic output + ros2 run autoware_pointcloud_map_validator autoware_pointcloud_checker input.pcd --histogram + + # Process directory of PCD files with custom threshold + ros2 run autoware_pointcloud_map_validator autoware_pointcloud_checker path/to/pcds/ --threshold 0.3 + + # Save processed point cloud and cache results, re-run the command will directly utilize the cached results + ros2 run autoware_pointcloud_map_validator autoware_pointcloud_checker input.pcd --output processed.pcd --cache +``` diff --git a/map/autoware_pointcloud_map_validator/autoware_pointcloud_map_validator/__init__.py b/map/autoware_pointcloud_map_validator/autoware_pointcloud_map_validator/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/map/autoware_pointcloud_map_validator/autoware_pointcloud_map_validator/autoware_pointcloud_checker.py b/map/autoware_pointcloud_map_validator/autoware_pointcloud_map_validator/autoware_pointcloud_checker.py new file mode 100755 index 00000000..954ece43 --- /dev/null +++ b/map/autoware_pointcloud_map_validator/autoware_pointcloud_map_validator/autoware_pointcloud_checker.py @@ -0,0 +1,394 @@ +#!/usr/bin/env python3 + +# Copyright 2024 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. + +""" +Analyze and visualize point cloud resolution and point-to-point distances. + +This script provides comprehensive analysis tools for point cloud data, including: +- Computing resolution (average point-to-point distance) using k-nearest neighbors +- Visualizing point clouds with color-coded distances +- Generating distance distribution histograms +- Supporting both single PCD files and directories of PCD files + +Usage: + python pointcloud_check.py [options] + +Arguments: + input_file Path to a PCD file or directory containing PCD files + +Options: + --neighbors, -k Number of nearest neighbors to consider (default: 3) + --threshold Distance threshold for visualization (default: 0.2) + --cache Enable caching of computed distances to file + --output, -o Output path for saving colored point cloud + --histogram Show histogram of point-to-point distances + +Key Features: +- Automatic merging of multiple point clouds when processing a directory +- Color-coded visualization (blue for distances below threshold, yellow for above) +- Statistical analysis including min, max, median, and standard deviation +- Interactive 3D visualization +- Distance distribution histogram with statistical annotations +- Distance computation result caching support for large point clouds + +Example Usage: + # Analyze a single PCD file with visualization + python pointcloud_check.py input.pcd --histogram + + # Process directory of PCD files with custom threshold + python pointcloud_check.py path/to/pcds/ --threshold 0.3 + + # Save processed point cloud and cache results, re-run the command will directly utilize the cached results + python pointcloud_check.py input.pcd --output processed.pcd --cache +""" + +import argparse +import logging +import os +from pathlib import Path +from typing import List +from typing import Optional +from typing import Tuple + +import matplotlib.pyplot as plt +import numpy as np +import open3d as o3d +from tqdm import tqdm + +DEFAULT_K_NEIGHBORS = 3 +DEFAULT_THRESHOLD = 0.2 +DEFAULT_COLOR_IN_THRESHOLD = [0, 0.1, 0.5] +DEFAULT_COLOR_ABOVE_THRESHOLD = [1, 1, 0.2] +VISUALIZATION_WINDOW_WIDTH = 1024 +VISUALIZATION_WINDOW_HEIGHT = 768 + + +def setup_logger() -> logging.Logger: + """Configure logging settings.""" + logging.basicConfig(level=logging.INFO, format="%(asctime)s - %(levelname)s - %(message)s") + return logging.getLogger(__name__) + + +def colorize_pointcloud( + point_cloud: o3d.geometry.PointCloud, + distances: np.ndarray, + threshold: float = DEFAULT_THRESHOLD, + color_in_threshold: List[float] = DEFAULT_COLOR_IN_THRESHOLD, + color_above_threshold: List[float] = DEFAULT_COLOR_ABOVE_THRESHOLD, +) -> o3d.geometry.PointCloud: # Add return type hint + # Color points based on distance threshold + colors = np.zeros((len(distances), 3)) + colors[distances >= threshold] = color_above_threshold # White for distant points + colors[distances < threshold] = color_in_threshold # Blue for close points + + point_cloud.colors = o3d.utility.Vector3dVector(colors) + + return point_cloud + + +def visualize_pointcloud(point_cloud: o3d.geometry.PointCloud): + """ + Visualize a point cloud using Open3D's visualization utility. + + Args: + point_cloud (o3d.geometry.PointCloud): The point cloud to visualize. + Should be a valid Open3D PointCloud object. + + Note: + This function opens an interactive 3D visualization window where you can: + - Rotate the view using left mouse button + - Pan by pressing mouse wheel + - Zoom using mouse wheel + """ + o3d.visualization.draw_geometries( + [point_cloud], + window_name="Point Cloud Resolution Visualization", + width=VISUALIZATION_WINDOW_WIDTH, + height=VISUALIZATION_WINDOW_HEIGHT, + point_show_normal=False, + ) + + +def plot_distance_histogram( + distances: np.ndarray, threshold: float, range_limit: Tuple[float, float] = (0, 0.8) +) -> None: + """Plot a histogram showing the distribution of point-to-point distances with statistical annotations. + + This function creates a histogram visualization of point cloud distances, including key statistics + and a threshold line. The histogram shows frequency ratios rather than absolute counts. + + Args: + distances (np.ndarray): Array of point-to-point distances to be plotted. + Should contain float values representing distances in meters. + threshold (float): Distance threshold value to be shown as a vertical line + in the plot. Used to visualize a critical distance value. + range_limit (Tuple[float, float], optional): The range of distances to include + in the histogram, specified as (min, max) in meters. + Defaults to (0, 0.8). + + Returns: + None: Displays the plot using matplotlib.pyplot.show() + + Statistics Displayed: + - Minimum distance + - Maximum distance + - Median distance + - Standard deviation + + Example: + >>> distances = np.array([0.1, 0.2, 0.3, 0.4, 0.5]) + >>> plot_distance_histogram(distances, threshold=0.3) + """ + plt.figure(figsize=(10, 6)) + + # Calculate histogram with density=True to get frequency ratios + plt.hist(distances, bins=100, range=range_limit, density=True, stacked=True) + + # Add vertical line for threshold + plt.axvline(x=threshold, color="r", linestyle="--", label=f"Threshold ({threshold}m)") + + # Calculate statistics + min_dist = np.min(distances) + max_dist = np.max(distances) + median_dist = np.median(distances) + std_dist = np.std(distances) + ratio_above_threshold = np.sum(distances > threshold) / len(distances) + + # Create statistics text + stats_text = ( + f"Min: {min_dist:.6f}m\n" + f"Max: {max_dist:.6f}m\n" + f"Median: {median_dist:.6f}m\n" + f"Std Dev: {std_dist:.6f}m\n" + f"Above Thr: {ratio_above_threshold:.3f}" + ) + + # Add text box with statistics + # Position is in axes coordinates (0,0 is bottom left, 1,1 is top right) + plt.text( + 0.8, + 0.8, + stats_text, + transform=plt.gca().transAxes, + verticalalignment="top", + horizontalalignment="right", + # cspell: disable-next-line + bbox={"boxstyle": "round", "facecolor": "white", "alpha": 0.8}, + ) + + plt.title("Point-to-Point Distance Distribution") + plt.xlabel("Distance (m)") + plt.ylabel("Frequency Ratio") + plt.grid(True, alpha=0.3) + plt.legend() # Add legend to show threshold line label + plt.show() + + +def visualize_results(pcd, distances, args): + """ + Visualize analysis results including histogram of point-to-point distances and the point cloud itself. + + Args: + pcd (o3d.geometry.PointCloud): The point cloud to visualize + distances (numpy.ndarray): Array of point-to-point distances + args (argparse.Namespace): Command line arguments containing: + - histogram (bool): Whether to show distance distribution histogram + - threshold (float): Distance threshold value + - output (str): Path to save the point cloud (optional) + + The function can: + 1. Display a histogram of point-to-point distances (if args.histogram is True) + 2. Save the point cloud to a file (if args.output is provided) + 3. Show an interactive 3D visualization of the point cloud + """ + if args.histogram: + plot_distance_histogram(distances, args.threshold) + + if args.output: + o3d.io.write_point_cloud(args.output, pcd) + + visualize_pointcloud(pcd) + + +def load_point_clouds(folder_path: Path) -> List[o3d.geometry.PointCloud]: + """ + Load all PCD files from the specified directory. + + Args: + folder_path (Path): Path to the directory containing PCD files. + + Returns: + List[o3d.geometry.PointCloud]: List of loaded point cloud objects. + + Raises: + FileNotFoundError: If the specified directory doesn't exist. + """ + if not folder_path.exists(): + raise FileNotFoundError(f"Directory not found: {folder_path}") + + point_clouds = [] + logger = logging.getLogger(__name__) + + pcd_files = list(folder_path.glob("*.pcd")) + if not pcd_files: + logger.warning("No PCD files found in the specified directory.") + return point_clouds + + for file_path in pcd_files: + try: + pcd = o3d.io.read_point_cloud(str(file_path)) + logger.info(f"Loaded {file_path.name} with {len(pcd.points)} points") + point_clouds.append(pcd) + except Exception as e: + logger.error(f"Error loading {file_path}: {str(e)}") + + return point_clouds + + +def merge_point_clouds( + point_clouds: List[o3d.geometry.PointCloud], +) -> Optional[o3d.geometry.PointCloud]: + """ + Merge multiple point clouds into a single point cloud. + + Args: + point_clouds (List[o3d.geometry.PointCloud]): List of point clouds to merge. + + Returns: + Optional[o3d.geometry.PointCloud]: Merged point cloud or None if input list is empty. + """ + if not point_clouds: + return None + + merged_cloud = point_clouds[0] + for pc in point_clouds[1:]: + merged_cloud += pc + + return merged_cloud + + +def compute_resolution( + point_cloud: o3d.geometry.PointCloud, k: int = DEFAULT_K_NEIGHBORS +) -> Tuple[float, np.ndarray]: + """ + Compute the resolution of a point cloud using k-nearest neighbors. + + Args: + point_cloud (o3d.geometry.PointCloud): Input point cloud + k (int): Number of nearest neighbors to consider (default: 3) + + Returns: + Tuple[float, np.ndarray]: Tuple containing: + - Average resolution (mean distance between points) + - Array of distances for each point + + Note: + The resolution is calculated as the average distance between each point + and its k-1 nearest neighbors (excluding the point itself). + """ + kdtree = o3d.geometry.KDTreeFlann(point_cloud) + distances = [] + + # cspell: disable-next-line + for point in tqdm(point_cloud.points, dynamic_ncols=True): + # Find k nearest neighbors for each point + [_, idx, dists] = kdtree.search_knn_vector_3d(point, k + 1) + # Calculate average distance to neighbors (excluding self) + avg_distance = np.mean(np.sqrt(dists[1:])) + distances.append(avg_distance) + + distances = np.array(distances) + resolution = np.mean(distances) + + return resolution, distances + + +def main(): + parser = argparse.ArgumentParser(description="Compute the resolution of a point cloud file.") + parser.add_argument("input_file", type=str, help="Input PCD file path") + parser.add_argument( + "--neighbors", + "-k", + type=int, + default=3, + help="Number of nearest neighbors to consider (default: 3)", + ) + parser.add_argument( + "--threshold", + type=float, + default=0.2, + help="distance threshold of the pointcloud visualization", + ) + parser.add_argument( + "--cache", + action="store_true", + help="Cache the computed distance array into a temporary file, and will try to use cache", + ) + parser.add_argument( + "--output", "-o", type=str, help="Output path for saving colored point cloud" + ) + parser.add_argument( + "--histogram", + action="store_true", + help="Show histogram of point-to-point distances", + ) + + args = parser.parse_args() + logger = setup_logger() + + try: + input_path = Path(args.input_file) + if not input_path.exists(): + raise FileNotFoundError(f"File not found: {input_path}") + + # Load point cloud + if os.path.isdir(input_path): + logger.info( + f"{input_path} is a directory, will merge all the pcd files under the directory" + ) + pointclouds = load_point_clouds(input_path) + pcd = merge_point_clouds(pointclouds) + else: + logger.info( + f"{input_path} is a single pcd file, will directly load and analyze the pointcloud" + ) + pcd = o3d.io.read_point_cloud(str(input_path)) + logger.info(f"Loaded point cloud with {len(pcd.points)} points") + + if args.cache and os.path.isfile(f"{input_path.stem}_distances.npy"): + distances = np.load(f"{input_path.stem}_distances.npy") + else: + # Compute resolution + resolution, distances = compute_resolution(pcd, args.neighbors) + + if args.cache: + # Save distances to numpy file + np.save(f"{input_path.stem}_distances.npy", distances) + + logger.info(f"Point cloud resolution (average point distance): {resolution:.6f}") + + pcd = colorize_pointcloud(pcd, distances, args.threshold) + visualize_results(pcd, distances, args) + + return 0 + + except Exception as e: + logger.error(f"An error occurred: {str(e)}") + return 1 + + +if __name__ == "__main__": + exit(main()) diff --git a/map/autoware_pointcloud_map_validator/package.xml b/map/autoware_pointcloud_map_validator/package.xml new file mode 100644 index 00000000..ede74a2a --- /dev/null +++ b/map/autoware_pointcloud_map_validator/package.xml @@ -0,0 +1,35 @@ + + + + autoware_pointcloud_map_validator + 0.0.0 + The autoware_pointcloud_map_validator package + Yukinari Hisaki + Satoshi Ota + Kosuke Takeuchi + Shintaro Sakoda + Fumiya Watanabe + Yuxuan Liu + Anh Nguyen + Masahiro Sakamoto + Taiki Yamada + Yamato Ando + + Apache License 2.0 + + python3-matplotlib + python3-numpy + python3-open3d + python3-tqdm + rclpy + tier4_debug_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/map/autoware_pointcloud_map_validator/resource/autoware_pointcloud_map_validator b/map/autoware_pointcloud_map_validator/resource/autoware_pointcloud_map_validator new file mode 100644 index 00000000..e69de29b diff --git a/map/autoware_pointcloud_map_validator/setup.cfg b/map/autoware_pointcloud_map_validator/setup.cfg new file mode 100644 index 00000000..553a4650 --- /dev/null +++ b/map/autoware_pointcloud_map_validator/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/autoware_pointcloud_map_validator +[install] +install_scripts=$base/lib/autoware_pointcloud_map_validator diff --git a/map/autoware_pointcloud_map_validator/setup.py b/map/autoware_pointcloud_map_validator/setup.py new file mode 100644 index 00000000..597b9a25 --- /dev/null +++ b/map/autoware_pointcloud_map_validator/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages +from setuptools import setup + +package_name = "autoware_pointcloud_map_validator" + +setup( + name=package_name, + version="0.0.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ], + install_requires=["setuptools", "pyperclip"], + zip_safe=True, + maintainer="Yuxuan Liu", + maintainer_email="uken.ryu.2@tier4.jp", + description="The autoware_pointcloud_map_validator package", + license="Apache License 2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "autoware_pointcloud_checker = autoware_pointcloud_map_validator.autoware_pointcloud_checker:main", + ], + }, +)