Fix thread safety of UniformTSDFVolume::ExtractVoxelGrid

Author: nicolaloi

CHANGELOG.md

@@ -63,6 +63,7 @@
 -   Exposed `get_plotly_fig` and modified `draw_plotly` to return the `Figure` it creates. (PR #7258)
 -   Fix build with librealsense v2.44.0 and upcoming VS 2022 17.13 (PR #7074)
 -   Fix `deprecated-declarations` warnings when compiling code with C++20 standard (PR #7303)
+-   Fix thread safety of UniformTSDFVolume::ExtractVoxelGrid (PR #7314)
 
 ## 0.13
 

cpp/open3d/pipelines/integration/UniformTSDFVolume.cpp

@@ -257,28 +257,45 @@ std::shared_ptr<geometry::VoxelGrid> UniformTSDFVolume::ExtractVoxelGrid()
     voxel_grid->voxel_size_ = voxel_length_;
     voxel_grid->origin_ = origin_;
 
+    // Create a vector to hold voxels for each thread in the parallel region,
+    // since access to voxel_grid->voxels_ (std::unordered_map) is not
+    // thread-safe.
+    std::vector<std::vector<geometry::Voxel>> per_thread_voxels;
+
+#pragma omp parallel num_threads(utility::EstimateMaxThreads())
+    {
+#pragma omp single
+        { per_thread_voxels.resize(omp_get_num_threads()); }
+        int thread_id = omp_get_thread_num();
+
 #ifdef _WIN32
-#pragma omp parallel for schedule(static) \
-        num_threads(utility::EstimateMaxThreads())
+#pragma omp for schedule(static)
 #else
-#pragma omp parallel for collapse(2) schedule(static) \
-        num_threads(utility::EstimateMaxThreads())
+#pragma omp for collapse(2) schedule(static)
 #endif
-    for (int x = 0; x < resolution_; x++) {
-        for (int y = 0; y < resolution_; y++) {
-            for (int z = 0; z < resolution_; z++) {
-                const int ind = IndexOf(x, y, z);
-                const float w = voxels_[ind].weight_;
-                const float f = voxels_[ind].tsdf_;
-                if (w != 0.0f && f < 0.98f && f >= -0.98f) {
-                    double c = (f + 1.0) * 0.5;
-                    Eigen::Vector3d color = Eigen::Vector3d(c, c, c);
-                    Eigen::Vector3i index = Eigen::Vector3i(x, y, z);
-                    voxel_grid->voxels_[index] = geometry::Voxel(index, color);
+        for (int x = 0; x < resolution_; x++) {
+            for (int y = 0; y < resolution_; y++) {
+                for (int z = 0; z < resolution_; z++) {
+                    const int ind = IndexOf(x, y, z);
+                    const float w = voxels_[ind].weight_;
+                    const float f = voxels_[ind].tsdf_;
+                    if (w != 0.0f && f < 0.98f && f >= -0.98f) {
+                        double c = (f + 1.0) * 0.5;
+                        Eigen::Vector3d color(c, c, c);
+                        Eigen::Vector3i index(x, y, z);
+                        per_thread_voxels[thread_id].emplace_back(index, color);
+                    }
                 }
             }
         }
     }
+
+    for (const auto &thread_vector : per_thread_voxels) {
+        for (const auto &voxel : thread_vector) {
+            voxel_grid->voxels_[voxel.grid_index_] = voxel;
+        }
+    }
+
     return voxel_grid;
 }