chore(ndt_scan_matcher): refactor NDT map update (autowarefoundation#…

…6505) * Tested Signed-off-by: anhnv3991 <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: anhnv3991 <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Signed-off-by: kaigohirao <[email protected]>
kaigohirao · Mar 22, 2024 · dd70862 · dd70862
1 parent 79beaa4
commit dd70862
Showing 1 changed file with 5 additions and 9 deletions.
diff --git 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<pcl::shared_ptr<pcl::PointCloud<PointTarget>>> points_pcl(add_size);
-
-  for (size_t i = 0; i < add_size; i++) {
-    points_pcl[i] = pcl::make_shared<pcl::PointCloud<PointTarget>>();
-    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::PointCloud<PointTarget>>();
+
+    pcl::fromROSMsg(map.pointcloud, *cloud);
+    ndt.addTarget(cloud, map.cell_id);
   }
 
   // Remove pcd