Merge pull request #25 from JustusBraun/pr/normal-clearance-algorithm

Add calcNormalClearance algorithm

include/lvr2/algorithm/GeometryAlgorithms.hpp

@@ -35,6 +35,7 @@
 #include "lvr2/geometry/BaseMesh.hpp"
 #include "lvr2/attrmaps/AttrMaps.hpp"
 #include "lvr2/geometry/Handles.hpp"
+#include "lvr2/geometry/Normal.hpp"
 #include <list>
 
 namespace lvr2
@@ -211,6 +212,20 @@ template<typename BaseVecT>
 DenseVertexMap<float> calcBorderCosts(const BaseMesh<BaseVecT>& mesh, double border_cost = 1.0);
 
 
+/**
+ * @brief Compute the distances to the next intersections along the vertex normals of the mesh
+ *
+ * @param mesh          The mesh containing the geometry of interest
+ * @param normals       The vertex normals used by the algorithm
+ * @return              A dense vertex map containing the free space along the normal of each vertex
+ */
+template <typename BaseVecT>
+DenseVertexMap<float> calcNormalClearance(
+    const BaseMesh<BaseVecT>& mesh,
+    const DenseVertexMap<Normal<typename BaseVecT::CoordType>>& normals
+);
+
+
 /**
  * @brief  Dijkstra's algorithm
  *

include/lvr2/algorithm/GeometryAlgorithms.tcc

@@ -33,9 +33,20 @@
 #include <limits>
 #include <queue>
 #include <set>
+#include <list>
 
 #include "lvr2/attrmaps/AttrMaps.hpp"
 #include "lvr2/io/Progress.hpp"
+#include "lvr2/algorithm/FinalizeAlgorithms.hpp"
+#include "lvr2/util/Util.hpp"
+#include "lvr2/geometry/BaseMesh.hpp"
+
+#ifdef LVR2_USE_EMBREE
+#include "lvr2/algorithm/raycasting/EmbreeRaycaster.hpp"
+#else
+#include "lvr2/algorithm/raycasting/BVHRaycaster.hpp"
+#endif
+
 
 namespace lvr2
 {
@@ -548,6 +559,74 @@ DenseVertexMap<float> calcBorderCosts(
     return borderCosts;
 }
 
+
+template <typename BaseVecT>
+DenseVertexMap<float> calcNormalClearance(
+    const BaseMesh<BaseVecT>& mesh,
+    const DenseVertexMap<Normal<typename BaseVecT::CoordType>>& normals
+)
+{
+    // Create a MeshBufferPtr to pass to raycaster implementations
+    SimpleFinalizer<BaseVecT> fin;
+    MeshBufferPtr buffer = fin.apply(mesh);
+
+    // Create a raycaster implementation
+#ifdef LVR2_USE_EMBREE
+    auto raycaster = EmbreeRaycaster<DistInt>(buffer);
+#else
+    auto raycaster = BVHRaycaster<DistInt>(buffer);
+#endif
+
+    // Reserve enough memory for all vertices to prevent unnecessary reallocations
+    DenseVertexMap<float> freespace;
+    freespace.reserve(mesh.nextVertexIndex());
+
+    std::stringstream msg;
+    msg << timestamp << "[calcNormalClearance] Calculating free space along vertex normals";
+    ProgressBar progress(mesh.numVertices(), msg.str());
+
+    // Cast rays for each vertex in parallel
+    #pragma omp parallel for
+    for (auto i = 0; i < mesh.nextVertexIndex(); i++)
+    {
+        // Create a vertex handle and check if the mesh contains a vertex with index i
+        auto vertexH = VertexHandle(i);
+        if (!mesh.containsVertex(vertexH))
+        {
+            continue;
+        }
+
+        float distance = std::numeric_limits<float>::infinity();
+        DistInt result;
+        const Vector3f origin = Util::to_eigen(mesh.getVertexPosition(vertexH));
+        const Vector3f normal = Util::to_eigen(normals[vertexH]);
+        // Add a small offset to the vertex position to avoid intersections with its incident faces
+        if (raycaster.castRay(
+            origin + normal * 0.001,
+            normal,
+            result
+        ))
+        {
+            // Add the same offset to the distance result
+            distance = result.dist + 0.001;
+        }
+
+        #pragma omp critical
+        {
+            freespace.insert(vertexH, distance);
+            ++progress;
+        }
+    }
+
+    if (!timestamp.isQuiet())
+    {
+        cout << endl;
+    }
+
+    return freespace;
+}
+
+
 class CompareDist
 {
 public:

include/lvr2/algorithm/raycasting/BVHRaycaster.hpp

@@ -44,7 +44,6 @@
 #include "Intersection.hpp"
 
 #define EPSILON 0.0000001
-#define PI 3.14159265
 
 namespace lvr2
 {
@@ -58,7 +57,11 @@ class BVHRaycaster : public RaycasterBase<IntT> {
     /**
      * @brief Constructor: Stores mesh as member
      */
-    BVHRaycaster(const MeshBufferPtr mesh, unsigned int stack_size = 64);
+    BVHRaycaster(const MeshBufferPtr mesh, unsigned int stack_size);
+    /**
+     * @brief Constructor: Stores mesh as member; Uses BVH depth as stack size
+     */
+    BVHRaycaster(const MeshBufferPtr mesh);
 
     /**
      * @brief Cast a single ray onto the mesh
@@ -183,4 +186,4 @@ class BVHRaycaster : public RaycasterBase<IntT> {
 
 #include "BVHRaycaster.tcc"
 
-#endif // LVR2_ALGORITHM_RAYCASTING_BVHRAYCASTER
+#endif // LVR2_ALGORITHM_RAYCASTING_BVHRAYCASTER

include/lvr2/algorithm/raycasting/BVHRaycaster.tcc

@@ -16,6 +16,20 @@ BVHRaycaster<IntT>::BVHRaycaster(const MeshBufferPtr mesh, unsigned int stack_si
 
 }
 
+template<typename IntT>
+BVHRaycaster<IntT>::BVHRaycaster(const MeshBufferPtr mesh)
+:RaycasterBase<IntT>(mesh)
+,m_bvh(mesh)
+,m_faces(mesh->getFaceIndices())
+,m_vertices(mesh->getVertices())
+,m_BVHindicesOrTriLists(m_bvh.getIndexesOrTrilists().data())
+,m_BVHlimits(m_bvh.getLimits().data())
+,m_TriangleIntersectionData(m_bvh.getTrianglesIntersectionData().data())
+,m_TriIdxList(m_bvh.getTriIndexList().data())
+,m_stack_size(m_bvh.getMaxDepth())
+{
+}
+
 template<typename IntT>
 bool BVHRaycaster<IntT>::castRay(
     const Vector3f& origin,
@@ -140,16 +154,23 @@ BVHRaycaster<IntT>::intersectTrianglesBVH(
             // if ray intersects inner node, push indices of left and right child nodes on the stack
             if (rayIntersectsBox(origin, ray, &clBVHlimits[bvh_limits_scale * 3 * boxId]))
             {
+                // return if stack size is exceeded
+                if ( stackId >= m_stack_size)
+                {
+                    printf("BVH stack size exceeded!\n");
+                    result.hit = false;
+                    return result;
+                }
                 stack[stackId++] = clBVHindicesOrTriLists[4 * boxId + 1];
-                stack[stackId++] = clBVHindicesOrTriLists[4 * boxId + 2];
 
                 // return if stack size is exceeded
-                if ( stackId > m_stack_size)
+                if ( stackId >= m_stack_size)
                 {
                     printf("BVH stack size exceeded!\n");
-                    result.hit = 0;
+                    result.hit = false;
                     return result;
                 }
+                stack[stackId++] = clBVHindicesOrTriLists[4 * boxId + 2];
             }
         }
         else // leaf node
@@ -277,4 +298,4 @@ bool BVHRaycaster<IntT>::rayIntersectsBox(
     return true;
 }
 
-} // namespace lvr2
+} // namespace lvr2

include/lvr2/geometry/BVH.hpp

@@ -121,6 +121,11 @@ class BVHTree
      */
     const vector<float>& getTrianglesIntersectionData() const;
 
+    /**
+     * @brief Returns the depth of the tree at its deepest node
+     */
+    const uint32_t getMaxDepth() const noexcept;
+
 private:
 
     // Internal triangle representation
@@ -184,6 +189,8 @@ class BVHTree
     // working variables for tree construction
     BVHNodePtr m_root;
     vector<Triangle> m_triangles;
+    // Tree depth
+    uint32_t m_depth;
 
     // cache friendly data for the SIMD device
     vector<uint32_t> m_triIndexList;

include/lvr2/geometry/BVH.tcc

@@ -60,6 +60,7 @@ BVHTree<BaseVecT>::Triangle::Triangle()
 
 template<typename BaseVecT>
 BVHTree<BaseVecT>::BVHTree(const vector<float>& vertices, const vector<uint32_t>& faces)
+: m_depth(0)
 {
     m_root = buildTree(vertices, faces);
     createCFTree();
@@ -69,6 +70,7 @@ template<typename BaseVecT>
 BVHTree<BaseVecT>::BVHTree(
     const floatArr vertices, size_t n_vertices,
     const indexArray faces, size_t n_faces)
+: m_depth(0)
 {
     m_root = buildTree(vertices, n_vertices, faces, n_faces);
     createCFTree();
@@ -317,6 +319,10 @@ typename BVHTree<BaseVecT>::BVHNodePtr BVHTree<BaseVecT>::buildTreeRecursive(vec
                 leaf->triangles.push_back(triangle);
             }
         }
+        #pragma omp critical(bvh_depth_update)
+        {
+            m_depth = std::max(m_depth, depth);
+        }
         return move(leaf);
     }
 
@@ -437,6 +443,10 @@ typename BVHTree<BaseVecT>::BVHNodePtr BVHTree<BaseVecT>::buildTreeRecursive(vec
                 leaf->triangles.push_back(triangle);
             }
         }
+        #pragma omp critical(bvh_depth_update)
+        {
+            m_depth = std::max(m_depth, depth);
+        }
         return move(leaf);
     }
 
@@ -626,4 +636,10 @@ const vector<float>& BVHTree<BaseVecT>::getTrianglesIntersectionData() const
     return m_trianglesIntersectionData;
 }
 
+template<typename BaseVecT>
+const uint32_t BVHTree<BaseVecT>::getMaxDepth() const noexcept
+{
+    return m_depth;
+}
+
 } /* namespace lvr2 */

include/lvr2/util/Util.hpp

@@ -238,6 +238,24 @@ class Util
                    (lhs.x == rhs.x && lhs.y == rhs.y && lhs.z < rhs.z);
         }
     };
+
+    /**
+    * @brief Convert a BaseVecT to Eigen::Vector3
+    *
+    * @param    vec     The BaseVec to convert to eigen
+    * 
+    * @return Returns the vector as an Eigen::Vector3 object
+    */
+    template <typename BaseVecT>
+    [[nodiscard]]
+    static inline Vector3<typename BaseVecT::CoordType> to_eigen(const BaseVecT& vec) noexcept
+    {
+        return Vector3<typename BaseVecT::CoordType>(
+            vec[0],
+            vec[1],
+            vec[2]
+        );
+    }
 };
 
 } // namespace lvr2

src/tools/lvr2_hdf5_mesh_tool/HDF5MeshTool.cpp

@@ -37,7 +37,6 @@
 #include "lvr2/algorithm/GeometryAlgorithms.hpp"
 #include "lvr2/geometry/HalfEdgeMesh.hpp"
 #include "lvr2/algorithm/ReductionAlgorithms.hpp"
-
 #include "Options.hpp"
 
 #include <string>
@@ -118,6 +117,10 @@ int main( int argc, char ** argv )
         invalid_face_cnt++;
       }
     }
+    if (invalid_face_cnt > 0)
+    {
+      std::cout << timestamp << "Invalid faces found during HalfEdgeMesh construction: " << invalid_face_cnt << std::endl;
+    }
 
     HDF5MeshToolIO hdf5;
     bool writeToHdf5Input = false;
@@ -455,6 +458,41 @@ int main( int argc, char ** argv )
       std::cout << timestamp << "Border costs already included." << std::endl;
     }
 
+    // Free space above vertices
+    DenseVertexMap<float> freeSpace;
+    boost::optional<DenseVertexMap<float>> freeSpaceOpt;
+    if (readFromHdf5)
+    {
+      freeSpaceOpt = hdf5In.getDenseAttributeMap<DenseVertexMap<float>>("freespace");
+    }
+    if (freeSpaceOpt)
+    {
+      std::cout << timestamp << "Using existing free space ..." << std::endl;
+      freeSpace = freeSpaceOpt.value();
+    }
+    else
+    {
+      std::cout << timestamp << "Computing free space ..." << std::endl;
+      freeSpace = calcNormalClearance(hem, vertexNormals);
+    }
+    if (!freeSpaceOpt || !writeToHdf5Input)
+    {
+      std::cout << timestamp << "Adding free space..." << std::endl;
+      bool addedBorderCosts = hdf5.addDenseAttributeMap<DenseVertexMap<float>>(
+              hem, freeSpace, "freespace");
+      if (addedBorderCosts)
+      {
+        std::cout << timestamp << "successfully added free space." << std::endl;
+      }
+      else
+      {
+        std::cout << timestamp << "could not add free space!" << std::endl;
+      }
+    }
+    else
+    {
+      std::cout << timestamp << "Free space already included." << std::endl;
+    }
   }
   else
   {