diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 4089a017..b70d3dd6 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -31,18 +31,11 @@ namespace { using kiss_icp::Voxel; - -std::vector GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1) { - std::vector voxel_neighborhood; - for (int i = voxel.x() - adjacent_voxels; i < voxel.x() + adjacent_voxels + 1; ++i) { - for (int j = voxel.y() - adjacent_voxels; j < voxel.y() + adjacent_voxels + 1; ++j) { - for (int k = voxel.z() - adjacent_voxels; k < voxel.z() + adjacent_voxels + 1; ++k) { - voxel_neighborhood.emplace_back(i, j, k); - } - } - } - return voxel_neighborhood; -} +static const std::array shifts{ + {{0, 0, 0}, {1, 0, 0}, {-1, 0, 0}, {0, 1, 0}, {0, -1, 0}, {0, 0, 1}, {0, 0, -1}, + {1, 1, 0}, {1, -1, 0}, {-1, 1, 0}, {-1, -1, 0}, {1, 0, 1}, {1, 0, -1}, {-1, 0, 1}, + {-1, 0, -1}, {0, 1, 1}, {0, 1, -1}, {0, -1, 1}, {0, -1, -1}, {1, 1, 1}, {1, 1, -1}, + {1, -1, 1}, {1, -1, -1}, {-1, 1, 1}, {-1, 1, -1}, {-1, -1, 1}, {-1, -1, -1}}}; } // namespace namespace kiss_icp { @@ -51,12 +44,11 @@ std::tuple VoxelHashMap::GetClosestNeighbor( const Eigen::Vector3d &query) const { // Convert the point to voxel coordinates const auto &voxel = PointToVoxel(query, voxel_size_); - // Get nearby voxels on the map - const auto &query_voxels = GetAdjacentVoxels(voxel); // Find the nearest neighbor Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero(); double closest_distance = std::numeric_limits::max(); - std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &query_voxel) { + std::for_each(shifts.cbegin(), shifts.cend(), [&](const auto &voxel_shift) { + const auto &query_voxel = voxel + voxel_shift; auto search = map_.find(query_voxel); if (search != map_.end()) { const auto &points = search.value();