diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index 4c46decb39e52..139f7774b42b3 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -95,6 +95,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( const size_t obstacle_pointcloud_size = obstacle_pointcloud.width * obstacle_pointcloud.height; const size_t raw_reserve_size = raw_pointcloud_size / angle_bin_size; const size_t obstacle_reserve_size = obstacle_pointcloud_size / angle_bin_size; + // Reserve a certain amount of memory in advance for performance reasons for (auto & raw_pointcloud_angle_bin : raw_pointcloud_angle_bins) { raw_pointcloud_angle_bin.reserve(raw_reserve_size); } diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index 6d45f768cd182..8a609077b8d3c 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -110,6 +110,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const size_t obstacle_pointcloud_size = obstacle_pointcloud.width * obstacle_pointcloud.height; const size_t raw_reserve_size = raw_pointcloud_size / angle_bin_size; const size_t obstacle_reserve_size = obstacle_pointcloud_size / angle_bin_size; + // Reserve a certain amount of memory in advance for performance reasons for (auto & raw_pointcloud_angle_bin : raw_pointcloud_angle_bins) { raw_pointcloud_angle_bin.reserve(raw_reserve_size); } 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 2a726eb50faec..2f2b330e2df26 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 @@ -147,7 +147,7 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( bool is_obstacle_transformed = false; // Prepare for applying height filter if (use_height_filter_) { - // make sure that frame is base_link + // 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;