Function 'filterPointCloudByTrajectory' does not work as intended

[maxime-clem]

Checklist

• I've read the contribution guidelines.
• I've searched other issues and no duplicate issues were found.
• I'm convinced that this is not my fault but a bug.

Description

Function filterPointCloudByTrajectory is used to filter a point cloud and only keep the points that are within a given distance from a trajectory. The current implementation is wrong and can return a point cloud with duplicate points and can be larger than the original point cloud.

autoware.universe/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp

Lines 52 to 67 in b06ff44

	pcl::PointCloud<pcl::PointXYZ> filterPointCloudByTrajectory(
	const pcl::PointCloud<pcl::PointXYZ> & pointcloud,
	const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double radius)
	{
	pcl::PointCloud<pcl::PointXYZ> filtered_pointcloud;
	for (const auto & trajectory_point : trajectory.points) {
	for (const auto & point : pointcloud.points) {
	const double dx = trajectory_point.pose.position.x - point.x;
	const double dy = trajectory_point.pose.position.y - point.y;
	if (std::hypot(dx, dy) < radius) {
	filtered_pointcloud.points.push_back(point);
	}
	}
	}
	return filtered_pointcloud;
	}

Expected behavior

Function filterPointCloudByTrajectory returns a point cloud of size at most equal to the size of the input.

Actual behavior

Function filterPointCloudByTrajectory can return a point cloud larger than the input.

Steps to reproduce

Passing a very large radius will cause the bug.

Versions

No response

Possible causes

The same point from the point cloud can be added multiple time if close enough to multiple trajectory_point.

Additional context

Possible fix:

for (const auto & point : pointcloud.points) {
    for (const auto & trajectory_point : trajectory.points) {
      const double dx = trajectory_point.pose.position.x - point.x;
      const double dy = trajectory_point.pose.position.y - point.y;
      if (std::hypot(dx, dy) < radius) {
        filtered_pointcloud.points.push_back(point);
        break;
      }
    }
  }