diff --git a/perception/occupancy_grid_map_outlier_filter/CMakeLists.txt b/perception/occupancy_grid_map_outlier_filter/CMakeLists.txt new file mode 100644 index 0000000000000..a9aa90cdb7a90 --- /dev/null +++ b/perception/occupancy_grid_map_outlier_filter/CMakeLists.txt @@ -0,0 +1,75 @@ +cmake_minimum_required(VERSION 3.5) +project(occupancy_grid_map_outlier_filter) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +# Ignore PCL errors in Clang +if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wno-gnu-anonymous-struct -Wno-nested-anon-types) +endif() + +find_package(ament_cmake_auto REQUIRED) +find_package(OpenCV REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(Boost REQUIRED) +find_package(PCL REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(OpenMP) +ament_auto_find_build_dependencies() + + +########### +## Build ## +########### + +include_directories( + include + ${Boost_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${GRID_MAP_INCLUDE_DIR} +) + +ament_auto_add_library(occupancy_grid_map_outlier_filter SHARED + src/occupancy_grid_map_outlier_filter_nodelet.cpp +) + +target_link_libraries(occupancy_grid_map_outlier_filter + ${Boost_LIBRARIES} + ${OpenCV_LIBRARIES} + ${PCL_LIBRARIES} +) + +if(OPENMP_FOUND) + set_target_properties(occupancy_grid_map_outlier_filter PROPERTIES + COMPILE_FLAGS ${OpenMP_CXX_FLAGS} + LINK_FLAGS ${OpenMP_CXX_FLAGS} + ) +endif() + + +# -- Occupancy Grid Map Outlier Filter -- +rclcpp_components_register_node(occupancy_grid_map_outlier_filter + PLUGIN "occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent" + EXECUTABLE occupancy_grid_map_outlier_filter_node) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +############# +## Install ## +############# + +ament_auto_package(INSTALL_TO_SHARE) diff --git a/perception/occupancy_grid_map_outlier_filter/README.md b/perception/occupancy_grid_map_outlier_filter/README.md new file mode 100644 index 0000000000000..fd064af19902f --- /dev/null +++ b/perception/occupancy_grid_map_outlier_filter/README.md @@ -0,0 +1,63 @@ +# occupancy_grid_map_outlier_filter + +## Purpose + +This node is an outlier filter based on a occupancy grid map. +Depending on the implementation of occupancy grid map, it can be called an outlier filter in time series, since the occupancy grid map expresses the occupancy probabilities in time series. + +## Inner-workings / Algorithms + +1. Use the occupancy grid map to separate point clouds into those with low occupancy probability and those with high occupancy probability. + +2. The point clouds that belong to the low occupancy probability are not necessarily outliers. In particular, the top of the moving object tends to belong to the low occupancy probability. Therefore, if `use_radius_search_2d_filter` is true, then apply an radius search 2d outlier filter to the point cloud that is determined to have a low occupancy probability. + 1. For each low occupancy probability point, determine the outlier from the radius (`radius_search_2d_filter/search_radius`) and the number of point clouds. In this case, the point cloud to be referenced is not only low occupancy probability points, but all point cloud including high occupancy probability points. + 2. The number of point clouds can be multiplied by `radius_search_2d_filter/min_points_and_distance_ratio` and distance from base link. However, the minimum and maximum number of point clouds is limited. + +The following video is a sample. Yellow points are high occupancy probability, green points are low occupancy probability which is not an outlier, and red points are outliers. At around 0:15 and 1:16 in the first video, a bird crosses the road, but it is considered as an outlier. + +- [movie1](https://www.youtube.com/watch?v=hEVv0LaTpP8) +- [movie2](https://www.youtube.com/watch?v=VaHs1CdLcD0) + +![occupancy_grid_map_outlier_filter](./image/occupancy_grid_map_outlier_filter.drawio.svg) + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ---------------------------- | ------------------------- | ------------------------------------------------------------------------------------------ | +| `~/input/pointcloud` | `sensor_msgs/PointCloud2` | Obstacle point cloud with ground removed. | +| `~/input/occupancy_grid_map` | `nav_msgs/OccupancyGrid` | A map in which the probability of the presence of an obstacle is occupancy probability map | + +### Output + +| Name | Type | Description | +| ------------------------------------------- | ------------------------- | ---------------------------------------------------------------------------------------------------------------------------- | +| `~/output/pointcloud` | `sensor_msgs/PointCloud2` | Point cloud with outliers removed. trajectory | +| `~/output/debug/outlier/pointcloud` | `sensor_msgs/PointCloud2` | Point clouds removed as outliers. | +| `~/output/debug/low_confidence/pointcloud` | `sensor_msgs/PointCloud2` | Point clouds that had a low probability of occupancy in the occupancy grid map. However, it is not considered as an outlier. | +| `~/output/debug/high_confidence/pointcloud` | `sensor_msgs/PointCloud2` | Point clouds that had a high probability of occupancy in the occupancy grid map. trajectory | + +## Parameters + +| Name | Type | Description | +| ------------------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `map_frame` | string | map frame id | +| `base_link_frame` | string | base link frame id | +| `cost_threshold` | int | Cost threshold of occupancy grid map (0~100). 100 means 100% probability that there is an obstacle, close to 50 means that it is indistinguishable whether it is an obstacle or free space, 0 means that there is no obstacle. | +| `enable_debugger` | bool | Whether to output the point cloud for debugging. | +| `use_radius_search_2d_filter` | bool | Whether or not to apply density-based outlier filters to objects that are judged to have low probability of occupancy on the occupancy grid map. | +| `radius_search_2d_filter/search_radius` | float | Radius when calculating the density | +| `radius_search_2d_filter/min_points_and_distance_ratio` | float | Threshold value of the number of point clouds per radius when the distance from baselink is 1m, because the number of point clouds varies with the distance from baselink. | +| `radius_search_2d_filter/min_points` | int | Minimum number of point clouds per radius | +| `radius_search_2d_filter/max_points` | int | Maximum number of point clouds per radius | + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/perception/occupancy_grid_map_outlier_filter/image/occupancy_grid_map_outlier_filter.drawio.svg b/perception/occupancy_grid_map_outlier_filter/image/occupancy_grid_map_outlier_filter.drawio.svg new file mode 100644 index 0000000000000..5c71a72cdc538 --- /dev/null +++ b/perception/occupancy_grid_map_outlier_filter/image/occupancy_grid_map_outlier_filter.drawio.svg @@ -0,0 +1,4 @@ + + + +
x
x
y
y
find a low confidence point and apply it to radius search outlier filter
find a low confidence p...
filter ground to generate occupancy grid map
filter ground to genera...
generate occupancy grid map
generate occupancy...
find low confidence point from occupancy grid map
find low confidence point from occupancy gr...Viewer does not support full SVG 1.1 \ No newline at end of file diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp similarity index 90% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp rename to perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp index e794bebdc1520..52e3f22344e8b 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp +++ b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODELET_HPP_ -#define POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODELET_HPP_ +#ifndef OCCUPANCY_GRID_MAP_OUTLIER_FILTER__OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODELET_HPP_ +#define OCCUPANCY_GRID_MAP_OUTLIER_FILTER__OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODELET_HPP_ + +#include "pointcloud_preprocessor/filter.hpp" #include #include @@ -36,7 +38,7 @@ #include #include -namespace pointcloud_preprocessor +namespace occupancy_grid_map_outlier_filter { using geometry_msgs::msg::Pose; using nav_msgs::msg::OccupancyGrid; @@ -121,6 +123,6 @@ class OccupancyGridMapOutlierFilterComponent : public rclcpp::Node std::string base_link_frame_; int cost_threshold_; }; -} // namespace pointcloud_preprocessor +} // namespace occupancy_grid_map_outlier_filter -#endif // POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODELET_HPP_ +#endif // OCCUPANCY_GRID_MAP_OUTLIER_FILTER__OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODELET_HPP_ diff --git a/perception/occupancy_grid_map_outlier_filter/package.xml b/perception/occupancy_grid_map_outlier_filter/package.xml new file mode 100644 index 0000000000000..deb5563251f3d --- /dev/null +++ b/perception/occupancy_grid_map_outlier_filter/package.xml @@ -0,0 +1,51 @@ + + + occupancy_grid_map_outlier_filter + 0.1.0 + The ROS2 occupancy_grid_map_outlier_filter package + + amc-nu + Yukihiro Saito + Apache License 2.0 + + Open Perception + Julius Kammerl + William Woodall + Paul Bovbel + Kentaro Wada + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_vehicle_msgs + autoware_debug_msgs + autoware_utils + diagnostic_updater + image_transport + lanelet2_extension + libopencv-dev + libpcl-all-dev + message_filters + nav_msgs + pcl_conversions + pcl_msgs + pcl_ros + pointcloud_preprocessor + rclcpp + rclcpp_components + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_pcl_extensions + vehicle_info_util + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp similarity index 96% rename from sensing/pointcloud_preprocessor/src/outlier_filter/occupancy_grid_map_outlier_filter_nodelet.cpp rename to perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index dc00e0e86ac5a..680ea1614d2c0 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp" +#include "occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp" #include #include @@ -42,8 +42,7 @@ bool transformPointcloud( target_frame, input.header.frame_id, input.header.stamp, rclcpp::Duration::from_seconds(0.5)); } catch (const tf2::TransformException & ex) { RCLCPP_WARN_THROTTLE( - rclcpp::get_logger("pointcloud_processor").get_child("occupancy_grid_map_outlier_filter"), - clock, 5000, "%s", ex.what()); + rclcpp::get_logger("occupancy_grid_map_outlier_filter"), clock, 5000, "%s", ex.what()); return false; } // transform pointcloud @@ -65,8 +64,7 @@ geometry_msgs::msg::PoseStamped getPoseStamped( tf2.lookupTransform(target_frame_id, src_frame_id, time, rclcpp::Duration::from_seconds(0.5)); } catch (const tf2::TransformException & ex) { RCLCPP_WARN_THROTTLE( - rclcpp::get_logger("pointcloud_processor").get_child("occupancy_grid_map_outlier_filter"), - clock, 5000, "%s", ex.what()); + rclcpp::get_logger("occupancy_grid_map_outlier_filter"), clock, 5000, "%s", ex.what()); } return autoware_utils::transform2pose(tf_stamped); } @@ -96,7 +94,7 @@ boost::optional getCost( } // namespace -namespace pointcloud_preprocessor +namespace occupancy_grid_map_outlier_filter { RadiusSearch2dfilter::RadiusSearch2dfilter(rclcpp::Node & node) { @@ -319,7 +317,8 @@ void OccupancyGridMapOutlierFilterComponent::Debugger::transformToBaseLink( transformPointcloud(ros_input, *(node_.tf2_), node_.base_link_frame_, output); } -} // namespace pointcloud_preprocessor +} // namespace occupancy_grid_map_outlier_filter #include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::OccupancyGridMapOutlierFilterComponent) +RCLCPP_COMPONENTS_REGISTER_NODE( + occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent) diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index d5ebd3193c6b2..9f4e18780b53a 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -49,7 +49,6 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/outlier_filter/ring_outlier_filter_nodelet.cpp src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp - src/outlier_filter/occupancy_grid_map_outlier_filter_nodelet.cpp src/outlier_filter/dual_return_outlier_filter_nodelet.cpp src/passthrough_filter/passthrough_filter_nodelet.cpp src/passthrough_filter/passthrough_filter_uint16_nodelet.cpp @@ -115,11 +114,6 @@ rclcpp_components_register_node(pointcloud_preprocessor_filter PLUGIN "pointcloud_preprocessor::RadiusSearch2DOutlierFilterComponent" EXECUTABLE radius_search_2d_outlier_filter_node) -# -- Occupancy Grid Map Outlier Filter -- -rclcpp_components_register_node(pointcloud_preprocessor_filter - PLUGIN "pointcloud_preprocessor::OccupancyGridMapOutlierFilterComponent" - EXECUTABLE occupancy_grid_map_outlier_filter_node) - # -- DualReturn Outlier Filter-- rclcpp_components_register_node(pointcloud_preprocessor_filter PLUGIN "pointcloud_preprocessor::DualReturnOutlierFilterComponent" diff --git a/sensing/pointcloud_preprocessor/docs/outlier-filter.md b/sensing/pointcloud_preprocessor/docs/outlier-filter.md index 2487a988bc717..e992870bcf798 100644 --- a/sensing/pointcloud_preprocessor/docs/outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/outlier-filter.md @@ -2,8 +2,7 @@ ## Purpose -This node is an outlier filter based on a occupancy grid map. -Depending on the implementation of occupancy grid map, it can be called an outlier filter in time series, since the occupancy grid map expresses the occupancy probabilities in time series. +The `outlier_filter` is a node that removes points caused by hardware problems, rain drops and small insects as a noise. ## Inner-workings / Algorithms @@ -11,19 +10,6 @@ Depending on the implementation of occupancy grid map, it can be called an outli WIP -### Occupancy GridMap Outlier Filter - -1. Use the occupancy grid map to separate point clouds into those with low occupancy probability and those with high occupancy probability. - -2. The point clouds that belong to the low occupancy probability are not necessarily outliers. In particular, the top of the moving object tends to belong to the low occupancy probability. Therefore, if `use_radius_search_2d_filter` is true, then apply an radius search 2d outlier filter to the point cloud that is determined to have a low occupancy probability. - 1. For each low occupancy probability point, determine the outlier from the radius (`radius_search_2d_filter/search_radius`) and the number of point clouds. In this case, the point cloud to be referenced is not only low occupancy probability points, but all point cloud including high occupancy probability points. - 2. The number of point clouds can be multiplied by `radius_search_2d_filter/min_points_and_distance_ratio` and distance from base link. However, the minimum and maximum number of point clouds is limited. - -The following video is a sample. Yellow points are high occupancy probability, green points are low occupancy probability which is not an outlier, and red points are outliers. At around 0:15 and 1:16 in the first video, a bird crosses the road, but it is considered as an outlier. - -- [movie1](https://www.youtube.com/watch?v=hEVv0LaTpP8) -- [movie2](https://www.youtube.com/watch?v=VaHs1CdLcD0) - ### Radius Search 2d Outlier Filter [1] WIP @@ -40,33 +26,28 @@ WIP ### Input -| Name | Type | Description | -| ---------------------------- | ------------------------- | ------------------------------------------------------------------------------------------ | -| `~/input/pointcloud` | `sensor_msgs/PointCloud2` | Obstacle point cloud with ground removed. | -| `~/input/occupancy_grid_map` | `nav_msgs/OccupancyGrid` | A map in which the probability of the presence of an obstacle is occupancy probability map | +| Name | Type | Description | +| -------------------- | ------------------------- | ----------------------------------------- | +| `~/input/pointcloud` | `sensor_msgs/PointCloud2` | Obstacle point cloud with ground removed. | ### Output -| Name | Type | Description | -| ------------------------------------------- | ------------------------- | ---------------------------------------------------------------------------------------------------------------------------- | -| `~/output/pointcloud` | `sensor_msgs/PointCloud2` | Point cloud with outliers removed. trajectory | -| `~/output/debug/outlier/pointcloud` | `sensor_msgs/PointCloud2` | Point clouds removed as outliers. | -| `~/output/debug/low_confidence/pointcloud` | `sensor_msgs/PointCloud2` | Point clouds that had a low probability of occupancy in the occupancy grid map. However, it is not considered as an outlier. | -| `~/output/debug/high_confidence/pointcloud` | `sensor_msgs/PointCloud2` | Point clouds that had a high probability of occupancy in the occupancy grid map. trajectory | +| Name | Type | Description | +| ----------------------------------- | ------------------------- | --------------------------------------------- | +| `~/output/pointcloud` | `sensor_msgs/PointCloud2` | Point cloud with outliers removed. trajectory | +| `~/output/debug/outlier/pointcloud` | `sensor_msgs/PointCloud2` | Point clouds removed as outliers. | ## Parameters -| Name | Type | Description | -| ------------------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -| `map_frame` | string | map frame id | -| `base_link_frame` | string | base link frame id | -| `cost_threshold` | int | Cost threshold of occupancy grid map (0~100). 100 means 100% probability that there is an obstacle, close to 50 means that it is indistinguishable whether it is an obstacle or free space, 0 means that there is no obstacle. | -| `enable_debugger` | bool | Whether to output the point cloud for debugging. | -| `use_radius_search_2d_filter` | bool | Whether or not to apply density-based outlier filters to objects that are judged to have low probability of occupancy on the occupancy grid map. | -| `radius_search_2d_filter/search_radius` | float | Radius when calculating the density | -| `radius_search_2d_filter/min_points_and_distance_ratio` | float | Threshold value of the number of point clouds per radius when the distance from baselink is 1m, because the number of point clouds varies with the distance from baselink. | -| `radius_search_2d_filter/min_points` | int | Minimum number of point clouds per radius | -| `radius_search_2d_filter/max_points` | int | Maximum number of point clouds per radius | +| Name | Type | Description | +| ------------------------------------------------------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `map_frame` | string | map frame id | +| `base_link_frame` | string | base link frame id | +| `enable_debugger` | bool | Whether to output the point cloud for debugging. | +| `radius_search_2d_filter/search_radius` | float | Radius when calculating the density | +| `radius_search_2d_filter/min_points_and_distance_ratio` | float | Threshold value of the number of point clouds per radius when the distance from baselink is 1m, because the number of point clouds varies with the distance from baselink. | +| `radius_search_2d_filter/min_points` | int | Minimum number of point clouds per radius | +| `radius_search_2d_filter/max_points` | int | Maximum number of point clouds per radius | ## Assumptions / Known limits