diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 950457f0d49b1..31d1c6588165f 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -152,18 +152,14 @@ bool MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt } const auto exe_start_time = std::chrono::system_clock::now(); - const size_t add_size = maps_to_add.size(); // Perform heavy processing outside of the lock scope - std::vector>> points_pcl(add_size); - - for (size_t i = 0; i < add_size; i++) { - points_pcl[i] = pcl::make_shared>(); - pcl::fromROSMsg(maps_to_add[i].pointcloud, *points_pcl[i]); - } // Add pcd - for (size_t i = 0; i < add_size; i++) { - ndt.addTarget(points_pcl[i], maps_to_add[i].cell_id); + for (auto & map : maps_to_add) { + auto cloud = pcl::make_shared>(); + + pcl::fromROSMsg(map.pointcloud, *cloud); + ndt.addTarget(cloud, map.cell_id); } // Remove pcd