diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp
index 2b2057af9cc12..09ad0a4d554a1 100644
--- a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp
+++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp
@@ -52,8 +52,10 @@
 #ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_
 #define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_
 
+#include <autoware/universe_utils/math/unit_conversion.hpp>
 #include <nav2_costmap_2d/costmap_2d.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
 
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
@@ -85,11 +87,52 @@ class OccupancyGridMapInterface : public nav2_costmap_2d::Costmap2D
 
   virtual void initRosParam(rclcpp::Node & node) = 0;
 
+  void setHeightLimit(const double min_height, const double max_height);
+
+  double min_height_;
+  double max_height_;
+
+  void setFieldOffsets(const PointCloud2 & input_raw, const PointCloud2 & input_obstacle);
+
+  int x_offset_raw_;
+  int y_offset_raw_;
+  int z_offset_raw_;
+  int x_offset_obstacle_;
+  int y_offset_obstacle_;
+  int z_offset_obstacle_;
+  bool offset_initialized_;
+
+  const double min_angle_ = autoware::universe_utils::deg2rad(-180.0);
+  const double max_angle_ = autoware::universe_utils::deg2rad(180.0);
+  const double angle_increment_inv_ = 1.0 / autoware::universe_utils::deg2rad(0.1);
+
+  Eigen::Matrix4f mat_map_, mat_scan_;
+
+  bool isPointValid(const Eigen::Vector4f & pt) const
+  {
+    // Apply height filter and exclude invalid points
+    return min_height_ < pt[2] && pt[2] < max_height_ && std::isfinite(pt[0]) &&
+           std::isfinite(pt[1]) && std::isfinite(pt[2]);
+  }
+  // Transform pt to (pt_map, pt_scan), then calculate angle_bin_index and range
+  void transformPointAndCalculate(
+    const Eigen::Vector4f & pt, Eigen::Vector4f & pt_map, int & angle_bin_index,
+    double & range) const
+  {
+    pt_map = mat_map_ * pt;
+    Eigen::Vector4f pt_scan(mat_scan_ * pt_map);
+    const double angle = atan2(pt_scan[1], pt_scan[0]);
+    angle_bin_index = (angle - min_angle_) * angle_increment_inv_;
+    range = std::sqrt(pt_scan[1] * pt_scan[1] + pt_scan[0] * pt_scan[0]);
+  }
+
 private:
   bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) const;
 
   rclcpp::Logger logger_{rclcpp::get_logger("pointcloud_based_occupancy_grid_map")};
   rclcpp::Clock clock_{RCL_ROS_TIME};
+
+  double resolution_inv_;
 };
 
 }  // namespace costmap_2d
diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp
index 9dfd8b8689123..3301fd1987bd3 100644
--- a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp
+++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp
@@ -36,6 +36,7 @@ class OccupancyGridMapFixedBlindSpot : public OccupancyGridMapInterface
 
   using OccupancyGridMapInterface::raytrace;
   using OccupancyGridMapInterface::setCellValue;
+  using OccupancyGridMapInterface::setFieldOffsets;
   using OccupancyGridMapInterface::updateOrigin;
 
   void initRosParam(rclcpp::Node & node) override;
diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp
index 1f0f92d933ff3..7569a60b36466 100644
--- a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp
+++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp
@@ -40,6 +40,7 @@ class OccupancyGridMapProjectiveBlindSpot : public OccupancyGridMapInterface
 
   using OccupancyGridMapInterface::raytrace;
   using OccupancyGridMapInterface::setCellValue;
+  using OccupancyGridMapInterface::setFieldOffsets;
   using OccupancyGridMapInterface::updateOrigin;
 
   void initRosParam(rclcpp::Node & node) override;
diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp
index 50b470d904ef6..a8288d2720f48 100644
--- a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp
+++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp
@@ -97,6 +97,8 @@ void transformPointcloud(
   const sensor_msgs::msg::PointCloud2 & input, const geometry_msgs::msg::Pose & pose,
   sensor_msgs::msg::PointCloud2 & output);
 
+Eigen::Matrix4f getTransformMatrix(const geometry_msgs::msg::Pose & pose);
+
 bool cropPointcloudByHeight(
   const sensor_msgs::msg::PointCloud2 & input, const tf2_ros::Buffer & tf2,
   const std::string & target_frame, const float min_height, const float max_height,
diff --git a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp
index 5e9b5598c8f63..39855ec36260c 100644
--- a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp
+++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp
@@ -80,17 +80,21 @@ OccupancyGridMapInterface::OccupancyGridMapInterface(
   const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution)
 : Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION)
 {
+  min_height_ = -std::numeric_limits<double>::infinity();
+  max_height_ = std::numeric_limits<double>::infinity();
+  resolution_inv_ = 1.0 / resolution_;
+  offset_initialized_ = false;
 }
 
-bool OccupancyGridMapInterface::worldToMap(
+inline bool OccupancyGridMapInterface::worldToMap(
   double wx, double wy, unsigned int & mx, unsigned int & my) const
 {
   if (wx < origin_x_ || wy < origin_y_) {
     return false;
   }
 
-  mx = static_cast<int>(std::floor((wx - origin_x_) / resolution_));
-  my = static_cast<int>(std::floor((wy - origin_y_) / resolution_));
+  mx = static_cast<int>(std::floor((wx - origin_x_) * resolution_inv_));
+  my = static_cast<int>(std::floor((wy - origin_y_) * resolution_inv_));
 
   if (mx < size_x_ && my < size_y_) {
     return true;
@@ -232,5 +236,23 @@ void OccupancyGridMapInterface::raytrace(
   raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);
 }
 
+void OccupancyGridMapInterface::setHeightLimit(const double min_height, const double max_height)
+{
+  min_height_ = min_height;
+  max_height_ = max_height;
+}
+
+void OccupancyGridMapInterface::setFieldOffsets(
+  const PointCloud2 & input_raw, const PointCloud2 & input_obstacle)
+{
+  x_offset_raw_ = input_raw.fields[pcl::getFieldIndex(input_raw, "x")].offset;
+  y_offset_raw_ = input_raw.fields[pcl::getFieldIndex(input_raw, "y")].offset;
+  z_offset_raw_ = input_raw.fields[pcl::getFieldIndex(input_raw, "z")].offset;
+  x_offset_obstacle_ = input_obstacle.fields[pcl::getFieldIndex(input_obstacle, "x")].offset;
+  y_offset_obstacle_ = input_obstacle.fields[pcl::getFieldIndex(input_obstacle, "y")].offset;
+  z_offset_obstacle_ = input_obstacle.fields[pcl::getFieldIndex(input_obstacle, "z")].offset;
+  offset_initialized_ = true;
+}
+
 }  // namespace costmap_2d
 }  // namespace autoware::occupancy_grid_map
diff --git a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp
index dffbda18d0c8a..c8af97fcf3ba0 100644
--- a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp
+++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp
@@ -58,21 +58,20 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud(
   const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud,
   const Pose & robot_pose, const Pose & scan_origin)
 {
-  constexpr double min_angle = autoware::universe_utils::deg2rad(-180.0);
-  constexpr double max_angle = autoware::universe_utils::deg2rad(180.0);
-  constexpr double angle_increment = autoware::universe_utils::deg2rad(0.1);
-  const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1 /*margin*/);
-
-  // Transform from base_link to map frame
-  PointCloud2 map_raw_pointcloud, map_obstacle_pointcloud;  // point cloud in map frame
-  utils::transformPointcloud(raw_pointcloud, robot_pose, map_raw_pointcloud);
-  utils::transformPointcloud(obstacle_pointcloud, robot_pose, map_obstacle_pointcloud);
-
-  // Transform from map frame to scan frame
-  PointCloud2 scan_raw_pointcloud, scan_obstacle_pointcloud;      // point cloud in scan frame
+  const size_t angle_bin_size =
+    ((max_angle_ - min_angle_) * angle_increment_inv_) + size_t(1 /*margin*/);
+
+  // Transform Matrix from base_link to map frame
+  mat_map_ = utils::getTransformMatrix(robot_pose);
+
   const auto scan2map_pose = utils::getInversePose(scan_origin);  // scan -> map transform pose
-  utils::transformPointcloud(map_raw_pointcloud, scan2map_pose, scan_raw_pointcloud);
-  utils::transformPointcloud(map_obstacle_pointcloud, scan2map_pose, scan_obstacle_pointcloud);
+
+  // Transform Matrix from map frame to scan frame
+  mat_scan_ = utils::getTransformMatrix(scan2map_pose);
+
+  if (!offset_initialized_) {
+    setFieldOffsets(raw_pointcloud, obstacle_pointcloud);
+  }
 
   // Create angle bins and sort by distance
   struct BinInfo
@@ -86,41 +85,73 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud(
     double wx;
     double wy;
   };
-  std::vector</*angle bin*/ std::vector<BinInfo>> obstacle_pointcloud_angle_bins;
-  std::vector</*angle bin*/ std::vector<BinInfo>> raw_pointcloud_angle_bins;
-  obstacle_pointcloud_angle_bins.resize(angle_bin_size);
-  raw_pointcloud_angle_bins.resize(angle_bin_size);
-  for (PointCloud2ConstIterator<float> iter_x(scan_raw_pointcloud, "x"),
-       iter_y(scan_raw_pointcloud, "y"), iter_wx(map_raw_pointcloud, "x"),
-       iter_wy(map_raw_pointcloud, "y");
-       iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_wx, ++iter_wy) {
-    const double angle = atan2(*iter_y, *iter_x);
-    const int angle_bin_index = (angle - min_angle) / angle_increment;
-    raw_pointcloud_angle_bins.at(angle_bin_index)
-      .push_back(BinInfo(std::hypot(*iter_y, *iter_x), *iter_wx, *iter_wy));
+
+  std::vector</*angle bin*/ std::vector<BinInfo>> obstacle_pointcloud_angle_bins(angle_bin_size);
+  std::vector</*angle bin*/ std::vector<BinInfo>> raw_pointcloud_angle_bins(angle_bin_size);
+
+  const size_t raw_pointcloud_size = raw_pointcloud.width * raw_pointcloud.height;
+  const size_t obstacle_pointcloud_size = obstacle_pointcloud.width * obstacle_pointcloud.height;
+
+  // Reserve a certain amount of memory in advance for performance reasons
+  const size_t raw_reserve_size = raw_pointcloud_size / angle_bin_size;
+  const size_t obstacle_reserve_size = obstacle_pointcloud_size / angle_bin_size;
+  for (size_t i = 0; i < angle_bin_size; i++) {
+    raw_pointcloud_angle_bins[i].reserve(raw_reserve_size);
+    obstacle_pointcloud_angle_bins[i].reserve(obstacle_reserve_size);
+  }
+
+  // Updated every loop inside transformPointAndCalculate()
+  Eigen::Vector4f pt_map;
+  int angle_bin_index;
+  double range;
+
+  size_t global_offset = 0;
+  for (size_t i = 0; i < raw_pointcloud_size; ++i) {
+    Eigen::Vector4f pt(
+      *reinterpret_cast<const float *>(&raw_pointcloud.data[global_offset + x_offset_raw_]),
+      *reinterpret_cast<const float *>(&raw_pointcloud.data[global_offset + y_offset_raw_]),
+      *reinterpret_cast<const float *>(&raw_pointcloud.data[global_offset + z_offset_raw_]), 1);
+    global_offset += raw_pointcloud.point_step;
+    if (!isPointValid(pt)) {
+      continue;
+    }
+    transformPointAndCalculate(pt, pt_map, angle_bin_index, range);
+
+    raw_pointcloud_angle_bins.at(angle_bin_index).emplace_back(range, pt_map[0], pt_map[1]);
   }
+
   for (auto & raw_pointcloud_angle_bin : raw_pointcloud_angle_bins) {
     std::sort(raw_pointcloud_angle_bin.begin(), raw_pointcloud_angle_bin.end(), [](auto a, auto b) {
       return a.range < b.range;
     });
   }
-  // create and sort bin for obstacle pointcloud
-  for (PointCloud2ConstIterator<float> iter_x(scan_obstacle_pointcloud, "x"),
-       iter_y(scan_obstacle_pointcloud, "y"), iter_wx(map_obstacle_pointcloud, "x"),
-       iter_wy(map_obstacle_pointcloud, "y");
-       iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_wx, ++iter_wy) {
-    const double angle = atan2(*iter_y, *iter_x);
-    const int angle_bin_index = (angle - min_angle) / angle_increment;
-    const double range = std::hypot(*iter_y, *iter_x);
+
+  // Create obstacle angle bins and sort points by range
+  global_offset = 0;
+  for (size_t i = 0; i < obstacle_pointcloud_size; ++i) {
+    Eigen::Vector4f pt(
+      *reinterpret_cast<const float *>(
+        &obstacle_pointcloud.data[global_offset + x_offset_obstacle_]),
+      *reinterpret_cast<const float *>(
+        &obstacle_pointcloud.data[global_offset + y_offset_obstacle_]),
+      *reinterpret_cast<const float *>(
+        &obstacle_pointcloud.data[global_offset + z_offset_obstacle_]),
+      1);
+    global_offset += obstacle_pointcloud.point_step;
+    if (!isPointValid(pt)) {
+      continue;
+    }
+    transformPointAndCalculate(pt, pt_map, angle_bin_index, range);
+
     // Ignore obstacle points exceed the range of the raw points
     if (raw_pointcloud_angle_bins.at(angle_bin_index).empty()) {
       continue;  // No raw point in this angle bin
     } else if (range > raw_pointcloud_angle_bins.at(angle_bin_index).back().range) {
       continue;  // Obstacle point exceeds the range of the raw points
     }
-    obstacle_pointcloud_angle_bins.at(angle_bin_index)
-      .push_back(BinInfo(range, *iter_wx, *iter_wy));
+    obstacle_pointcloud_angle_bins.at(angle_bin_index).emplace_back(range, pt_map[0], pt_map[1]);
   }
+
   for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) {
     std::sort(
       obstacle_pointcloud_angle_bin.begin(), obstacle_pointcloud_angle_bin.end(),
diff --git a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp
index 39224bc7e3a69..f5f2c70ae6dfc 100644
--- a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp
+++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp
@@ -59,21 +59,20 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud(
   const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud,
   const Pose & robot_pose, const Pose & scan_origin)
 {
-  constexpr double min_angle = autoware::universe_utils::deg2rad(-180.0);
-  constexpr double max_angle = autoware::universe_utils::deg2rad(180.0);
-  constexpr double angle_increment = autoware::universe_utils::deg2rad(0.1);
-  const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1 /*margin*/);
+  const size_t angle_bin_size =
+    ((max_angle_ - min_angle_) * angle_increment_inv_) + size_t(1 /*margin*/);
 
   // Transform from base_link to map frame
-  PointCloud2 map_raw_pointcloud, map_obstacle_pointcloud;  // point cloud in map frame
-  utils::transformPointcloud(raw_pointcloud, robot_pose, map_raw_pointcloud);
-  utils::transformPointcloud(obstacle_pointcloud, robot_pose, map_obstacle_pointcloud);
+  mat_map_ = utils::getTransformMatrix(robot_pose);
 
-  // Transform from map frame to scan frame
-  PointCloud2 scan_raw_pointcloud, scan_obstacle_pointcloud;      // point cloud in scan frame
   const auto scan2map_pose = utils::getInversePose(scan_origin);  // scan -> map transform pose
-  utils::transformPointcloud(map_raw_pointcloud, scan2map_pose, scan_raw_pointcloud);
-  utils::transformPointcloud(map_obstacle_pointcloud, scan2map_pose, scan_obstacle_pointcloud);
+
+  // Transform from map frame to scan frame
+  mat_scan_ = utils::getTransformMatrix(scan2map_pose);
+
+  if (!offset_initialized_) {
+    setFieldOffsets(raw_pointcloud, obstacle_pointcloud);
+  }
 
   // Create angle bins and sort points by range
   struct BinInfo3D
@@ -100,57 +99,82 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud(
     double projected_wy;
   };
 
-  std::vector</*angle bin*/ std::vector<BinInfo3D>> obstacle_pointcloud_angle_bins;
-  std::vector</*angle bin*/ std::vector<BinInfo3D>> raw_pointcloud_angle_bins;
-  obstacle_pointcloud_angle_bins.resize(angle_bin_size);
-  raw_pointcloud_angle_bins.resize(angle_bin_size);
-  for (PointCloud2ConstIterator<float> iter_x(scan_raw_pointcloud, "x"),
-       iter_y(scan_raw_pointcloud, "y"), iter_wx(map_raw_pointcloud, "x"),
-       iter_wy(map_raw_pointcloud, "y"), iter_wz(map_raw_pointcloud, "z");
-       iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_wx, ++iter_wy, ++iter_wz) {
-    const double angle = atan2(*iter_y, *iter_x);
-    const int angle_bin_index = (angle - min_angle) / angle_increment;
+  std::vector</*angle bin*/ std::vector<BinInfo3D>> obstacle_pointcloud_angle_bins(angle_bin_size);
+  std::vector</*angle bin*/ std::vector<BinInfo3D>> raw_pointcloud_angle_bins(angle_bin_size);
+
+  const size_t raw_pointcloud_size = raw_pointcloud.width * raw_pointcloud.height;
+  const size_t obstacle_pointcloud_size = obstacle_pointcloud.width * obstacle_pointcloud.height;
+
+  // Updated every loop inside transformPointAndCalculate()
+  Eigen::Vector4f pt_map;
+  int angle_bin_index;
+  double range;
+
+  size_t global_offset = 0;
+  for (size_t i = 0; i < raw_pointcloud_size; i++) {
+    Eigen::Vector4f pt(
+      *reinterpret_cast<const float *>(&raw_pointcloud.data[global_offset + x_offset_raw_]),
+      *reinterpret_cast<const float *>(&raw_pointcloud.data[global_offset + y_offset_raw_]),
+      *reinterpret_cast<const float *>(&raw_pointcloud.data[global_offset + z_offset_raw_]), 1);
+    global_offset += raw_pointcloud.point_step;
+    if (!isPointValid(pt)) {
+      continue;
+    }
+    transformPointAndCalculate(pt, pt_map, angle_bin_index, range);
+
     raw_pointcloud_angle_bins.at(angle_bin_index)
-      .emplace_back(std::hypot(*iter_y, *iter_x), *iter_wx, *iter_wy, *iter_wz);
+      .emplace_back(range, pt_map[0], pt_map[1], pt_map[2]);
   }
+
   for (auto & raw_pointcloud_angle_bin : raw_pointcloud_angle_bins) {
     std::sort(raw_pointcloud_angle_bin.begin(), raw_pointcloud_angle_bin.end(), [](auto a, auto b) {
       return a.range < b.range;
     });
   }
+
   // Create obstacle angle bins and sort points by range
-  for (PointCloud2ConstIterator<float> iter_x(scan_obstacle_pointcloud, "x"),
-       iter_y(scan_obstacle_pointcloud, "y"), iter_z(scan_obstacle_pointcloud, "z"),
-       iter_wx(map_obstacle_pointcloud, "x"), iter_wy(map_obstacle_pointcloud, "y"),
-       iter_wz(map_obstacle_pointcloud, "z");
-       iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_wx, ++iter_wy, ++iter_wz) {
+  global_offset = 0;
+  for (size_t i = 0; i < obstacle_pointcloud_size; i++) {
+    Eigen::Vector4f pt(
+      *reinterpret_cast<const float *>(
+        &obstacle_pointcloud.data[global_offset + x_offset_obstacle_]),
+      *reinterpret_cast<const float *>(
+        &obstacle_pointcloud.data[global_offset + y_offset_obstacle_]),
+      *reinterpret_cast<const float *>(
+        &obstacle_pointcloud.data[global_offset + z_offset_obstacle_]),
+      1);
+    global_offset += obstacle_pointcloud.point_step;
+    if (!isPointValid(pt)) {
+      continue;
+    }
+    transformPointAndCalculate(pt, pt_map, angle_bin_index, range);
     const double scan_z = scan_origin.position.z - robot_pose.position.z;
-    const double obstacle_z = (*iter_wz) - robot_pose.position.z;
+    const double obstacle_z = (pt_map[2]) - robot_pose.position.z;
     const double dz = scan_z - obstacle_z;
-    const double angle = atan2(*iter_y, *iter_x);
-    const int angle_bin_index = (angle - min_angle) / angle_increment;
-    const double range = std::hypot(*iter_x, *iter_y);
+
     // Ignore obstacle points exceed the range of the raw points
     if (raw_pointcloud_angle_bins.at(angle_bin_index).empty()) {
       continue;  // No raw point in this angle bin
     } else if (range > raw_pointcloud_angle_bins.at(angle_bin_index).back().range) {
       continue;  // Obstacle point exceeds the range of the raw points
     }
+
     if (dz > projection_dz_threshold_) {
       const double ratio = obstacle_z / dz;
       const double projection_length = range * ratio;
-      const double projected_wx = (*iter_wx) + ((*iter_wx) - scan_origin.position.x) * ratio;
-      const double projected_wy = (*iter_wy) + ((*iter_wy) - scan_origin.position.y) * ratio;
+      const double projected_wx = (pt_map[0]) + ((pt_map[0]) - scan_origin.position.x) * ratio;
+      const double projected_wy = (pt_map[1]) + ((pt_map[1]) - scan_origin.position.y) * ratio;
       obstacle_pointcloud_angle_bins.at(angle_bin_index)
         .emplace_back(
-          range, *iter_wx, *iter_wy, *iter_wz, projection_length, projected_wx, projected_wy);
+          range, pt_map[0], pt_map[1], pt_map[2], projection_length, projected_wx, projected_wy);
     } else {
       obstacle_pointcloud_angle_bins.at(angle_bin_index)
         .emplace_back(
-          range, *iter_wx, *iter_wy, *iter_wz, std::numeric_limits<double>::infinity(),
+          range, pt_map[0], pt_map[1], pt_map[2], std::numeric_limits<double>::infinity(),
           std::numeric_limits<double>::infinity(), std::numeric_limits<double>::infinity());
     }
   }
+
   for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) {
     std::sort(
       obstacle_pointcloud_angle_bin.begin(), obstacle_pointcloud_angle_bin.end(),
diff --git a/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp
index e8c2cdb2617df..902de2148cffe 100644
--- a/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp
+++ b/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp
@@ -60,6 +60,13 @@ void transformPointcloud(
   output.header.frame_id = "";
 }
 
+Eigen::Matrix4f getTransformMatrix(const geometry_msgs::msg::Pose & pose)
+{
+  auto transform = autoware::universe_utils::pose2transform(pose);
+  Eigen::Matrix4f tf_matrix = tf2::transformToEigen(transform).matrix().cast<float>();
+  return tf_matrix;
+}
+
 bool cropPointcloudByHeight(
   const sensor_msgs::msg::PointCloud2 & input, const tf2_ros::Buffer & tf2,
   const std::string & target_frame, const float min_height, const float max_height,
diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp
index 3fe75263b3f28..0d772e73c1ad8 100644
--- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp
+++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp
@@ -37,6 +37,7 @@
 #endif
 
 #include <algorithm>
+#include <limits>
 #include <memory>
 #include <string>
 
@@ -141,33 +142,46 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw(
     scan_origin_frame_ = input_raw_msg->header.frame_id;
   }
 
-  // Apply height filter
-  PointCloud2 cropped_obstacle_pc{};
-  PointCloud2 cropped_raw_pc{};
+  PointCloud2 trans_input_raw{}, trans_input_obstacle{};
+  bool is_raw_transformed = false;
+  bool is_obstacle_transformed = false;
+
+  // Prepare for applying height filter
   if (use_height_filter_) {
-    if (!utils::cropPointcloudByHeight(
-          *input_obstacle_msg, *tf2_, base_link_frame_, min_height_, max_height_,
-          cropped_obstacle_pc)) {
-      return;
+    // Make sure that the frame is base_link
+    if (input_raw_msg->header.frame_id != base_link_frame_) {
+      if (!utils::transformPointcloud(*input_raw_msg, *tf2_, base_link_frame_, trans_input_raw)) {
+        return;
+      }
+      is_raw_transformed = true;
     }
-    if (!utils::cropPointcloudByHeight(
-          *input_raw_msg, *tf2_, base_link_frame_, min_height_, max_height_, cropped_raw_pc)) {
-      return;
+    if (input_obstacle_msg->header.frame_id != base_link_frame_) {
+      if (!utils::transformPointcloud(
+            *input_obstacle_msg, *tf2_, base_link_frame_, trans_input_obstacle)) {
+        return;
+      }
+      is_obstacle_transformed = true;
     }
+    occupancy_grid_map_ptr_->setHeightLimit(min_height_, max_height_);
+  } else {
+    occupancy_grid_map_ptr_->setHeightLimit(
+      -std::numeric_limits<double>::infinity(), std::numeric_limits<double>::infinity());
   }
-  const PointCloud2 & filtered_obstacle_pc =
-    use_height_filter_ ? cropped_obstacle_pc : *input_obstacle_msg;
-  const PointCloud2 & filtered_raw_pc = use_height_filter_ ? cropped_raw_pc : *input_raw_msg;
+
+  const PointCloud2::ConstSharedPtr input_raw_use =
+    is_raw_transformed ? std::make_shared<PointCloud2>(trans_input_raw) : input_raw_msg;
+  const PointCloud2::ConstSharedPtr input_obstacle_use =
+    is_obstacle_transformed ? std::make_shared<PointCloud2>(trans_input_obstacle)
+                            : input_obstacle_msg;
 
   // Filter obstacle pointcloud by raw pointcloud
-  PointCloud2 filtered_obstacle_pc_common{};
+  PointCloud2 input_obstacle_pc_common{};
+  bool use_input_obstacle_pc_common = false;
   if (filter_obstacle_pointcloud_by_raw_pointcloud_) {
-    if (!utils::extractCommonPointCloud(
-          filtered_obstacle_pc, filtered_raw_pc, filtered_obstacle_pc_common)) {
-      filtered_obstacle_pc_common = filtered_obstacle_pc;
+    if (utils::extractCommonPointCloud(
+          *input_obstacle_use, *input_raw_use, input_obstacle_pc_common)) {
+      use_input_obstacle_pc_common = true;
     }
-  } else {
-    filtered_obstacle_pc_common = filtered_obstacle_pc;
   }
 
   // Get from map to sensor frame pose
@@ -191,7 +205,8 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw(
     gridmap_origin.position.x - occupancy_grid_map_ptr_->getSizeInMetersX() / 2,
     gridmap_origin.position.y - occupancy_grid_map_ptr_->getSizeInMetersY() / 2);
   occupancy_grid_map_ptr_->updateWithPointCloud(
-    filtered_raw_pc, filtered_obstacle_pc_common, robot_pose, scan_origin);
+    *input_raw_use, (use_input_obstacle_pc_common ? input_obstacle_pc_common : *input_obstacle_use),
+    robot_pose, scan_origin);
 
   if (enable_single_frame_mode_) {
     // publish