diff --git a/Mapping/point_cloud_sampling/point_cloud_sampling.py b/Mapping/point_cloud_sampling/point_cloud_sampling.py new file mode 100644 index 00000000..df7cde41 --- /dev/null +++ b/Mapping/point_cloud_sampling/point_cloud_sampling.py @@ -0,0 +1,168 @@ +""" +Point cloud sampling example codes. This code supports +- Voxel point sampling +- Farthest point sampling +- Poisson disk sampling + +""" +import matplotlib.pyplot as plt +import numpy as np +import numpy.typing as npt +from collections import defaultdict + +do_plot = True + + +def voxel_point_sampling(original_points: npt.NDArray, voxel_size: float): + """ + Voxel Point Sampling function. + This function sample N-dimensional points with voxel grid. + Points in a same voxel grid will be merged by mean operation for sampling. + + Parameters + ---------- + original_points : (M, N) N-dimensional points for sampling. + The number of points is M. + voxel_size : voxel grid size + + Returns + ------- + sampled points (M', N) + """ + voxel_dict = defaultdict(list) + for i in range(original_points.shape[0]): + xyz = original_points[i, :] + xyz_index = tuple(xyz // voxel_size) + voxel_dict[xyz_index].append(xyz) + points = np.vstack([np.mean(v, axis=0) for v in voxel_dict.values()]) + return points + + +def farthest_point_sampling(orig_points: npt.NDArray, + n_points: int, seed: int): + """ + Farthest point sampling function + This function sample N-dimensional points with the farthest point policy. + + Parameters + ---------- + orig_points : (M, N) N-dimensional points for sampling. + The number of points is M. + n_points : number of points for sampling + seed : random seed number + + Returns + ------- + sampled points (n_points, N) + + """ + rng = np.random.default_rng(seed) + n_orig_points = orig_points.shape[0] + first_point_id = rng.choice(range(n_orig_points)) + min_distances = np.ones(n_orig_points) * float("inf") + selected_ids = [first_point_id] + while len(selected_ids) < n_points: + base_point = orig_points[selected_ids[-1], :] + distances = np.linalg.norm(orig_points[np.newaxis, :] - base_point, + axis=2).flatten() + min_distances = np.minimum(min_distances, distances) + distances_rank = np.argsort(-min_distances) # Farthest order + for i in distances_rank: # From the farthest point + if i not in selected_ids: # if not selected yes, select it + selected_ids.append(i) + break + return orig_points[selected_ids, :] + + +def poisson_disk_sampling(orig_points: npt.NDArray, n_points: int, + min_distance: float, seed: int, MAX_ITER=1000): + """ + Poisson disk sampling function + This function sample N-dimensional points randomly until the number of + points keeping minimum distance between selected points. + + Parameters + ---------- + orig_points : (M, N) N-dimensional points for sampling. + The number of points is M. + n_points : number of points for sampling + min_distance : minimum distance between selected points. + seed : random seed number + MAX_ITER : Maximum number of iteration. Default is 1000. + + Returns + ------- + sampled points (n_points or less, N) + """ + rng = np.random.default_rng(seed) + selected_id = rng.choice(range(orig_points.shape[0])) + selected_ids = [selected_id] + loop = 0 + while len(selected_ids) < n_points and loop <= MAX_ITER: + selected_id = rng.choice(range(orig_points.shape[0])) + base_point = orig_points[selected_id, :] + distances = np.linalg.norm( + orig_points[np.newaxis, selected_ids] - base_point, + axis=2).flatten() + if min(distances) >= min_distance: + selected_ids.append(selected_id) + loop += 1 + if len(selected_ids) != n_points: + print("Could not find the specified number of points...") + + return orig_points[selected_ids, :] + + +def plot_sampled_points(original_points, sampled_points, method_name): + fig = plt.figure() + ax = fig.add_subplot(projection='3d') + ax.scatter(original_points[:, 0], original_points[:, 1], + original_points[:, 2], marker=".", label="Original points") + ax.scatter(sampled_points[:, 0], sampled_points[:, 1], + sampled_points[:, 2], marker="o", + label="Filtered points") + plt.legend() + plt.title(method_name) + plt.axis("equal") + + +def main(): + n_points = 1000 + seed = 1234 + rng = np.random.default_rng(seed) + + x = rng.normal(0.0, 10.0, n_points) + y = rng.normal(0.0, 1.0, n_points) + z = rng.normal(0.0, 10.0, n_points) + original_points = np.vstack((x, y, z)).T + print(f"{original_points.shape=}") + print("Voxel point sampling") + voxel_size = 20.0 + voxel_sampling_points = voxel_point_sampling(original_points, voxel_size) + print(f"{voxel_sampling_points.shape=}") + + print("Farthest point sampling") + n_points = 20 + farthest_sampling_points = farthest_point_sampling(original_points, + n_points, seed) + print(f"{farthest_sampling_points.shape=}") + + print("Poisson disk sampling") + n_points = 20 + min_distance = 10.0 + poisson_disk_points = poisson_disk_sampling(original_points, n_points, + min_distance, seed) + print(f"{poisson_disk_points.shape=}") + + if do_plot: + plot_sampled_points(original_points, voxel_sampling_points, + "Voxel point sampling") + plot_sampled_points(original_points, farthest_sampling_points, + "Farthest point sampling") + plot_sampled_points(original_points, poisson_disk_points, + "poisson disk sampling") + plt.show() + + +if __name__ == '__main__': + main() diff --git a/docs/modules/mapping/mapping_main.rst b/docs/modules/mapping/mapping_main.rst index 0f7ec549..14a7cce7 100644 --- a/docs/modules/mapping/mapping_main.rst +++ b/docs/modules/mapping/mapping_main.rst @@ -9,6 +9,7 @@ Mapping gaussian_grid_map/gaussian_grid_map ray_casting_grid_map/ray_casting_grid_map lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial + point_cloud_sampling/point_cloud_sampling k_means_object_clustering/k_means_object_clustering circle_fitting/circle_fitting rectangle_fitting/rectangle_fitting diff --git a/docs/modules/mapping/point_cloud_sampling/farthest_point_sampling.png b/docs/modules/mapping/point_cloud_sampling/farthest_point_sampling.png new file mode 100644 index 00000000..6024a5f2 Binary files /dev/null and b/docs/modules/mapping/point_cloud_sampling/farthest_point_sampling.png differ diff --git a/docs/modules/mapping/point_cloud_sampling/point_cloud_sampling_main.rst b/docs/modules/mapping/point_cloud_sampling/point_cloud_sampling_main.rst new file mode 100644 index 00000000..cbb5652f --- /dev/null +++ b/docs/modules/mapping/point_cloud_sampling/point_cloud_sampling_main.rst @@ -0,0 +1,70 @@ +.. _point_cloud_sampling: + +Point cloud Sampling +---------------------- + +This sections explains point cloud sampling algorithms in PythonRobotics. + +Point clouds are two-dimensional and three-dimensional based data +acquired by external sensors like LIDAR, cameras, etc. +In general, Point Cloud data is very large in number of data. +So, if you process all the data, computation time might become an issue. + +Point cloud sampling is a technique for solving this computational complexity +issue by extracting only representative point data and thinning the point +cloud data without compromising the performance of processing using the point +cloud data. + +Voxel Point Sampling +~~~~~~~~~~~~~~~~~~~~~~~~ +.. figure:: voxel_point_sampling.png + +Voxel grid sampling is a method of reducing point cloud data by using the +`Voxel grids `_ which is regular grids +in three-dimensional space. + +This method determines which each point is in a grid, and replaces the point +clouds that are in the same Voxel with their average to reduce the number of +points. + +API +===== + +.. autofunction:: Mapping.point_cloud_sampling.point_cloud_sampling.voxel_point_sampling + + +Farthest Point Sampling +~~~~~~~~~~~~~~~~~~~~~~~~~ +.. figure:: farthest_point_sampling.png + +Farthest Point Sampling is a point cloud sampling method by a specified +number of points so that the distance between points is as far from as +possible. + +This method is useful for machine learning and other situations where +you want to obtain a specified number of points from point cloud. + +API +===== + +.. autofunction:: Mapping.point_cloud_sampling.point_cloud_sampling.farthest_point_sampling + +Poisson Disk Sampling +~~~~~~~~~~~~~~~~~~~~~~~~~ +.. figure:: poisson_disk_sampling.png + +Poisson disk sample is a point cloud sampling method by a specified +number of points so that the algorithm selects points where the distance +from selected points is greater than a certain distance. + +Although this method does not have good performance comparing the Farthest +distance sample where each point is distributed farther from each other, +this is suitable for real-time processing because of its fast computation time. + +API +===== + +.. autofunction:: Mapping.point_cloud_sampling.point_cloud_sampling.poisson_disk_sampling + + + diff --git a/docs/modules/mapping/point_cloud_sampling/poisson_disk_sampling.png b/docs/modules/mapping/point_cloud_sampling/poisson_disk_sampling.png new file mode 100644 index 00000000..6863c9e0 Binary files /dev/null and b/docs/modules/mapping/point_cloud_sampling/poisson_disk_sampling.png differ diff --git a/docs/modules/mapping/point_cloud_sampling/voxel_point_sampling.png b/docs/modules/mapping/point_cloud_sampling/voxel_point_sampling.png new file mode 100644 index 00000000..fe1d171f Binary files /dev/null and b/docs/modules/mapping/point_cloud_sampling/voxel_point_sampling.png differ diff --git a/tests/test_point_cloud_sampling.py b/tests/test_point_cloud_sampling.py new file mode 100644 index 00000000..8f6447c6 --- /dev/null +++ b/tests/test_point_cloud_sampling.py @@ -0,0 +1,15 @@ +import conftest # Add root path to sys.path +from Mapping.point_cloud_sampling import point_cloud_sampling as m + + +def test_1(capsys): + m.do_plot = False + m.main() + captured = capsys.readouterr() + assert "voxel_sampling_points.shape=(27, 3)" in captured.out + assert "farthest_sampling_points.shape=(20, 3)" in captured.out + assert "poisson_disk_points.shape=(20, 3)" in captured.out + + +if __name__ == '__main__': + conftest.run_this_test(__file__)