Fix thread safety of UniformTSDFVolume::ExtractVoxelGrid

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 #7315)
 
 ## 0.13
 

cpp/open3d/pipelines/integration/UniformTSDFVolume.cpp

@@ -257,28 +257,53 @@ 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<std::pair<Eigen::Vector3i, geometry::Voxel>>>
+            per_thread_voxels;
+
+    int num_threads = utility::EstimateMaxThreads();
+
+#pragma omp parallel num_threads(num_threads)
+    {
+#pragma omp single
+        { per_thread_voxels.resize(num_threads); }
+        auto &thread_voxels = per_thread_voxels[utility::GetThreadNum()];
+
 #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);
+                        thread_voxels.emplace_back(std::make_pair(
+                                index, geometry::Voxel(index, color)));
+                    }
                 }
             }
         }
     }
+
+    size_t total_voxels = 0;
+    for (const auto &thread_vector : per_thread_voxels) {
+        total_voxels += thread_vector.size();
+    }
+    voxel_grid->voxels_.reserve(total_voxels);
+
+    for (const auto &thread_vector : per_thread_voxels) {
+        voxel_grid->voxels_.insert(thread_vector.begin(), thread_vector.end());
+    }
+
     return voxel_grid;
 }
 

cpp/open3d/utility/Parallel.cpp

@@ -45,6 +45,14 @@ int EstimateMaxThreads() {
 #endif
 }
 
+int GetThreadNum() {
+#ifdef _OPENMP
+    return omp_get_thread_num();
+#else
+    return 0;  // No parallelism available, so thread ID is always 0.
+#endif
+}
+
 bool InParallel() {
     // TODO: when we add TBB/Parallel STL support to ParallelFor, update this.
 #ifdef _OPENMP

cpp/open3d/utility/Parallel.h

@@ -13,6 +13,9 @@ namespace utility {
 /// Estimate the maximum number of threads to be used in a parallel region.
 int EstimateMaxThreads();
 
+/// Returns the thread ID in the current parallel region.
+int GetThreadNum();
+
 /// Returns true if in an parallel section.
 bool InParallel();