From 4376909f4d684ed89ebb3f59f43a9d968b9b71df Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Wed, 7 Aug 2024 17:01:17 +0200 Subject: [PATCH] added new layer for avoiding navigation on borders of the mesh --- include/lvr2/algorithm/GeometryAlgorithms.hpp | 12 ++++ include/lvr2/algorithm/GeometryAlgorithms.tcc | 58 +++++++++++++++++++ .../lvr2_hdf5_mesh_tool/HDF5MeshTool.cpp | 39 +++++++++++++ src/tools/lvr2_hdf5_mesh_tool/Options.cpp | 1 + src/tools/lvr2_hdf5_mesh_tool/Options.hpp | 4 +- 5 files changed, 113 insertions(+), 1 deletion(-) diff --git a/include/lvr2/algorithm/GeometryAlgorithms.hpp b/include/lvr2/algorithm/GeometryAlgorithms.hpp index 1fe6dbccb..3e6beb51d 100644 --- a/include/lvr2/algorithm/GeometryAlgorithms.hpp +++ b/include/lvr2/algorithm/GeometryAlgorithms.hpp @@ -199,6 +199,18 @@ void calcVertexRoughnessAndHeightDifferences( template DenseEdgeMap calcVertexDistances(const BaseMesh& 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 +DenseVertexMap calcBorderCosts(const BaseMesh& mesh, double border_cost = 1.0); + + /** * @brief Dijkstra's algorithm * diff --git a/include/lvr2/algorithm/GeometryAlgorithms.tcc b/include/lvr2/algorithm/GeometryAlgorithms.tcc index b897dfcca..29d89fba6 100644 --- a/include/lvr2/algorithm/GeometryAlgorithms.tcc +++ b/include/lvr2/algorithm/GeometryAlgorithms.tcc @@ -491,6 +491,64 @@ DenseEdgeMap calcVertexDistances(const BaseMesh &mesh) return distances; } + +template +DenseVertexMap calcBorderCosts( + const BaseMesh &mesh, + double border_cost) +{ + DenseVertexMap 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: diff --git a/src/tools/lvr2_hdf5_mesh_tool/HDF5MeshTool.cpp b/src/tools/lvr2_hdf5_mesh_tool/HDF5MeshTool.cpp index 7151084b3..91ce17758 100644 --- a/src/tools/lvr2_hdf5_mesh_tool/HDF5MeshTool.cpp +++ b/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 borderCosts; + boost::optional> borderCostsOpt; + if (readFromHdf5) + { + borderCostsOpt = hdf5In.getDenseAttributeMap>("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>( + 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 { diff --git a/src/tools/lvr2_hdf5_mesh_tool/Options.cpp b/src/tools/lvr2_hdf5_mesh_tool/Options.cpp index 79a020629..eae1cc88a 100644 --- a/src/tools/lvr2_hdf5_mesh_tool/Options.cpp +++ b/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()->default_value("mesh"), "The name of the mesh to write") ("edgeCollapse,e", value()->default_value(0), "Edge collapse reduction algorithm, the number of edges to collapse.") ("localRadius,r", value()->default_value(0.3), "The local radius used for roughness and height differences computation.") + ("borderVertexCost,b", value()->default_value(1.0), "Setting the each vertex on the border of the mesh to this value (seperate 'border' layer)") ; diff --git a/src/tools/lvr2_hdf5_mesh_tool/Options.hpp b/src/tools/lvr2_hdf5_mesh_tool/Options.hpp index e43052f8e..ec26b5ff3 100644 --- a/src/tools/lvr2_hdf5_mesh_tool/Options.hpp +++ b/src/tools/lvr2_hdf5_mesh_tool/Options.hpp @@ -64,6 +64,7 @@ class Options { string getMeshName() const { return m_variables["meshName"].as();} size_t getEdgeCollapseNum() const { return m_variables["edgeCollapse"].as();} float getLocalRadius() const { return m_variables["localRadius"].as();} + float getBorderVertexCost() const { return m_variables["borderVertexCost"].as();} 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_ */