added new layer for avoiding navigation on borders of the mesh

include/lvr2/algorithm/GeometryAlgorithms.hpp

@@ -199,6 +199,18 @@ void calcVertexRoughnessAndHeightDifferences(
 template<typename BaseVecT>
 DenseEdgeMap<float> calcVertexDistances(const BaseMesh<BaseVecT>& mesh);
 
+
+/**
+ * @brief Computes / set costs for each border vertex to predefined value
+ *
+ * @param mesh          The mesh containing the vertices and edges of interest
+ * @param border_cost   Predfined value to set the border vertices to. Defaults to 1.0
+ * @return              The dense edge map with the border cost values
+ */
+template<typename BaseVecT>
+DenseVertexMap<float> calcBorderCosts(const BaseMesh<BaseVecT>& mesh, double border_cost = 1.0);
+
+
 /**
  * @brief  Dijkstra's algorithm
  *

include/lvr2/algorithm/GeometryAlgorithms.tcc

@@ -491,6 +491,64 @@ DenseEdgeMap<float> calcVertexDistances(const BaseMesh<BaseVecT> &mesh)
     return distances;
 }
 
+
+template <typename BaseVecT>
+DenseVertexMap<float> calcBorderCosts(
+  const BaseMesh<BaseVecT> &mesh, 
+  double border_cost)
+{
+    DenseVertexMap<float> borderCosts;
+    borderCosts.reserve(mesh.nextVertexIndex());
+
+    // Output
+    string msg = timestamp.getElapsedTime() + "Computing border weights...";
+    ProgressBar progress(mesh.numVertices(), msg);
+    ++progress;
+
+    // Calculate height difference for each vertex
+    // for(auto vH : mesh.vertices())
+    #pragma omp parallel for
+    for (size_t i = 0; i < mesh.nextVertexIndex(); i++)
+    {
+        auto vH = VertexHandle(i);
+        if (!mesh.containsVertex(vH))
+        {
+          continue;
+        }
+
+        // new! give border vertex a high high
+        bool is_border_vertex = false;
+        for(auto edge : mesh.getEdgesOfVertex(vH))
+        {
+          if(mesh.isBorderEdge(edge))
+          {
+            is_border_vertex = true;
+            break; // early finish
+          }
+        }
+
+        // Calculate the final border weight
+        #pragma omp critical
+        {
+            if(is_border_vertex)
+            {
+                borderCosts.insert(vH, border_cost);
+            } else {
+                borderCosts.insert(vH, 0.0);
+            }
+
+            ++progress;
+        }
+    }
+
+    if(!timestamp.isQuiet())
+    {
+      std::cout << std::endl;
+    }
+
+    return borderCosts;
+}
+
 class CompareDist
 {
 public:

src/tools/lvr2_hdf5_mesh_tool/HDF5MeshTool.cpp

@@ -416,6 +416,45 @@ int main( int argc, char ** argv )
     {
       std::cout << timestamp << "Height differences already included." << std::endl;
     }
+
+
+    // border costs
+    DenseVertexMap<float> borderCosts;
+    boost::optional<DenseVertexMap<float>> borderCostsOpt;
+    if (readFromHdf5)
+    {
+      borderCostsOpt = hdf5In.getDenseAttributeMap<DenseVertexMap<float>>("border");
+    }
+    if (borderCostsOpt)
+    {
+      std::cout << timestamp << "Using existing border costs..." << std::endl;
+      borderCosts = *borderCostsOpt;
+    }
+    else
+    {
+      std::cout << timestamp << "Computing border costs ... Setting border vertex costs to "
+                << options.getBorderVertexCost() << " ..." << std::endl;
+      borderCosts = calcBorderCosts(hem, 1.0);
+    }
+    if (!borderCostsOpt || !writeToHdf5Input)
+    {
+      std::cout << timestamp << "Adding border costs..." << std::endl;
+      bool addedBorderCosts = hdf5.addDenseAttributeMap<DenseVertexMap<float>>(
+              hem, borderCosts, "border");
+      if (addedBorderCosts)
+      {
+        std::cout << timestamp << "successfully added border costs." << std::endl;
+      }
+      else
+      {
+        std::cout << timestamp << "could not add border costs!" << std::endl;
+      }
+    }
+    else
+    {
+      std::cout << timestamp << "Border costs already included." << std::endl;
+    }
+
   }
   else
   {

src/tools/lvr2_hdf5_mesh_tool/Options.cpp

@@ -42,6 +42,7 @@ Options::Options(int argc, char** argv) : m_descr("Supported options")
             ("meshName,m", value<string>()->default_value("mesh"), "The name of the mesh to write")
             ("edgeCollapse,e", value<size_t>()->default_value(0), "Edge collapse reduction algorithm, the number of edges to collapse.")
             ("localRadius,r", value<float>()->default_value(0.3), "The local radius used for roughness and height differences computation.")
+            ("borderVertexCost,b", value<float>()->default_value(1.0), "Setting the each vertex on the border of the mesh to this value (seperate 'border' layer)")
             ;
 
 

src/tools/lvr2_hdf5_mesh_tool/Options.hpp

@@ -64,6 +64,7 @@ class Options {
     string  getMeshName()         const { return m_variables["meshName"].as<string>();}
     size_t  getEdgeCollapseNum()  const { return m_variables["edgeCollapse"].as<size_t >();}
     float  getLocalRadius() const { return m_variables["localRadius"].as<float>();}
+    float  getBorderVertexCost() const { return m_variables["borderVertexCost"].as<float>();}
 private:
     /// The internally used variable map
     variables_map                   m_variables;
@@ -86,10 +87,11 @@ inline ostream& operator<<(ostream& os, const Options &o)
     cout << "##### Mesh name \t\t: "  << o.getMeshName() << endl;
     cout << "##### Edge collapse num \t\t: "  << o.getEdgeCollapseNum() << endl;
     cout << "##### Local Radius \t\t: "  << o.getLocalRadius() << endl;
+    cout << "##### Border Vertex Cost \t\t: " << o.getBorderVertexCost() << endl;
   return os;
 }
 
-} // namespace reconstruct
+} // namespace hdf5meshtool
 
 
 #endif /* OPTIONS_H_ */