common/autoware_perception_rviz_plugin/CMakeLists.txt

@@ -6,6 +6,8 @@ autoware_package()
 
 find_package(Qt5 REQUIRED COMPONENTS Widgets)
 
+find_package(PCL REQUIRED)
+
 set(OD_PLUGIN_LIB_SRC
   src/object_detection/detected_objects_display.cpp
   src/object_detection/tracked_objects_display.cpp
@@ -46,9 +48,14 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
 target_link_libraries(${PROJECT_NAME}
   rviz_common::rviz_common
   Qt5::Widgets
+  ${PCL_LIBRARIES}
 )
 target_include_directories(${PROJECT_NAME} PRIVATE include)
 
+target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC
+"${PCL_INCLUDE_DIRS}"
+)
+
 # Settings to improve the build as suggested on https://github.com/ros2/rviz/blob/ros2/docs/plugin_development.md
 target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 target_compile_definitions(${PROJECT_NAME} PRIVATE "OBJECT_DETECTION_PLUGINS_BUILDING_LIBRARY")

common/autoware_perception_rviz_plugin/README.md

@@ -19,9 +19,10 @@ Example:
 
 #### Input Types
 
-| Name | Type                                             | Description            |
-| ---- | ------------------------------------------------ | ---------------------- |
-|      | `autoware_perception_msgs::msg::DetectedObjects` | detection result array |
+| Name | Type                                             | Description               |
+| ---- | ------------------------------------------------ | ------------------------- |
+|      | `autoware_perception_msgs::msg::DetectedObjects` | detection result array    |
+|      | `sensor_msgs::msg::PointCloud2`                  | point cloud for filtering |
 
 #### Visualization Result
 
@@ -31,9 +32,10 @@ Example:
 
 #### Input Types
 
-| Name | Type                                            | Description           |
-| ---- | ----------------------------------------------- | --------------------- |
-|      | `autoware_perception_msgs::msg::TrackedObjects` | tracking result array |
+| Name | Type                                            | Description               |
+| ---- | ----------------------------------------------- | ------------------------- |
+|      | `autoware_perception_msgs::msg::TrackedObjects` | tracking result array     |
+|      | `sensor_msgs::msg::PointCloud2`                 | point cloud for filtering |
 
 #### Visualization Result
 
@@ -45,16 +47,24 @@ Overwrite tracking results with detection results.
 
 #### Input Types
 
-| Name | Type                                              | Description             |
-| ---- | ------------------------------------------------- | ----------------------- |
-|      | `autoware_perception_msgs::msg::PredictedObjects` | prediction result array |
+<<<<<<< HEAD:common/autoware_perception_rviz_plugin/README.md
+| Name | Type | Description |
+| ---- | ------------------------------------------------------ | ------------------------ |
+| | `autoware_perception_msgs::msg::PredictedObjects` | prediction result array |
+| | `sensor_msgs::msg::PointCloud2` | pointcloud for filtering |
 
 #### Visualization Result
 
 Overwrite prediction results with tracking results.
 
 ![predicted-object-visualization-description](./images/predicted-object-visualization-description.jpg)
 
+### Visualization with active point cloud publishing
+
+Publishing colored point clouds. With colors according to different classes of detected objects, same as polygons.
+
+![visualization-with-pointcloud](./images/3d_pointcloud.png)
+
 ## References/External links
 
 [1] <https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/tree/master/src/tools/visualization/autoware_rviz_plugins>

common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp

@@ -16,7 +16,11 @@
 
 #include "autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp"
 
+#include <autoware_perception_msgs/msg/detected_object.hpp>
 #include <autoware_perception_msgs/msg/detected_objects.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <memory>
 
 namespace autoware
 {
@@ -37,6 +41,9 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay
 
 private:
   void processMessage(DetectedObjects::ConstSharedPtr msg) override;
+  void processPointCloud(
+    const DetectedObjects::ConstSharedPtr & input_objs_msg,
+    const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg);
 };
 
 }  // namespace object_detection

common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp

@@ -32,6 +32,13 @@
 #include <geometry_msgs/msg/twist_with_covariance.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 
+#include <boost/geometry.hpp>
+#include <boost/geometry/geometries/point_xy.hpp>
+#include <boost/geometry/geometries/polygon.hpp>
+#include <boost/geometry/strategies/buffer.hpp>
+#include <boost/geometry/strategies/transform.hpp>
+#include <boost/polygon/polygon.hpp>
+
 #include <algorithm>
 #include <map>
 #include <string>
@@ -196,6 +203,10 @@ AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_lis
   const autoware_perception_msgs::msg::Shape & shape,
   std::vector<geometry_msgs::msg::Point> & points);
 
+AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_triangle_list(
+  const autoware_perception_msgs::msg::Shape & shape,
+  std::vector<geometry_msgs::msg::Point> & points);
+
 AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_direction_line_list(
   const autoware_perception_msgs::msg::Shape & shape,
   std::vector<geometry_msgs::msg::Point> & points);