diff --git a/common/autoware_perception_rviz_plugin/CMakeLists.txt b/common/autoware_perception_rviz_plugin/CMakeLists.txt
index 74671d74f7397..2233b65a1f46f 100644
--- a/common/autoware_perception_rviz_plugin/CMakeLists.txt
+++ b/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")
diff --git a/common/autoware_perception_rviz_plugin/README.md b/common/autoware_perception_rviz_plugin/README.md
index ed6f3e1675ace..74308847d3ee5 100644
--- a/common/autoware_perception_rviz_plugin/README.md
+++ b/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,9 +47,11 @@ 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
 
@@ -55,6 +59,12 @@ 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>
diff --git a/common/autoware_perception_rviz_plugin/images/3d_pointcloud.png b/common/autoware_perception_rviz_plugin/images/3d_pointcloud.png
new file mode 100644
index 0000000000000..1887bcec33994
Binary files /dev/null and b/common/autoware_perception_rviz_plugin/images/3d_pointcloud.png differ
diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp
index 4dcc4e9ea1545..508eebf229ad3 100644
--- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp
+++ b/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
diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp
index 8d29900e1da26..a02f1e0ad38a4 100644
--- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp
+++ b/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);
diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp
index b05ba5f27f551..fa13ffe21f247 100644
--- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp
+++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp
@@ -17,18 +17,57 @@
 #include "autoware_perception_rviz_plugin/common/color_alpha_property.hpp"
 #include "autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp"
 #include "autoware_perception_rviz_plugin/visibility_control.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rviz_common/properties/ros_topic_property.hpp"
+#include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp"
 
 #include <rviz_common/display.hpp>
+#include <rviz_common/display_context.hpp>
 #include <rviz_common/properties/color_property.hpp>
 #include <rviz_common/properties/enum_property.hpp>
 #include <rviz_common/properties/float_property.hpp>
+#include <rviz_common/properties/status_property.hpp>
+#include <rviz_common/ros_integration/ros_node_abstraction_iface.hpp>
 #include <rviz_default_plugins/displays/marker/marker_common.hpp>
 #include <rviz_default_plugins/displays/marker_array/marker_array_display.hpp>
 
+// #include "autoware/universe_utils/autoware_universe_utils.hpp"
+
+#include "autoware/universe_utils/geometry/boost_geometry.hpp"
+#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp"
+
 #include <autoware_perception_msgs/msg/object_classification.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
 #include <unique_identifier_msgs/msg/uuid.hpp>
 
+#include <boost/geometry.hpp>
+#include <boost/optional.hpp>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/synchronizer.h>
+#include <pcl/PCLPointCloud2.h>
+#include <pcl/common/common.h>
+#include <pcl/common/transforms.h>
+#include <pcl/conversions.h>
+#include <pcl/filters/crop_box.h>
+#include <pcl/filters/crop_hull.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/search/pcl_search.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <algorithm>
 #include <bitset>
+#include <deque>
 #include <list>
 #include <memory>
 #include <string>
@@ -41,6 +80,39 @@ namespace rviz_plugins
 {
 namespace object_detection
 {
+
+using Polygon2d = autoware::universe_utils::Polygon2d;
+using Shape = autoware_perception_msgs::msg::Shape;
+
+// struct for creating objects buffer
+struct object_info
+{
+  Shape shape;
+  geometry_msgs::msg::Pose position;
+  std::vector<autoware_perception_msgs::msg::ObjectClassification> classification;
+};
+
+inline pcl::PointXYZRGB toPCL(const double x, const double y, const double z)
+{
+  pcl::PointXYZRGB pcl_point;
+  pcl_point.x = x;
+  pcl_point.y = y;
+  pcl_point.z = z;
+  return pcl_point;
+}
+
+inline pcl::PointXYZRGB toPCL(const geometry_msgs::msg::Point & point)
+{
+  return toPCL(point.x, point.y, point.z);
+}
+
+// Define a custom comparator function to compare pointclouds by timestamp
+inline bool comparePointCloudsByTimestamp(
+  const sensor_msgs::msg::PointCloud2 & pc1, const sensor_msgs::msg::PointCloud2 & pc2)
+{
+  return (static_cast<int>(pc1.header.stamp.nanosec - pc2.header.stamp.nanosec)) < 0;
+}
+
 /// \brief Base rviz plugin class for all object msg types. The class defines common properties
 ///        for the plugin and also defines common helper functions that can be used by its derived
 ///        classes.
@@ -59,9 +131,10 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
   using PolygonPropertyMap =
     std::unordered_map<ObjectClassificationMsg::_label_type, common::ColorAlphaProperty>;
 
-  explicit ObjectPolygonDisplayBase(const std::string & default_topic)
-  : m_marker_common(this),
-    // m_display_type_property{"Polygon Type", "3d", "Type of the polygon to display object", this},
+  explicit ObjectPolygonDisplayBase(
+    const std::string & default_topic, const std::string & default_pointcloud_topic)
+  : point_cloud_common(new rviz_default_plugins::PointCloudCommon(this)),
+    m_marker_common(this),
     m_display_label_property{"Display Label", true, "Enable/disable label visualization", this},
     m_display_uuid_property{"Display UUID", true, "Enable/disable uuid visualization", this},
     m_display_velocity_text_property{
@@ -91,7 +164,8 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
       this},
 
     m_line_width_property{"Line Width", 0.03, "Line width of object-shape", this},
-    m_default_topic{default_topic}
+    m_default_topic{default_topic},
+    qos_profile_points(5)
   {
     m_display_type_property = new rviz_common::properties::EnumProperty(
       "Polygon Type", "3d", "Type of the polygon to display object.", this);
@@ -112,6 +186,14 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     m_confidence_interval_property->addOption("95%", 2);
     m_confidence_interval_property->addOption("99%", 3);
 
+    m_publish_objs_pointcloud = new rviz_common::properties::BoolProperty(
+      "Publish Objects Pointcloud", true, "Enable/disable objects pointcloud publishing");
+    m_default_pointcloud_topic = new rviz_common::properties::RosTopicProperty(
+      "Input pointcloud topic", QString::fromStdString(default_pointcloud_topic), "",
+      "Input for pointcloud visualization of objects detection pipeline.", this,
+      SLOT(updateTopic()));
+    qos_profile_points_property = new rviz_common::properties::QosProfileProperty(
+      m_default_pointcloud_topic, qos_profile_points);
     m_object_fill_type_property = new rviz_common::properties::EnumProperty(
       "Object Fill Type", "skeleton", "Change object fill type in visualization", this);
     m_object_fill_type_property->addOption(
@@ -139,12 +221,39 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
 
   void onInitialize() override
   {
+    static int init_count = 0;
+    RCLCPP_INFO(
+      rclcpp::get_logger("autoware_perception_plugin"), "onInitialize called %d times",
+      init_count++);
+
     RosTopicDisplay::RTDClass::onInitialize();
     m_marker_common.initialize(this->context_, this->scene_node_);
     QString message_type = QString::fromStdString(rosidl_generator_traits::name<MsgT>());
     this->topic_property_->setMessageType(message_type);
     this->topic_property_->setValue(m_default_topic.c_str());
     this->topic_property_->setDescription("Topic to subscribe to.");
+
+    qos_profile_points_property->initialize([this](rclcpp::QoS profile) {
+      this->qos_profile_points = profile;
+      RosTopicDisplay::updateTopic();
+    });
+
+    // get weak_ptr to properly init RosTopicProperty
+    rviz_ros_node = this->context_->getRosNodeAbstraction();
+
+    m_default_pointcloud_topic->initialize(rviz_ros_node);
+    QString points_message_type =
+      QString::fromStdString(rosidl_generator_traits::name<sensor_msgs::msg::PointCloud2>());
+    m_default_pointcloud_topic->setMessageType(points_message_type);
+
+    // get access to rviz node to sub and to pub to topics
+    rclcpp::Node::SharedPtr raw_node =
+      this->context_->getRosNodeAbstraction().lock()->get_raw_node();
+
+    tf_buffer = std::make_unique<tf2_ros::Buffer>(raw_node->get_clock());
+    tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer);
+
+    point_cloud_common->initialize(this->context_, this->scene_node_);
   }
 
   void load(const rviz_common::Config & config) override
@@ -153,12 +262,90 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     m_marker_common.load(config);
   }
 
-  void update(float wall_dt, float ros_dt) override { m_marker_common.update(wall_dt, ros_dt); }
+  void update(float wall_dt, float ros_dt) override
+  {
+    m_marker_common.update(wall_dt, ros_dt);
+    point_cloud_common->update(wall_dt, ros_dt);
+  }
 
   void reset() override
   {
+    static int reset_count = 0;
+    RCLCPP_INFO(
+      rclcpp::get_logger("autoware_perception_plugin"), "reset called %d times", reset_count++);
+
     RosTopicDisplay::reset();
     m_marker_common.clearMarkers();
+    point_cloud_common->reset();
+  }
+
+  void subscribe()
+  {
+    if (!RosTopicDisplay::isEnabled()) {
+      return;
+    }
+
+    RosTopicDisplay::subscribe();
+    points_subscribe();
+  }
+
+  void updateTopic() override
+  {
+    static int update_count = 0;
+    RCLCPP_INFO(
+      rclcpp::get_logger("autoware_perception_plugin"), "updateTopic called %d times",
+      update_count++);
+    unsubscribe();
+    reset();
+    subscribe();
+    this->context_->queueRender();
+  }
+
+  void points_subscribe()
+  {
+    if (!m_publish_objs_pointcloud->getBool()) {
+      return;
+    }
+
+    if (m_default_pointcloud_topic->isEmpty()) {
+      RosTopicDisplay::setStatus(
+        rviz_common::properties::StatusProperty::Error, "Topic",
+        QString("Error subscribing: Empty topic name"));
+      return;
+    }
+
+    try {
+      rclcpp::SubscriptionOptions sub_opts;
+      sub_opts.event_callbacks.message_lost_callback = [&](rclcpp::QOSMessageLostInfo & info) {
+        std::ostringstream sstream;
+        sstream << "Some messages were lost:\n>\tNumber of new lost messages: "
+                << info.total_count_change
+                << " \n>\tTotal number of messages lost: " << info.total_count;
+        RosTopicDisplay::setStatus(
+          rviz_common::properties::StatusProperty::Warn, "Topic", QString(sstream.str().c_str()));
+      };
+
+      rclcpp::Node::SharedPtr raw_node =
+        this->context_->getRosNodeAbstraction().lock()->get_raw_node();
+      pointcloud_subscription = raw_node->create_subscription<sensor_msgs::msg::PointCloud2>(
+        m_default_pointcloud_topic->getTopicStd(), rclcpp::SensorDataQoS(),
+        std::bind(&ObjectPolygonDisplayBase::pointCloudCallback, this, std::placeholders::_1));
+      RosTopicDisplay::setStatus(rviz_common::properties::StatusProperty::Ok, "Topic", "OK");
+    } catch (rclcpp::exceptions::InvalidTopicNameError & e) {
+      RosTopicDisplay::setStatus(
+        rviz_common::properties::StatusProperty::Error, "Topic",
+        QString("Error subscribing: ") + e.what());
+    }
+  }
+
+  void unsubscribe()
+  {
+    RosTopicDisplay::unsubscribe();
+    pointcloud_subscription.reset();
+    static int unsubscribe_count = 0;
+    RCLCPP_INFO(
+      rclcpp::get_logger("autoware_perception_plugin"), "Unsubscribe called %d times",
+      unsubscribe_count++);
   }
 
   void clear_markers() { m_marker_common.clearMarkers(); }
@@ -178,6 +365,61 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     m_marker_common.deleteMarker(marker_id);
   }
 
+  void add_pointcloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud)
+  {
+    point_cloud_common->addMessage(cloud);
+  }
+
+  // transform detected object pose to target frame and return bool result
+  bool transformObjects(
+    const MsgT & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer,
+    MsgT & output_msg)
+  {
+    output_msg = input_msg;
+
+    // transform to world coordinate
+    if (input_msg.header.frame_id != target_frame_id) {
+      output_msg.header.frame_id = target_frame_id;
+      tf2::Transform tf_target2objects_world;
+      tf2::Transform tf_target2objects;
+      tf2::Transform tf_objects_world2objects;
+      {
+        const auto ros_target2objects_world = getTransform(
+          tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp);
+        if (!ros_target2objects_world) {
+          return false;
+        }
+        tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world);
+      }
+      for (auto & object : output_msg.objects) {
+        tf2::fromMsg(object.kinematics.pose_with_covariance.pose, tf_objects_world2objects);
+        tf_target2objects = tf_target2objects_world * tf_objects_world2objects;
+        tf2::toMsg(tf_target2objects, object.kinematics.pose_with_covariance.pose);
+      }
+    }
+    return true;
+  }
+
+  // get transformation from tf2
+  boost::optional<geometry_msgs::msg::Transform> getTransform(
+    const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id,
+    const std::string & target_frame_id, const rclcpp::Time & time)
+  {
+    try {
+      geometry_msgs::msg::TransformStamped self_transform_stamped;
+      self_transform_stamped = tf_buffer.lookupTransform(
+        target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5));
+      return self_transform_stamped.transform;
+    } catch (tf2::TransformException & ex) {
+      RCLCPP_WARN_STREAM(rclcpp::get_logger("autoware_perception_plugin"), ex.what());
+      return boost::none;
+    }
+  }
+
+  // variables for transfer detected objects information between callbacks
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener{nullptr};
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer;
+
 protected:
   /// \brief Convert given shape msg into a Marker
   /// \tparam ClassificationContainerT List type with ObjectClassificationMsg
@@ -523,6 +765,114 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     }
   }
 
+  // helper function to get radius for kd-search
+  std::optional<float> getMaxRadius(object_info & object)
+  {
+    if (object.shape.type == Shape::BOUNDING_BOX || object.shape.type == Shape::CYLINDER) {
+      return std::hypot(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f);
+    }
+
+    if (object.shape.type == Shape::POLYGON) {
+      float max_dist = 0.0;
+      for (const auto & point : object.shape.footprint.points) {
+        const float dist = std::hypot(point.x, point.y);
+        max_dist = std::max(max_dist, dist);
+      }
+      return max_dist;
+    }
+
+    return std::nullopt;
+  }
+
+  void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg)
+  {
+    if (!m_publish_objs_pointcloud->getBool()) {
+      return;
+    }
+
+    pointCloudBuffer_mutex.lock();
+    pointCloudBuffer.push_front(input_pointcloud_msg);
+    if (pointCloudBuffer.size() > 5) {
+      pointCloudBuffer.pop_back();
+    }
+    pointCloudBuffer_mutex.unlock();
+  }
+
+  void filterPolygon(
+    const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr & in_cloud,
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud, const object_info & object)
+  {
+    pcl::PointCloud<pcl::PointXYZRGB> filtered_cloud;
+    std::vector<pcl::Vertices> vertices_array;
+    pcl::Vertices vertices;
+
+    Polygon2d poly2d = autoware::universe_utils::toPolygon2d(object.position, object.shape);
+    if (boost::geometry::is_empty(poly2d)) {
+      return;
+    }
+
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr hull_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+    for (size_t i = 0; i < poly2d.outer().size(); ++i) {
+      vertices.vertices.emplace_back(i);
+      vertices_array.emplace_back(vertices);
+      hull_cloud->emplace_back(
+        static_cast<float>(poly2d.outer().at(i).x()), static_cast<float>(poly2d.outer().at(i).y()),
+        static_cast<float>(0.0));
+    }
+
+    pcl::CropHull<pcl::PointXYZRGB> crop_hull_filter;
+    crop_hull_filter.setInputCloud(in_cloud);
+    crop_hull_filter.setDim(2);
+    crop_hull_filter.setHullIndices(vertices_array);
+    crop_hull_filter.setHullCloud(hull_cloud);
+    crop_hull_filter.setCropOutside(true);
+
+    crop_hull_filter.filter(filtered_cloud);
+
+    const std_msgs::msg::ColorRGBA color_rgba =
+      get_color_rgba(object.classification);  // need to be converted to int
+
+    for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it) {
+      cloud_it->r = std::max(0, std::min(255, static_cast<int>(floor(color_rgba.r * 256.0))));
+      cloud_it->g = std::max(0, std::min(255, static_cast<int>(floor(color_rgba.g * 256.0))));
+      cloud_it->b = std::max(0, std::min(255, static_cast<int>(floor(color_rgba.b * 256.0))));
+    }
+
+    *out_cloud += filtered_cloud;
+  }
+
+  // Function that returns the pointcloud with the nearest timestamp from a buffer of pointclouds
+  sensor_msgs::msg::PointCloud2 getNearestPointCloud(
+    std::deque<sensor_msgs::msg::PointCloud2> & buffer, const rclcpp::Time & timestamp)
+  {
+    if (buffer.empty()) {
+      // Handle the case where the buffer is empty
+      throw std::runtime_error("Buffer is empty");
+    }
+    sensor_msgs::msg::PointCloud2 result = buffer.front();
+    rclcpp::Duration diff = timestamp - rclcpp::Time(result.header.stamp);
+    pointCloudBuffer_mutex.lock();
+    for (size_t i = 0; i != buffer.size(); i++) {
+      if (diff.nanoseconds() > (timestamp - rclcpp::Time(buffer[i].header.stamp)).nanoseconds()) {
+        diff = timestamp - rclcpp::Time(buffer[i].header.stamp);
+        result = buffer[i];
+      }
+    }
+    pointCloudBuffer_mutex.unlock();
+
+    return result;
+  }
+
+  rviz_common::properties::RosTopicProperty * m_default_pointcloud_topic;
+  // Property to enable/disable objects pointcloud publishing
+  rviz_common::properties::BoolProperty * m_publish_objs_pointcloud;
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_subscription;
+  // plugin to visualize pointclouds
+  std::unique_ptr<rviz_default_plugins::PointCloudCommon> point_cloud_common;
+  rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node;
+  std::deque<sensor_msgs::msg::PointCloud2> pointCloudBuffer;
+  std::mutex pointCloudBuffer_mutex;
+
 private:
   // All rviz plugins should have this. Should be initialized with pointer to this class
   MarkerCommon m_marker_common;
@@ -571,6 +921,8 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
   std::string m_default_topic;
 
   std::vector<std_msgs::msg::ColorRGBA> predicted_path_colors;
+  rclcpp::QoS qos_profile_points;
+  rviz_common::properties::QosProfileProperty * qos_profile_points_property;
 };
 }  // namespace object_detection
 }  // namespace rviz_plugins
diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp
index 1445f02e34290..2947b2a36fa40 100644
--- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp
+++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp
@@ -127,6 +127,15 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay
 
   void update(float wall_dt, float ros_dt) override;
 
+  bool transformObjects(
+    const autoware_perception_msgs::msg::PredictedObjects & input_msg,
+    const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer,
+    autoware_perception_msgs::msg::PredictedObjects & output_msg);
+
+  void processPointCloud(
+    const PredictedObjects::ConstSharedPtr & input_objs_msg,
+    const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg);
+
   std::unordered_map<boost::uuids::uuid, int32_t, boost::hash<boost::uuids::uuid>> id_map;
   // std::unordered_map<boost::uuids::uuid, int32_t> id_map;
   std::list<int32_t> unused_marker_ids;
diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp
index 9ccaa5cd150f6..379b89576b8d9 100644
--- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp
+++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp
@@ -16,6 +16,7 @@
 
 #include "autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp"
 
+#include <autoware_perception_msgs/msg/tracked_object.hpp>
 #include <autoware_perception_msgs/msg/tracked_objects.hpp>
 
 #include <boost/uuid/uuid.hpp>
@@ -23,6 +24,7 @@
 
 #include <list>
 #include <map>
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -105,6 +107,10 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay
     return id_map.at(uuid);
   }
 
+  void processPointCloud(
+    const TrackedObjects::ConstSharedPtr & input_objs_msg,
+    const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg);
+
   std::map<boost::uuids::uuid, int32_t> id_map;
   std::list<int32_t> unused_marker_ids;
   int32_t marker_id = 0;
diff --git a/common/autoware_perception_rviz_plugin/package.xml b/common/autoware_perception_rviz_plugin/package.xml
index 460186c33b7d8..92bf2a2431bf7 100644
--- a/common/autoware_perception_rviz_plugin/package.xml
+++ b/common/autoware_perception_rviz_plugin/package.xml
@@ -19,8 +19,18 @@
   <build_depend>qtbase5-dev</build_depend>
 
   <depend>autoware_perception_msgs</depend>
+  <depend>autoware_universe_utils</depend>
+  <depend>pcl_conversions</depend>
+  <depend>perception_utils</depend>
   <depend>rviz_common</depend>
   <depend>rviz_default_plugins</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_sensor_msgs</depend>
+  <!-- <depend>pcl_common</depend> -->
 
   <exec_depend>libqt5-widgets</exec_depend>
   <exec_depend>rviz2</exec_depend>
diff --git a/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp
index 9dd0b2923f09f..71e31132eedcd 100644
--- a/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp
+++ b/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp
@@ -24,7 +24,11 @@ namespace rviz_plugins
 {
 namespace object_detection
 {
-DetectedObjectsDisplay::DetectedObjectsDisplay() : ObjectPolygonDisplayBase("detected_objects")
+
+DetectedObjectsDisplay::DetectedObjectsDisplay()
+: ObjectPolygonDisplayBase(
+    "/perception/object_recognition/detection/objects",
+    "/perception/obstacle_segmentation/pointcloud")
 {
 }
 
@@ -153,6 +157,70 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg)
       add_marker(marker_ptr);
     }
   }
+
+  if (pointCloudBuffer.empty()) {
+    return;
+  }
+  // pointcloud pub
+  sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud =
+    std::make_shared<sensor_msgs::msg::PointCloud2>(
+      getNearestPointCloud(pointCloudBuffer, msg->header.stamp));
+  processPointCloud(msg, closest_pointcloud);
+}
+
+void DetectedObjectsDisplay::processPointCloud(
+  const DetectedObjects::ConstSharedPtr & input_objs_msg,
+  const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg)
+{
+  if (!m_publish_objs_pointcloud->getBool()) {
+    return;
+  }
+  // Transform to pointcloud frame
+  autoware_perception_msgs::msg::DetectedObjects transformed_objects;
+  if (!transformObjects(
+        *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) {
+    return;
+  }
+  // convert to pcl pointcloud
+  pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::fromROSMsg(*input_pointcloud_msg, *temp_cloud);
+  // Create a new point cloud with RGB color information and copy data from input cloud
+  pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
+  pcl::copyPointCloud(*temp_cloud, *colored_cloud);
+  // Create Kd-tree to search neighbor pointcloud to reduce cost.
+  pcl::search::Search<pcl::PointXYZRGB>::Ptr kdtree =
+    pcl::make_shared<pcl::search::KdTree<pcl::PointXYZRGB>>(false);
+  kdtree->setInputCloud(colored_cloud);
+
+  pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
+
+  for (const auto & object : transformed_objects.objects) {
+    std::vector<autoware_perception_msgs::msg::ObjectClassification> labels = object.classification;
+    object_info unified_object = {
+      object.shape, object.kinematics.pose_with_covariance.pose, object.classification};
+
+    const auto search_radius = getMaxRadius(unified_object);
+    // Search neighbor pointcloud to reduce cost.
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr neighbor_pointcloud(
+      new pcl::PointCloud<pcl::PointXYZRGB>);
+    std::vector<int> indices;
+    std::vector<float> distances;
+    kdtree->radiusSearch(
+      toPCL(unified_object.position.position), search_radius.value(), indices, distances);
+    for (const auto & index : indices) {
+      neighbor_pointcloud->push_back(colored_cloud->at(index));
+    }
+
+    filterPolygon(neighbor_pointcloud, out_cloud, unified_object);
+  }
+
+  sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr(
+    new sensor_msgs::msg::PointCloud2);
+  pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr);
+
+  output_pointcloud_msg_ptr->header = input_pointcloud_msg->header;
+
+  add_pointcloud(output_pointcloud_msg_ptr);
 }
 
 }  // namespace object_detection
diff --git a/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp
index 1d06454582a2f..4f3565bddf007 100644
--- a/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp
+++ b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp
@@ -549,11 +549,15 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr(
 {
   auto marker_ptr = std::make_shared<Marker>();
   marker_ptr->ns = std::string("shape");
+  marker_ptr->scale.x = line_width;
 
   using autoware_perception_msgs::msg::Shape;
   if (shape_msg.type == Shape::BOUNDING_BOX) {
-    marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
-    calc_2d_bounding_box_bottom_line_list(shape_msg, marker_ptr->points);
+    marker_ptr->type = visualization_msgs::msg::Marker::TRIANGLE_LIST;
+    calc_2d_bounding_box_bottom_triangle_list(shape_msg, marker_ptr->points);
+    marker_ptr->scale.x = 1.0;  // Set scale to 1 for TRIANGLE_LIST
+    marker_ptr->scale.y = 1.0;  // Set scale to 1 for TRIANGLE_LIST
+    marker_ptr->scale.z = 1.0;  // Set scale to 1 for TRIANGLE_LIST
     if (is_orientation_available) {
       calc_2d_bounding_box_bottom_direction_line_list(shape_msg, marker_ptr->points);
     } else {
@@ -573,12 +577,82 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr(
   marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
   marker_ptr->pose = to_pose(centroid, orientation);
   marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15);
-  marker_ptr->scale.x = line_width;
+
+  marker_ptr->mesh_use_embedded_materials = false;  // Ensure the material is not embedded
+
   marker_ptr->color = color_rgba;
+  marker_ptr->color.a = 0.75f;
 
   return marker_ptr;
 }
 
+void calc_2d_bounding_box_bottom_triangle_list(
+  const autoware_perception_msgs::msg::Shape & shape,
+  std::vector<geometry_msgs::msg::Point> & points)
+{
+  const double length_half = shape.dimensions.x * 0.5;
+  const double width_half = shape.dimensions.y * 0.5;
+  const double height_half = shape.dimensions.z * 0.5;
+
+  // Define Boost.Geometry types
+  using BoostPoint = boost::geometry::model::d2::point_xy<double>;
+  using BoostPolygon = boost::geometry::model::polygon<BoostPoint>;
+
+  // Create a rectangle polygon
+  BoostPolygon boost_polygon;
+  boost::geometry::append(boost_polygon.outer(), BoostPoint(length_half, width_half));
+  boost::geometry::append(boost_polygon.outer(), BoostPoint(length_half, -width_half));
+  boost::geometry::append(boost_polygon.outer(), BoostPoint(-length_half, -width_half));
+  boost::geometry::append(boost_polygon.outer(), BoostPoint(-length_half, width_half));
+  boost::geometry::append(boost_polygon.outer(), BoostPoint(length_half, width_half));
+  boost::geometry::correct(boost_polygon);
+
+  // Define buffer strategy with larger rounded corners and smoother approximation
+  boost::geometry::strategy::buffer::distance_symmetric<double> distance_strategy(
+    0.4);  // Adjust distance as needed
+  boost::geometry::strategy::buffer::join_round join_strategy(
+    16);  // Increase points per circle for smoother roundness
+  boost::geometry::strategy::buffer::end_flat end_strategy;
+  boost::geometry::strategy::buffer::point_circle point_strategy(16);  // Points per circle
+  boost::geometry::strategy::buffer::side_straight side_strategy;
+
+  // Create buffered polygon with rounded corners
+  std::vector<BoostPolygon> buffered_polygons;
+  boost::geometry::buffer(
+    boost_polygon, buffered_polygons, distance_strategy, side_strategy, join_strategy, end_strategy,
+    point_strategy);
+
+  if (buffered_polygons.empty()) {
+    RCLCPP_WARN(rclcpp::get_logger("ObjectPolygonDisplayBase"), "Buffering produced no output.");
+    return;
+  }
+
+  // Convert buffered polygon to RViz marker points
+  const auto & outer_ring = buffered_polygons.front().outer();
+  std::vector<geometry_msgs::msg::Point> polygon_points;
+
+  for (const auto & point : outer_ring) {
+    geometry_msgs::msg::Point p;
+    p.x = point.x();
+    p.y = point.y();
+    p.z = -height_half;  // Use the bottom part of the shape
+    polygon_points.push_back(p);
+  }
+
+  // Create triangles from polygon points
+  for (size_t i = 1; i < polygon_points.size() - 1; ++i) {
+    // Add front face
+    points.push_back(polygon_points[0]);
+    points.push_back(polygon_points[i]);
+    points.push_back(polygon_points[i + 1]);
+
+    // Add back face
+    points.push_back(polygon_points[0]);
+    points.push_back(polygon_points[i + 1]);
+    points.push_back(polygon_points[i]);
+  }
+}
+
 void calc_line_list_from_points(
   const double point_list[][3], const int point_pairs[][2], const int & num_pairs,
   std::vector<geometry_msgs::msg::Point> & points)
@@ -677,23 +751,71 @@ void calc_2d_bounding_box_bottom_line_list(
   const double length_half = shape.dimensions.x * 0.5;
   const double width_half = shape.dimensions.y * 0.5;
   const double height_half = shape.dimensions.z * 0.5;
-  geometry_msgs::msg::Point point;
 
-  // bounding box corner points
-  // top surface, clockwise
-  const double point_list[4][3] = {
-    {length_half, width_half, -height_half},
-    {length_half, -width_half, -height_half},
-    {-length_half, -width_half, -height_half},
-    {-length_half, width_half, -height_half},
-  };
-  const int point_pairs[4][2] = {
-    {0, 1},
-    {1, 2},
-    {2, 3},
-    {3, 0},
-  };
-  calc_line_list_from_points(point_list, point_pairs, 4, points);
+  // Define Boost.Geometry types
+  using BoostPoint = boost::geometry::model::d2::point_xy<double>;
+  using BoostPolygon = boost::geometry::model::polygon<BoostPoint>;
+
+  // Create a rectangle polygon
+  BoostPolygon boost_polygon;
+  boost::geometry::append(boost_polygon.outer(), BoostPoint(length_half, width_half));
+  boost::geometry::append(boost_polygon.outer(), BoostPoint(length_half, -width_half));
+  boost::geometry::append(boost_polygon.outer(), BoostPoint(-length_half, -width_half));
+  boost::geometry::append(boost_polygon.outer(), BoostPoint(-length_half, width_half));
+  boost::geometry::append(boost_polygon.outer(), BoostPoint(length_half, width_half));
+  boost::geometry::correct(boost_polygon);
+
+  // Define buffer strategy with rounded corners
+  boost::geometry::strategy::buffer::distance_symmetric<double> distance_strategy(
+    0.4);  // Adjust distance as needed
+  boost::geometry::strategy::buffer::join_round join_strategy(
+    16);  // Adjust points per circle for roundness
+  boost::geometry::strategy::buffer::end_flat end_strategy;
+  boost::geometry::strategy::buffer::point_circle point_strategy(16);  // Points per circle
+  boost::geometry::strategy::buffer::side_straight side_strategy;
+
+  // Create buffered polygon with rounded corners
+  std::vector<BoostPolygon> buffered_polygons;
+  boost::geometry::buffer(
+    boost_polygon, buffered_polygons, distance_strategy, side_strategy, join_strategy, end_strategy,
+    point_strategy);
+
+  if (buffered_polygons.empty()) {
+    RCLCPP_WARN(rclcpp::get_logger("ObjectPolygonDisplayBase"), "Buffering produced no output.");
+    return;
+  }
+
+  // Convert buffered polygon to RViz marker points
+  const auto & outer_ring = buffered_polygons.front().outer();
+  for (size_t i = 0; i < outer_ring.size() - 1; ++i) {  // -1 to avoid duplicating the closing point
+    geometry_msgs::msg::Point point;
+    point.x = outer_ring[i].x();
+    point.y = outer_ring[i].y();
+    point.z = -height_half;  // Use the bottom part of the shape
+    points.push_back(point);
+
+    point.x = outer_ring[i + 1].x();
+    point.y = outer_ring[i + 1].y();
+    point.z = -height_half;  // Use the bottom part of the shape
+    points.push_back(point);
+  }
+
+  // Closing the polygon
+  if (!outer_ring.empty()) {
+    geometry_msgs::msg::Point point;
+    point.x = outer_ring.back().x();
+    point.y = outer_ring.back().y();
+    point.z = -height_half;
+    points.push_back(point);
+
+    point.x = outer_ring.front().x();
+    point.y = outer_ring.front().y();
+    point.z = -height_half;
+    points.push_back(point);
+  }
+
+  RCLCPP_INFO(
+    rclcpp::get_logger("ObjectPolygonDisplayBase"), "Marker points size: %zu", points.size());
 }
 
 void calc_2d_bounding_box_bottom_direction_line_list(
diff --git a/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp
index d11aba912854d..668d9a30f4414 100644
--- a/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp
+++ b/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp
@@ -23,7 +23,8 @@ namespace rviz_plugins
 {
 namespace object_detection
 {
-PredictedObjectsDisplay::PredictedObjectsDisplay() : ObjectPolygonDisplayBase("tracks")
+PredictedObjectsDisplay::PredictedObjectsDisplay()
+: ObjectPolygonDisplayBase("predictions", "/perception/obstacle_segmentation/pointcloud")
 {
   max_num_threads = 1;  // hard code the number of threads to be created
 
@@ -260,6 +261,15 @@ std::vector<visualization_msgs::msg::Marker::SharedPtr> PredictedObjectsDisplay:
     }
   }
 
+  if (pointCloudBuffer.empty()) {
+    return markers;
+  }
+  // pointcloud pub
+  sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud =
+    std::make_shared<sensor_msgs::msg::PointCloud2>(
+      getNearestPointCloud(pointCloudBuffer, msg->header.stamp));
+  processPointCloud(msg, closest_pointcloud);
+
   return markers;
 }
 
@@ -299,6 +309,97 @@ void PredictedObjectsDisplay::update(float wall_dt, float ros_dt)
     wall_dt, ros_dt);
 }
 
+bool PredictedObjectsDisplay::transformObjects(
+  const autoware_perception_msgs::msg::PredictedObjects & input_msg,
+  const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer,
+  autoware_perception_msgs::msg::PredictedObjects & output_msg)
+{
+  output_msg = input_msg;
+
+  // transform to world coordinate
+  if (input_msg.header.frame_id != target_frame_id) {
+    output_msg.header.frame_id = target_frame_id;
+    tf2::Transform tf_target2objects_world;
+    tf2::Transform tf_target2objects;
+    tf2::Transform tf_objects_world2objects;
+    {
+      const auto ros_target2objects_world =
+        getTransform(tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp);
+      if (!ros_target2objects_world) {
+        return false;
+      }
+      tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world);
+    }
+    for (auto & object : output_msg.objects) {
+      tf2::fromMsg(object.kinematics.initial_pose_with_covariance.pose, tf_objects_world2objects);
+      tf_target2objects = tf_target2objects_world * tf_objects_world2objects;
+      tf2::toMsg(tf_target2objects, object.kinematics.initial_pose_with_covariance.pose);
+    }
+  }
+  return true;
+}
+
+void PredictedObjectsDisplay::processPointCloud(
+  const PredictedObjects::ConstSharedPtr & input_objs_msg,
+  const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg)
+{
+  if (!m_publish_objs_pointcloud->getBool()) {
+    return;
+  }
+  // Transform to pointcloud frame
+  autoware_perception_msgs::msg::PredictedObjects transformed_objects;
+  if (!transformObjects(
+        *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) {
+    return;
+  }
+  // convert to pcl pointcloud
+  pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::fromROSMsg(*input_pointcloud_msg, *temp_cloud);
+  // Create a new point cloud with RGB color information and copy data from input cloud
+  pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
+  pcl::copyPointCloud(*temp_cloud, *colored_cloud);
+  // Create Kd-tree to search neighbor pointcloud to reduce cost.
+  pcl::search::Search<pcl::PointXYZRGB>::Ptr kdtree =
+    pcl::make_shared<pcl::search::KdTree<pcl::PointXYZRGB>>(false);
+
+  // check if the pointcloud is empty to not call setInputCloud with an empty pointcloud
+  if (colored_cloud->empty()) {
+    return;
+  }
+
+  kdtree->setInputCloud(colored_cloud);
+
+  pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
+
+  for (const auto & object : transformed_objects.objects) {
+    std::vector<autoware_perception_msgs::msg::ObjectClassification> labels = object.classification;
+    object_info unified_object = {
+      object.shape, object.kinematics.initial_pose_with_covariance.pose, object.classification};
+
+    const auto search_radius = getMaxRadius(unified_object);
+    // Search neighbor pointcloud to reduce cost.
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr neighbor_pointcloud(
+      new pcl::PointCloud<pcl::PointXYZRGB>);
+    std::vector<int> indices;
+    std::vector<float> distances;
+    kdtree->radiusSearch(
+      toPCL(unified_object.position.position), search_radius.value(), indices, distances);
+    for (const auto & index : indices) {
+      neighbor_pointcloud->push_back(colored_cloud->at(index));
+    }
+
+    filterPolygon(neighbor_pointcloud, out_cloud, unified_object);
+  }
+
+  sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr(
+    new sensor_msgs::msg::PointCloud2);
+  pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr);
+
+  output_pointcloud_msg_ptr->header = input_pointcloud_msg->header;
+
+  add_pointcloud(output_pointcloud_msg_ptr);
+}
+
 }  // namespace object_detection
 }  // namespace rviz_plugins
 }  // namespace autoware
diff --git a/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp
index 214ed9ce70e63..d85637a23b4a1 100644
--- a/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp
+++ b/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp
@@ -24,7 +24,11 @@ namespace rviz_plugins
 {
 namespace object_detection
 {
-TrackedObjectsDisplay::TrackedObjectsDisplay() : ObjectPolygonDisplayBase("tracks")
+
+TrackedObjectsDisplay::TrackedObjectsDisplay()
+: ObjectPolygonDisplayBase(
+    "/perception/object_recognition/tracking/objects",
+    "/perception/obstacle_segmentation/pointcloud")
 {
   // Option for selective visualization by object dynamics
   m_select_object_dynamics_property = new rviz_common::properties::EnumProperty(
@@ -203,6 +207,70 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg)
       add_marker(marker_ptr);
     }
   }
+
+  if (pointCloudBuffer.empty()) {
+    return;
+  }
+  // pointcloud pub
+  sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud =
+    std::make_shared<sensor_msgs::msg::PointCloud2>(
+      getNearestPointCloud(pointCloudBuffer, msg->header.stamp));
+  processPointCloud(msg, closest_pointcloud);
+}
+
+void TrackedObjectsDisplay::processPointCloud(
+  const TrackedObjects::ConstSharedPtr & input_objs_msg,
+  const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg)
+{
+  if (!m_publish_objs_pointcloud->getBool()) {
+    return;
+  }
+  // Transform to pointcloud frame
+  autoware_perception_msgs::msg::TrackedObjects transformed_objects;
+  if (!transformObjects(
+        *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) {
+    return;
+  }
+  // convert to pcl pointcloud
+  pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::fromROSMsg(*input_pointcloud_msg, *temp_cloud);
+  // Create a new point cloud with RGB color information and copy data from input cloud
+  pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
+  pcl::copyPointCloud(*temp_cloud, *colored_cloud);
+  // Create Kd-tree to search neighbor pointcloud to reduce cost.
+  pcl::search::Search<pcl::PointXYZRGB>::Ptr kdtree =
+    pcl::make_shared<pcl::search::KdTree<pcl::PointXYZRGB>>(false);
+  kdtree->setInputCloud(colored_cloud);
+
+  pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
+
+  for (const auto & object : transformed_objects.objects) {
+    std::vector<autoware_perception_msgs::msg::ObjectClassification> labels = object.classification;
+    object_info unified_object = {
+      object.shape, object.kinematics.pose_with_covariance.pose, object.classification};
+
+    const auto search_radius = getMaxRadius(unified_object);
+    // Search neighbor pointcloud to reduce cost.
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr neighbor_pointcloud(
+      new pcl::PointCloud<pcl::PointXYZRGB>);
+    std::vector<int> indices;
+    std::vector<float> distances;
+    kdtree->radiusSearch(
+      toPCL(unified_object.position.position), search_radius.value(), indices, distances);
+    for (const auto & index : indices) {
+      neighbor_pointcloud->push_back(colored_cloud->at(index));
+    }
+
+    filterPolygon(neighbor_pointcloud, out_cloud, unified_object);
+  }
+
+  sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr(
+    new sensor_msgs::msg::PointCloud2);
+  pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr);
+
+  output_pointcloud_msg_ptr->header = input_pointcloud_msg->header;
+
+  add_pointcloud(output_pointcloud_msg_ptr);
 }
 
 }  // namespace object_detection