Skip to content

Commit

Permalink
feat: move occupancy grid map filter (#748 #757) (#138)
Browse files Browse the repository at this point in the history 
* fix: Move occupancy grid map filter (#748)

* create directory

* remove occupancy grid outlier filter from pointcloud_preprocessor

* rename package

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* add description image to readme (#757)

Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Co-authored-by: Taichi Higashide <taichi.higashide@tier4.jp>
Co-authored-by: Tomoya Kimura <tomoya.kimura@tier4.jp>
  • Loading branch information
@wep21 @satoshi-ota @tkimura4
4 people authored Dec 7, 2021
1 parent 52d22fd commit c2e79c3
Show file tree
Hide file tree
Showing 8 changed files with 224 additions and 55 deletions.
75 changes: 75 additions & 0 deletions perception/occupancy_grid_map_outlier_filter/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -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)
63 changes: 63 additions & 0 deletions perception/occupancy_grid_map_outlier_filter/README.md
View file
Original file line number Diff line number Diff line change
@@ -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
4 changes: 4 additions & 0 deletions ...ancy_grid_map_outlier_filter/image/occupancy_grid_map_outlier_filter.drawio.svg
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
12 changes: 7 additions & 5 deletions ...pancy_grid_map_outlier_filter_nodelet.hpp → ...pancy_grid_map_outlier_filter_nodelet.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <pcl/common/impl/common.hpp>
#include <rclcpp/rclcpp.hpp>
Expand All @@ -36,7 +38,7 @@
#include <memory>
#include <string>

namespace pointcloud_preprocessor
namespace occupancy_grid_map_outlier_filter
{
using geometry_msgs::msg::Pose;
using nav_msgs::msg::OccupancyGrid;
Expand Down Expand Up @@ -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_
51 changes: 51 additions & 0 deletions perception/occupancy_grid_map_outlier_filter/package.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,51 @@
<?xml version="1.0"?>
<package format="3">
<name>occupancy_grid_map_outlier_filter</name>
<version>0.1.0</version>
<description>The ROS2 occupancy_grid_map_outlier_filter package</description>

<maintainer email="abrahammonrroy@yahoo.com">amc-nu</maintainer>
<maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
<license>Apache License 2.0</license>

<author>Open Perception</author>
<author email="julius@kammerl.de">Julius Kammerl</author>
<author email="william@osrfoundation.org">William Woodall</author>
<maintainer email="paul@bovbel.com">Paul Bovbel</maintainer>
<maintainer email="www.kentaro.wada@gmail.com">Kentaro Wada</maintainer>
<license>Apache License 2.0</license>

<buildtool_depend>ament_cmake_auto</buildtool_depend>

<depend>autoware_auto_vehicle_msgs</depend>
<depend>autoware_debug_msgs</depend>
<depend>autoware_utils</depend>
<depend>diagnostic_updater</depend>
<depend>image_transport</depend>
<depend>lanelet2_extension</depend>
<depend>libopencv-dev</depend>
<depend>libpcl-all-dev</depend>
<depend>message_filters</depend>
<depend>nav_msgs</depend>
<depend>pcl_conversions</depend>
<depend>pcl_msgs</depend>
<depend>pcl_ros</depend>
<depend>pointcloud_preprocessor</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
<depend>tf2</depend>
<depend>tf2_eigen</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>tier4_pcl_extensions</depend>
<depend>vehicle_info_util</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
</package>
15 changes: 7 additions & 8 deletions ...pancy_grid_map_outlier_filter_nodelet.cpp → ...pancy_grid_map_outlier_filter_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <autoware_utils/autoware_utils.hpp>
#include <pcl_ros/transforms.hpp>
Expand Down Expand Up @@ -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
Expand All @@ -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);
}
Expand Down Expand Up @@ -96,7 +94,7 @@ boost::optional<char> getCost(

} // namespace

namespace pointcloud_preprocessor
namespace occupancy_grid_map_outlier_filter
{
RadiusSearch2dfilter::RadiusSearch2dfilter(rclcpp::Node & node)
{
Expand Down Expand Up @@ -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_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::OccupancyGridMapOutlierFilterComponent)
RCLCPP_COMPONENTS_REGISTER_NODE(
occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent)
6 changes: 0 additions & 6 deletions sensing/pointcloud_preprocessor/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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"
Expand Down
Loading

0 comments on commit c2e79c3

Please sign in to comment.