Skip to content

Commit eea40ae

Browse files
chore(ndt_scan_matcher): refactor NDT map update (#6505)
* Tested Signed-off-by: anhnv3991 <anh.nguyen.2@tier4.jp> * style(pre-commit): autofix --------- Signed-off-by: anhnv3991 <anh.nguyen.2@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent a6003cf commit eea40ae

File tree

1 file changed

+5
-9
lines changed

1 file changed

+5
-9
lines changed

localization/ndt_scan_matcher/src/map_update_module.cpp

+5-9
Original file line numberDiff line numberDiff line change
@@ -152,18 +152,14 @@ bool MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt
152152
}
153153

154154
const auto exe_start_time = std::chrono::system_clock::now();
155-
const size_t add_size = maps_to_add.size();
156155
// Perform heavy processing outside of the lock scope
157-
std::vector<pcl::shared_ptr<pcl::PointCloud<PointTarget>>> points_pcl(add_size);
158-
159-
for (size_t i = 0; i < add_size; i++) {
160-
points_pcl[i] = pcl::make_shared<pcl::PointCloud<PointTarget>>();
161-
pcl::fromROSMsg(maps_to_add[i].pointcloud, *points_pcl[i]);
162-
}
163156

164157
// Add pcd
165-
for (size_t i = 0; i < add_size; i++) {
166-
ndt.addTarget(points_pcl[i], maps_to_add[i].cell_id);
158+
for (auto & map : maps_to_add) {
159+
auto cloud = pcl::make_shared<pcl::PointCloud<PointTarget>>();
160+
161+
pcl::fromROSMsg(map.pointcloud, *cloud);
162+
ndt.addTarget(cloud, map.cell_id);
167163
}
168164

169165
// Remove pcd

0 commit comments

Comments
 (0)