parallelized normal estimation

pavelsevecek · Jul 4, 2020 · 7c46b18 · 7c46b18
1 parent 9aba589
commit 7c46b18
Show file tree

Hide file tree

Showing 3 changed files with 63 additions and 10 deletions.
diff --git a/CloudUtils.hpp b/CloudUtils.hpp
@@ -4,6 +4,7 @@
 #include "Matrix.hpp"
 #include "PlyWriter.hpp"
 #include "Svd.hpp"
+#include "utility"
 #include <iostream>
 
 namespace Pvl {
@@ -28,13 +29,22 @@ Vec3f centroid(const Cloud& cloud) {
     return pos / count;
 }
 
-template <typename Cloud>
+template <typename ConcurrencyTag = SequentialTag, typename Cloud>
 std::vector<Vec3f> estimateNormals(Cloud& cloud) {
+    return estimateNormals<ConcurrencyTag>(cloud, [](float) { return false; });
+}
+
+template <typename ConcurrencyTag = SequentialTag, typename Cloud, typename Progress>
+std::vector<Vec3f> estimateNormals(Cloud& cloud, const Progress& progress) {
     KdTree<Vec3f> tree;
     tree.build(cloud);
     std::vector<Vec3f> normals(cloud.size());
-
-    for (std::size_t i = 0; i < cloud.size(); ++i) {
+    auto meter = makeProgressMeter(cloud.size(), progress);
+    std::atomic<bool> wasCancelled{ false };
+    ParallelFor<ConcurrencyTag>()(std::size_t(0), cloud.size(), [&](std::size_t i) {
+        if (wasCancelled) {
+            return;
+        }
         const Vec3f& p = cloud[i];
         float radius = 0.01;
         std::vector<int> neighs;
@@ -56,12 +66,20 @@ std::vector<Vec3f> estimateNormals(Cloud& cloud) {
         }
         Svd<float> svd = singularValueDecomposition(cov);
 
-        //  std::cout << "singular values = " << svd.S[0] << "," << svd.S[1] << "," << svd.S[2]
-        //  << "\n";
         normals[i] = svd.U.column(argMin(svd.S));
-        normals[i] *= sign(dotProd(normals[i], cloud[i]));
+
+        // initially orient upwards
+        normals[i] *= sign(normals[i][2]);
+        if (meter.inc()) {
+            wasCancelled = true;
+            return;
+        }
+    });
+    if (!wasCancelled) {
+        return normals;
+    } else {
+        return {};
     }
-    return normals;
 }
 
 Vector<float, 6> join(Vec3f v, Vec3f n) {

diff --git a/KdTree.hpp b/KdTree.hpp
@@ -162,13 +162,13 @@ class KdTree : public Noncopyable {
 private:
     struct {
         /// Maximal number of particles in the leaf node
-        int leafSize;
+        Index leafSize;
 
         /// Maximal depth for which the build is parallelized
-        int maxParallelDepth;
+        Index maxParallelDepth;
     } config_;
 
-    using Float = typename Vec::Type;
+    using Float = typename Vec::Float;
     static constexpr int Dim = Vec::size();
     using Box = BoundingBox<Vector<Float, Dim>>;
     using InnerNode = KdInnerNode<Float, Index, Dim>;

diff --git a/Utils.hpp b/Utils.hpp
@@ -67,4 +67,39 @@ struct ParallelForEach<ParallelTag> {
     }
 };
 
+template <typename Progress>
+class ProgressMeter {
+    Progress func_;
+    tbb::atomic<int> counter_;
+    int target_;
+    int step_;
+    int next_;
+    tbb::tbb_thread::id callingThreadId_;
+
+public:
+    ProgressMeter(int target, Progress&& func)
+        : func_(std::move(func)) {
+        step_ = std::max(target / 100, 10);
+        next_ = step_;
+        target_ = target;
+        callingThreadId_ = tbb::this_tbb_thread::get_id();
+    }
+
+    bool inc() {
+        counter_++;
+        if (tbb::this_tbb_thread::get_id() == callingThreadId_ && counter_ > next_) {
+            float value = float(counter_) / target_ * 100;
+            next_ += step_;
+            return func_(value);
+        } else {
+            return false;
+        }
+    }
+};
+
+template <typename Progress>
+ProgressMeter<Progress> makeProgressMeter(int target, Progress&& func) {
+    return ProgressMeter<Progress>(target, std::move(func));
+}
+
 } // namespace Pvl