diff --git a/CMakeLists.txt b/CMakeLists.txt index 67a5ada..f6b770b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.21) -project (building-reconstruction VERSION 0.3.2) +project (building-reconstruction VERSION 0.4.5) option(GFP_WITH_PDAL "Build parts dependent on PDAL" OFF) @@ -17,7 +17,7 @@ endif(GFP_WITH_PDAL) add_library(ptinpoly STATIC thirdparty/ptinpoly/ptinpoly.c) set_target_properties( ptinpoly PROPERTIES - C_STANDARD 11 + C_STANDARD 90 POSITION_INDEPENDENT_CODE ON ) @@ -81,6 +81,7 @@ geoflow_create_plugin( src/MeshSimplify.cpp src/MeshGridSimplify.cpp src/Mesh2TriangleCollectionNode.cpp + src/tinsimp.cpp ) target_include_directories(gfp_buildingreconstruction PRIVATE @@ -109,4 +110,4 @@ if (MSVC) FILES_MATCHING PATTERN "*.dll" PATTERN "gfp*" EXCLUDE) -endif() \ No newline at end of file +endif() diff --git a/register.hpp b/register.hpp index da96719..a745bff 100644 --- a/register.hpp +++ b/register.hpp @@ -78,8 +78,11 @@ void register_nodes(geoflow::NodeRegister& node_register) { node_register.register_node("MaxInscribedCircle"); node_register.register_node("MeshSimplifyFastQuad"); node_register.register_node("MeshSimplify"); + node_register.register_node("SurfaceMesh2OFF"); node_register.register_node("MeshGridSimplify"); node_register.register_node("MeshClipper"); node_register.register_node("Mesh2TriangleCollection"); node_register.register_node("Mesh2CGALSurfaceMesh"); + node_register.register_node("RoofPartition3DBAGRasterise"); + node_register.register_node("MeshSimplify2D"); } \ No newline at end of file diff --git a/src/Mesh2TriangleCollectionNode.cpp b/src/Mesh2TriangleCollectionNode.cpp index 892bb2c..61707e5 100644 --- a/src/Mesh2TriangleCollectionNode.cpp +++ b/src/Mesh2TriangleCollectionNode.cpp @@ -2,6 +2,12 @@ #include #include +#include + +#include +#include +#include +#include namespace geoflow::nodes::stepedge { @@ -30,7 +36,7 @@ namespace geoflow::nodes::stepedge { TriangleCollection triangleCollection; vec3f normals; - for (auto& f : smesh.faces()) { + for (auto f : smesh.faces()) { Triangle t; unsigned i = 0; @@ -61,8 +67,9 @@ namespace geoflow::nodes::stepedge { SurfaceMesh smesh; { - std::map vertex_map; + std::map vertex_map; std::set vertex_set; + std::vector points; for (const auto &ring : gfmesh.get_polygons()) { for (auto &v : ring) @@ -70,22 +77,44 @@ namespace geoflow::nodes::stepedge { auto [it, did_insert] = vertex_set.insert(v); if (did_insert) { - vertex_map[v] = smesh.add_vertex(K::Point_3(v[0],v[1],v[2]));; + vertex_map[v] = points.size(); + points.push_back(K::Point_3(v[0],v[1],v[2])); } } } - + + // First build a polygon soup + std::vector > polygons; for (auto& ring : gfmesh.get_polygons()) { - std::vector rindices; + std::vector rindices; rindices.reserve(ring.size()); for(auto& p : ring) { rindices.push_back(vertex_map[p]); } - smesh.add_face(rindices); + polygons.push_back(rindices); } + + // Do CGAL mesh repair magic, see https://github.com/CGAL/cgal/issues/7529 + + // remove all kinds of typical issues in a polygon soup (degenerate polygons, isolated points, etc.) + CGAL::Polygon_mesh_processing::repair_polygon_soup(points, polygons); + + // duplicate non-manifold edges (but does not re-orient faces) + CGAL::Polygon_mesh_processing::duplicate_non_manifold_edges_in_polygon_soup(points, polygons); + + CGAL::Polygon_mesh_processing::polygon_soup_to_polygon_mesh(points, polygons, smesh); + + if(!CGAL::is_triangle_mesh(smesh)) PMP::triangulate_faces(smesh); + + // this prevents potentially getting stuck in infinite loop (see https://github.com/CGAL/cgal/issues/7529) + CGAL::Polygon_mesh_processing::duplicate_non_manifold_vertices( smesh ); + + // this is not needed since PMP::repair_polygon_soup() will perform this repair + // CGAL::Polygon_mesh_processing::remove_isolated_vertices( smesh ); + } - if(!CGAL::is_triangle_mesh(smesh)) PMP::triangulate_faces(smesh); + output("cgal_surface_mesh").set(smesh); } diff --git a/src/MeshClipperNode.cpp b/src/MeshClipperNode.cpp index 524374b..64f31fe 100644 --- a/src/MeshClipperNode.cpp +++ b/src/MeshClipperNode.cpp @@ -28,6 +28,43 @@ namespace geoflow::nodes::stepedge { return t; } + struct MeshBuilder { + std::map vertex_map; + std::set vertex_set; + std::vector points; + + std::vector > polygons; + + void add_point(const K::Point_3& p) { + auto [it, did_insert] = vertex_set.insert(p); + if (did_insert) + { + vertex_map[p] = points.size(); + points.push_back(p); + } + } + + void add_triangle(const K::Point_3& p0, const K::Point_3& p1, const K::Point_3& p2) { + add_point(p0); + add_point(p1); + add_point(p2); + + // First build a polygon soup + std::vector rindices; + rindices.reserve(3); + rindices.push_back(vertex_map[p0]); + rindices.push_back(vertex_map[p1]); + rindices.push_back(vertex_map[p2]); + polygons.push_back(rindices); + } + + void get_mesh(SurfaceMesh& smesh) { + smesh.clear(); + CGAL::Polygon_mesh_processing::polygon_soup_to_polygon_mesh(points, polygons, smesh); + } + + }; + void MeshClipperNode::process() { typedef SurfaceMesh::Vertex_index VertexIndex; @@ -38,32 +75,6 @@ namespace geoflow::nodes::stepedge { namespace PMP = CGAL::Polygon_mesh_processing; auto smesh = input("mesh").get(); - - // SurfaceMesh smesh; - // { - // std::map vertex_map; - // std::set vertex_set; - // for (const auto &ring : gfmesh.get_polygons()) - // { - // for (auto &v : ring) - // { - // auto [it, did_insert] = vertex_set.insert(v); - // if (did_insert) - // { - // vertex_map[v] = smesh.add_vertex(Point_3(v[0],v[1],v[2]));; - // } - // } - // } - - // for (auto& ring : gfmesh.get_polygons()) { - // std::vector rindices; - // rindices.reserve(ring.size()); - // for(auto& p : ring) { - // rindices.push_back(vertex_map[p]); - // } - // smesh.add_face(rindices); - // } - // } // clip if(!CGAL::is_triangle_mesh(smesh)) PMP::triangulate_faces(smesh); @@ -76,12 +87,6 @@ namespace geoflow::nodes::stepedge { Point_3(pmax[0], pmax[1], pmax[2]) ); - TriangleCollection triangleCollection; - vec3f normals; - - auto fnormals = smesh.add_property_map("f:normals", CGAL::NULL_VECTOR).first; - PMP::compute_face_normals(smesh, fnormals); - if (!skip_clip_) { if (cgal_clip_) { // use cgal mesh_processing if (!PMP::does_self_intersect(smesh)) { @@ -91,8 +96,9 @@ namespace geoflow::nodes::stepedge { ); } } else { + MeshBuilder mb; CGAL::Vertex_around_face_iterator vit, vend; - for (auto& f : smesh.faces()) { + for (auto f : smesh.faces()) { boost::tie(vit, vend) = CGAL::vertices_around_face(smesh.halfedge(f), smesh); auto end = vit; K::Point_3 p1 = smesh.point(*vit); vit++; @@ -106,56 +112,78 @@ namespace geoflow::nodes::stepedge { const auto result = CGAL::intersection(triangle, cuboid); if (result) { - auto& n = fnormals[f]; + // auto& n = fnormals[f]; if (const Triangle_3* tri = boost::get(&*result)) { - triangleCollection.push_back(create_gf_triangle( + mb.add_triangle( tri->vertex(0), tri->vertex(1), tri->vertex(2) - )); - normals.push_back(arr3f{ float(n.x()), float(n.y()), float(n.z()) }); - normals.push_back(arr3f{ float(n.x()), float(n.y()), float(n.z()) }); - normals.push_back(arr3f{ float(n.x()), float(n.y()), float(n.z()) }); + ); } else if (const Poly_3* poly = boost::get(&*result)) { // std::cout << "polygon! [" << poly->size() << std::endl; - for(unsigned i=0; isize()-2; ++i) { // std::cout << i << " "; - triangleCollection.push_back(create_gf_triangle( + mb.add_triangle( (*poly)[0], (*poly)[i+1], (*poly)[i+2] - )); - normals.push_back(arr3f{ float(n.x()), float(n.y()), float(n.z()) }); - normals.push_back(arr3f{ float(n.x()), float(n.y()), float(n.z()) }); - normals.push_back(arr3f{ float(n.x()), float(n.y()), float(n.z()) }); + ); } } } } + mb.get_mesh(smesh); } } - if (skip_clip_ || cgal_clip_) { - for (auto& f : smesh.faces()) { - Triangle t; - unsigned i = 0; - - for(VertexIndex vi : vertices_around_face(smesh.halfedge(f), smesh)) { - auto& p = smesh.point(vi); - t[i++] = arr3f{ + auto fnormals = smesh.add_property_map("f:normals", CGAL::NULL_VECTOR).first; + auto vnormals = smesh.add_property_map("v:normals", CGAL::NULL_VECTOR).first; + + if (smooth_normals_) { + PMP::compute_vertex_normals(smesh, vnormals); + } else { + PMP::compute_face_normals(smesh, fnormals); + } + + // convert to triangleCollection + TriangleCollection triangleCollection; + vec3f normals; + for (auto f : smesh.faces()) { + Triangle t; + + unsigned i = 0; + for(VertexIndex vi : vertices_around_face(smesh.halfedge(f), smesh)) { + if(i==3) { + std::cout << "WARNING: skipping triangulated SurfaceMesh face with more than 3 vertices\n"; + continue; + } + auto& p = smesh.point(vi); + t[i++] = arr3f{ (float) p.x(), (float) p.y(), (float) p.z() - }; + }; + // if (!smesh.is_border(vi)) { + if (smooth_normals_) { + normals.push_back(arr3f{ + float(vnormals[vi].x()), + float(vnormals[vi].y()), + float(vnormals[vi].z()) }); + } else { + normals.push_back(arr3f{ + float(fnormals[f].x()), + float(fnormals[f].y()), + float(fnormals[f].z()) }); } - auto& n = fnormals[f]; - normals.push_back(arr3f{ float(n.x()), float(n.y()), float(n.z()) }); - normals.push_back(arr3f{ float(n.x()), float(n.y()), float(n.z()) }); - normals.push_back(arr3f{ float(n.x()), float(n.y()), float(n.z()) }); - triangleCollection.push_back(t); - } + } + // if (!smooth_normals_) { + + + // normals.push_back(arr3f{ float(n.x()), float(n.y()), float(n.z()) }); + // normals.push_back(arr3f{ float(n.x()), float(n.y()), float(n.z()) }); + // } + triangleCollection.push_back(t); } output("triangles").set(triangleCollection); @@ -163,4 +191,4 @@ namespace geoflow::nodes::stepedge { // output("cgal_surface_mesh").set(smesh); } -} \ No newline at end of file +} diff --git a/src/MeshGridSimplify.cpp b/src/MeshGridSimplify.cpp index 8b00f6b..887588a 100644 --- a/src/MeshGridSimplify.cpp +++ b/src/MeshGridSimplify.cpp @@ -144,7 +144,7 @@ namespace geoflow::nodes::stepedge { // merge vertices in each grid cell, and create a mapping (old vertex indices -> new vertex indices, std::map) Grid2D G(box, cell_size_xy_, cell_size_z_); std::unordered_map vertex_map; - for (auto& vi : smesh.vertices()) { + for (auto vi : smesh.vertices()) { unsigned c = G.getCellCoordinate( smesh.point(vi) ); vertex_map[vi] = c; } @@ -161,7 +161,7 @@ namespace geoflow::nodes::stepedge { // Cluster(float x, float y) : x(x), y(y) {}; // }; std::unordered_map new_vertices; - for (auto& f : smesh.faces()) { + for (auto f : smesh.faces()) { for(VertexIndex vi : vertices_around_face(smesh.halfedge(f), smesh)) { auto p = G.getCellCenterPoint(vertex_map[vi]); new_mesh_vertices[vertex_map[vi]] = smesh_new.add_vertex(K::Point_3(p[0], p[1], p[2])); @@ -169,7 +169,7 @@ namespace geoflow::nodes::stepedge { } } { - for (auto& f : smesh.faces()) { + for (auto f : smesh.faces()) { std::set face_set; for(VertexIndex vi : vertices_around_face(smesh.halfedge(f), smesh)) { face_set.insert(vertex_map[vi]); diff --git a/src/MeshProcessingNodes.hpp b/src/MeshProcessingNodes.hpp index fb43136..6036610 100644 --- a/src/MeshProcessingNodes.hpp +++ b/src/MeshProcessingNodes.hpp @@ -1,9 +1,11 @@ #pragma once +#include #include #include #include +#include namespace geoflow::nodes::stepedge { typedef CGAL::Simple_cartesian K; @@ -12,6 +14,7 @@ namespace geoflow::nodes::stepedge { class MeshClipperNode:public Node { bool skip_clip_ = false; bool cgal_clip_ = false; + bool smooth_normals_ = false; // float reduce_fraction_ = 0.5; // float agressiveness_ = 7.0; @@ -27,6 +30,7 @@ namespace geoflow::nodes::stepedge { add_param(ParamBool(skip_clip_, "skip_clip", "Skip the clip")); add_param(ParamBool(cgal_clip_, "cgal_clip", "Use CGAL::Polygon_mesh_processing::clip instead of simpler but more robust triangle by triangle clip.")); + add_param(ParamBool(smooth_normals_, "smooth_normals", "Use use smooth vertex normals instead of flat face normals.")); // add_param(ParamFloat(reduce_fraction_, "reduce_fraction", "Target reduction in nr of triangles")); // add_param(ParamFloat(agressiveness_, "agressiveness", "Agressiveness")); // add_param(ParamInt(metrics_normal_k, "metrics_normal_k", "Number of neighbours used for normal estimation")); @@ -54,6 +58,23 @@ namespace geoflow::nodes::stepedge { void process() override; }; + class MeshSimplify2DNode:public Node { + float error_ = 0.5; + float minpts_ = 0.5; + + public: + using Node::Node; + void init() override { + add_input("cgal_surface_mesh", typeid(SurfaceMesh)); + add_output("cgal_surface_mesh", typeid(SurfaceMesh)); + // add_output("wall_triangles", typeid(TriangleCollection)); + + add_param(ParamBoundedFloat(error_, 0, 5, "error", "Target maximum eror after simplification")); + add_param(ParamBoundedFloat(minpts_, 0, 10, "minpts", "Minimum number of elevation points per m2 inside a polygon")); + } + void process() override; + }; + class MeshGridSimplifyNode:public Node { float cell_size_xy_ = 0.5; float cell_size_z_ = 0.5; @@ -111,5 +132,17 @@ namespace geoflow::nodes::stepedge { } void process() override; }; + class SurfaceMesh2OFFNode:public Node { + std::string filepath_=""; + + public: + using Node::Node; + void init() override { + add_input("cgal_surface_mesh", typeid(SurfaceMesh)); + + add_param(ParamPath(filepath_, "filepath", "File path")); + } + void process() override; + }; } \ No newline at end of file diff --git a/src/MeshSimplify.cpp b/src/MeshSimplify.cpp index 664df94..28dbd28 100644 --- a/src/MeshSimplify.cpp +++ b/src/MeshSimplify.cpp @@ -9,14 +9,30 @@ // Stop-condition policy #include +#include + #include #include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "tinsimp.hpp" + namespace geoflow::nodes::stepedge { typedef SurfaceMesh::Vertex_index VertexIndex; typedef SurfaceMesh::Face_index FaceIndex; + typedef boost::graph_traits::vertex_descriptor vertex_descriptor; typedef boost::graph_traits::halfedge_descriptor halfedge_descriptor; typedef boost::graph_traits::edge_descriptor edge_descriptor; namespace SMS = CGAL::Surface_mesh_simplification; @@ -36,12 +52,179 @@ namespace geoflow::nodes::stepedge { // Placement class typedef SMS::Constrained_placement, Border_is_constrained_edge_map > Placement; - namespace PMP = CGAL::Polygon_mesh_processing; + // namespace d = CGAL::Polygon_mesh_processing; + + struct MeshBuilder { + std::map vertex_map; + std::set vertex_set; + std::vector points; + + std::vector > polygons; + + void add_point(const K::Point_3& p) { + auto [it, did_insert] = vertex_set.insert(p); + if (did_insert) + { + vertex_map[p] = points.size(); + points.push_back(p); + } + } + void add_points(const K::Point_3& p0, const K::Point_3& p1, const K::Point_3& p2) { + add_point(p0); + add_point(p1); + add_point(p2); + } + + void add_triangle(const K::Point_3& p0, const K::Point_3& p1, const K::Point_3& p2) { + // First build a polygon soup + add_points(p0, p1, p2); + std::vector rindices; + rindices.reserve(3); + rindices.push_back(vertex_map[p0]); + rindices.push_back(vertex_map[p1]); + rindices.push_back(vertex_map[p2]); + polygons.push_back(rindices); + } + + void get_mesh(geoflow::nodes::stepedge::SurfaceMesh& smesh) { + smesh.clear(); + CGAL::Polygon_mesh_processing::repair_polygon_soup(points, polygons); + + // duplicate non-manifold edges (but does not re-orient faces) + CGAL::Polygon_mesh_processing::duplicate_non_manifold_edges_in_polygon_soup(points, polygons); + CGAL::Polygon_mesh_processing::polygon_soup_to_polygon_mesh(points, polygons, smesh); + } + + }; + + const K::Vector_3 up(0,0,1); + + bool is_vertical(const K::Point_3& a, const K::Point_3& b, const K::Point_3& c) { + auto n = CGAL::orthogonal_vector(a, b, c); + // check if face is vertical by checking angle of it's normal. Only add constraints for non-vertical faces + // std::cout << CGAL::abs(CGAL::approximate_angle(n, up))<< std::endl; + return (CGAL::abs(90 - CGAL::abs(CGAL::approximate_angle(n, up))) == 0); + } + + void MeshSimplify2DNode::process() { + auto smesh = input("cgal_surface_mesh").get(); + + // !d::does_self_intersect(smesh) + if (error_ > 0){ + tinsimp::CDT t; + + // collect vertical faces that make up the vertical wall + std::vector> wall_triangles; + for (auto face : smesh.faces()) { + std::vector triangle; + for(vertex_descriptor vd : vertices_around_face(smesh.halfedge(face), smesh)){ + triangle.push_back(smesh.point(vd)); + } + if (is_vertical(triangle[0], triangle[1], triangle[2])) { + wall_triangles.push_back(triangle); + CGAL::Euler::remove_face(smesh.halfedge(face), smesh); + } + } + smesh.collect_garbage(); + + for(halfedge_descriptor hd : halfedges(smesh)) + { + if(CGAL::is_border(hd, smesh)) + { + auto a = smesh.point(source(hd, smesh)); + auto b = smesh.point(target(hd, smesh)); + + t.insert_constraint( + tinsimp::Point( + a.x(), + a.y(), + a.z() + ), + tinsimp::Point( + b.x(), + b.y(), + b.z() + ) + ); + } + } + + std::vector zpts; + for(vertex_descriptor vd : smesh.vertices()) + { + if(!CGAL::is_border(vd, smesh)) + { + auto p = smesh.point(vd); + zpts.push_back( + tinsimp::Point( p.x(), p.y(), p.z() ) + ); + } + } + + tinsimp::mark_domains(t); + float sq_area = 0; + for (auto& fh : t.finite_face_handles()) { + if(fh->info().in_domain()) { + sq_area += t.triangle(fh).squared_area(); + } + } + float total_area = std::sqrt(sq_area); + + tinsimp::greedy_insert(t, zpts, error_, minpts_*total_area); + + // reset and recompute nesting levels + for (auto& fh : t.all_face_handles()) { + fh->info().nesting_level = -1; + } + tinsimp::mark_domains(t); + // std::cout << "\nFinished!\n" << r << " edges removed.\n" + // << smesh.number_of_edges() << " final edges.\n"; + + smesh.clear(); + MeshBuilder mb; + for (auto& fh : t.finite_face_handles()) { + if(fh->info().in_domain()) { + mb.add_triangle( + K::Point_3( + fh->vertex(0)->point().x(), + fh->vertex(0)->point().y(), + fh->vertex(0)->point().z() + ), + K::Point_3( + fh->vertex(1)->point().x(), + fh->vertex(1)->point().y(), + fh->vertex(1)->point().z() + ), + K::Point_3( + fh->vertex(2)->point().x(), + fh->vertex(2)->point().y(), + fh->vertex(2)->point().z() + ) + ); + } + } + for (auto& triangle : wall_triangles) { + mb.add_triangle(triangle[0], triangle[1], triangle[2]); + } + mb.get_mesh(smesh); + + // TriangleCollection tc; + // for ( auto& t : wall_triangles ) { + // Triangle gft; + // gft[0] = arr3f{static_cast(t[0].x()), static_cast(t[0].y()), static_cast(t[0].z())}; + // gft[1] = arr3f{static_cast(t[1].x()), static_cast(t[1].y()), static_cast(t[1].z())}; + // gft[2] = arr3f{static_cast(t[2].x()), static_cast(t[2].y()), static_cast(t[2].z())}; + // tc.push_back(gft); + // } + // output("wall_triangles").set(tc); + } + output("cgal_surface_mesh").set(smesh); + } void MeshSimplifyNode::process() { auto smesh = input("cgal_surface_mesh").get(); - // !PMP::does_self_intersect(smesh) + // !d::does_self_intersect(smesh) if (stop_ratio_ < 1.){ if(!CGAL::is_triangle_mesh(smesh)) CGAL::Polygon_mesh_processing::triangulate_faces(smesh); @@ -65,10 +248,15 @@ namespace geoflow::nodes::stepedge { // Contract the surface mesh as much as possible. Correct for the border edges that will not be removed SMS::Count_ratio_stop_predicate stop( stop_ratio ); Border_is_constrained_edge_map bem(smesh); + + // filter that checks if a placement would invert the normal of a face + SMS::Bounded_normal_change_filter<> filter; // This the actual call to the simplification algorithm. // The surface mesh and stop conditions are mandatory arguments. + int r = SMS::edge_collapse(smesh, stop, CGAL::parameters::edge_is_constrained_map(bem) + .filter(filter) .get_placement(Placement(bem))); // std::cout << "\nFinished!\n" << r << " edges removed.\n" // << smesh.number_of_edges() << " final edges.\n"; @@ -76,8 +264,16 @@ namespace geoflow::nodes::stepedge { output("cgal_surface_mesh").set(smesh); } - // void MeshGridClusterSimplifyNode::process() { + void SurfaceMesh2OFFNode::process() { - // } + auto smesh = input("cgal_surface_mesh").get(); + auto fname = manager.substitute_globals(filepath_); + + std::ofstream ofs; + ofs << std::fixed << std::setprecision(5); + ofs.open(fname); + CGAL::IO::write_OFF(ofs, smesh); + ofs.close(); + } } \ No newline at end of file diff --git a/src/detect_planes_node.cpp b/src/detect_planes_node.cpp index 49e3af7..b0264c6 100644 --- a/src/detect_planes_node.cpp +++ b/src/detect_planes_node.cpp @@ -13,13 +13,157 @@ // You should have received a copy of the GNU Affero General Public License // along with this program. If not, see . +#include "point_edge.h" #include "stepedge_nodes.hpp" -// #include - +#include #include "plane_detect.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +struct AdjacencyFinder { + + typedef CGAL::Search_traits_3 Traits_base; + typedef CGAL::Search_traits_adapter TreeTraits; + typedef CGAL::Orthogonal_k_neighbor_search Neighbor_search; + typedef Neighbor_search::Tree Tree; + + std::map> adjacencies; + + AdjacencyFinder(PNL_vector& points, size_t N=15) + { + Tree tree; + tree.insert(points.begin(), points.end()); + + for(auto& pi : points){ + auto& p = boost::get<0>(pi); + auto& l = boost::get<2>(pi); + if(l==0) continue; // skip unsegmented points + // std::cout << "pid:" << l << std::endl; + Neighbor_search search(tree, p, N+1); + // skip the first point since it is identical to the query point + for (auto nb = search.begin()+1 ; nb < search.end(); ++nb) { + // auto& p = boost::get<0>(pi); + auto l_nb = boost::get<2>(nb->first); + if(l_nb==0 || l_nb == l) continue; // skip unsegmented neighbours + // std::cout << "dist:" << nb->second << std::endl; + // std::cout << "p_x:" << boost::get<2>(nb->first) << std::endl; + // std::cout << "pid_:" << l_nb << std::endl; + if(l > l_nb) { + adjacencies[l][l_nb]++; + } else { + adjacencies[l_nb][l]++; + } + } + } + + // for(auto& [idl, value] : adjacencies) { + // for(auto& [idh, cnt] : value) { + // std::cout<< idl << ":" << idh << "-" << cnt << std::endl; + // } + // } + }; +}; + namespace geoflow::nodes::stepedge { + typedef CGAL::Shape_detection::Efficient_RANSAC_traits + Traits; + typedef CGAL::Shape_detection::Efficient_RANSAC Efficient_ransac; + typedef CGAL::Shape_detection::Plane RansacPlane; + + + // struct Custom_point_map + // { + // using key_type = arr3f; // The iterator's value type is an index + // using value_type = Point; // The object manipulated by the algorithm is a Point + // using reference = Point; // The object does not exist in memory, so there's no reference + // using category = boost::readable_property_map_tag; // The property map is used both + // // for reading and writing data + // // The get() function returns the object expected by the algorithm (here, Point) + // friend Point get (const Custom_point_map& map, const arr3f& p) + // { + // return Point (p[0], + // p[1], + // p[2]); + // } + // }; + struct Custom_plane_map + { + using key_type = Plane; // The iterator's value type is an index + using value_type = Plane; // The object manipulated by the algorithm is a Plane + using reference = Plane&; // The object does not exist in memory, so there's no reference + using category = boost::read_write_property_map_tag; // The property map is used both + // for reading and writing data + // The get() function returns the object expected by the algorithm (here, Point) + friend Plane get (const Custom_plane_map& map, Plane& plane) + { + return plane; + } + friend const Plane get (const Custom_plane_map& map, const Plane& plane) + { + return plane; + } + // The put() function updated the user's data structure from the + // object handled by the algorithm (here Plane) + friend void put (const Custom_plane_map& map, Plane& plane_old, const Plane& plane_new) + { + plane_old = plane_new; + } + }; + struct Custom_plane_index_map + { + using key_type = std::size_t; // The iterator's value type is an index + using value_type = int; // The object manipulated by the algorithm is a Plane + using reference = int; // The object does not exist in memory, so there's no reference + using category = boost::readable_property_map_tag; // The property map is used both + // for reading and writing data + PNL_vector* plane_id; + Custom_plane_index_map (PNL_vector* plane_id=nullptr) + : plane_id (plane_id) { } + // The get() function returns the object expected by the algorithm (here, Plane) + // return plane based on point idx + friend int get (const Custom_plane_index_map& map, const std::size_t& idx) + { + auto pid = boost::get<2>((*map.plane_id)[idx]); + if (pid == 0) + return -1; + else + return pid-1; + } + }; + + + // allow us to hash Plane instances + template + inline void hash_combine(std::size_t& seed, const T& v) + { + std::hash hasher; + seed ^= hasher(v) + 0x9e3779b9 + (seed<<6) + (seed>>2); + } + + struct PlaneHash { + std::size_t operator()(const Plane& k) const + { + size_t seed = std::hash{}(k.a()); + boost::hash_combine(seed, k.b()); + boost::hash_combine(seed, k.c()); + boost::hash_combine(seed, k.d()); + return seed; + } + }; + void DetectPlanesNode::process() { auto points = input("points").get(); @@ -49,105 +193,234 @@ namespace geoflow::nodes::stepedge { boost::get<1>(pv) = -n; } - // convert to lists required by the planedetector class - // size_t i=0; PointCollection points_vec; vec3f normals_vec; points_vec.reserve(points.size()); - for (auto &pt : pnl_points) { - auto& p = boost::get<0>(pt); - auto& n = boost::get<1>(pt); - points_vec.push_back( - {float(CGAL::to_double(p.x())), float(CGAL::to_double(p.y())), float(CGAL::to_double(p.z()))} - ); - normals_vec.push_back( - {float(CGAL::to_double(n.x())), float(CGAL::to_double(n.y())), float(CGAL::to_double(n.z()))} - ); - } - // perform plane detection - planedect::PlaneDS PDS(points_vec, normals_vec, metrics_plane_k); - planedect::DistAndNormalTester DNTester( - metrics_plane_epsilon * metrics_plane_epsilon, - metrics_plane_normal_threshold, - n_refit - ); - regiongrower::RegionGrower R; - R.min_segment_count = metrics_plane_min_points; - if(points.size()>metrics_plane_min_points) - R.grow_regions(PDS, DNTester); - - - // if (regularise_planes) { - // // Regularize detected planes. - // CGAL::regularize_planes(points, - // Point_map(), - // planes, - // CGAL::Shape_detection::Plane_map(), - // CGAL::Shape_detection::Point_to_shape_index_map(points, planes), - // true, // regularize parallelism - // true, // regularize orthogonality - // false, // do not regularize coplanarity - // true, // regularize Z-symmetry (default) - // } - - // classify horizontal/vertical planes using plane normals IndexedPlanesWithPoints pts_per_roofplane; size_t horiz_roofplane_cnt=0; size_t slant_roofplane_cnt=0; - if (only_horizontal) pts_per_roofplane[-1].second = std::vector(); - size_t horiz_pt_cnt=0, total_pt_cnt=0, wall_pt_cnt=0, unsegmented_pt_cnt=0; + size_t horiz_pt_cnt=0, total_pt_cnt=0, wall_pt_cnt=0, unsegmented_pt_cnt=0, total_plane_cnt=0; vec1f roof_elevations; - - for(auto region: R.regions){ - auto& plane = region.plane; - output("planes").push_back(plane); - Vector n = plane.orthogonal_vector(); - // this dot product is close to 0 for vertical planes - auto horizontality = CGAL::abs(n*Vector(0,0,1)); - bool is_wall = horizontality < metrics_is_wall_threshold; - bool is_horizontal = horizontality > metrics_is_horizontal_threshold; - // put slanted surface points at index -1 if we care only about horzontal surfaces - if (!is_wall) { - std::vector segpts; - for (auto& i : region.inliers) { - segpts.push_back(boost::get<0>(pnl_points[i])); - roof_elevations.push_back(float(boost::get<0>(pnl_points[i]).z())); + + std::vector planes; + + if (!use_ransac) { + // convert to lists required by the planedetector class + // size_t i=0; + + for (auto &pt : pnl_points) { + auto& p = boost::get<0>(pt); + auto& n = boost::get<1>(pt); + points_vec.push_back( + {float(CGAL::to_double(p.x())), float(CGAL::to_double(p.y())), float(CGAL::to_double(p.z()))} + ); + normals_vec.push_back( + {float(CGAL::to_double(n.x())), float(CGAL::to_double(n.y())), float(CGAL::to_double(n.z()))} + ); + } + // perform plane detection + planedect::PlaneDS PDS(points_vec, normals_vec, metrics_plane_k); + planedect::DistAndNormalTester DNTester( + metrics_plane_epsilon * metrics_plane_epsilon, + metrics_plane_normal_threshold, + n_refit + ); + regiongrower::RegionGrower R; + R.min_segment_count = metrics_plane_min_points; + if(points.size()>metrics_plane_min_points) + R.grow_regions(PDS, DNTester); + + total_plane_cnt = R.regions.size(); + // classify horizontal/vertical planes using plane normals + unsigned shape_id = 0; + for(auto region: R.regions){ + + if(region.get_region_id() == 0) continue; + + auto& plane = region.plane; + + Vector n = plane.orthogonal_vector(); + // this dot product is close to 0 for vertical planes + auto horizontality = CGAL::abs(n*Vector(0,0,1)); + bool is_wall = horizontality < metrics_is_wall_threshold; + bool is_horizontal = horizontality > metrics_is_horizontal_threshold; + + // put slanted surface points at index -1 if we care only about horzontal surfaces + if (!is_wall) { + ++shape_id; + planes.push_back(plane); + std::vector segpts; + for (auto& i : region.inliers) { + segpts.push_back(boost::get<0>(pnl_points[i])); + roof_elevations.push_back(float(boost::get<0>(pnl_points[i]).z())); + boost::get<2>(pnl_points[i]) = shape_id; + boost::get<3>(pnl_points[i]) = is_wall; + boost::get<9>(pnl_points[i]) = is_horizontal; + } + total_pt_cnt += segpts.size(); + pts_per_roofplane[shape_id].second = segpts; + pts_per_roofplane[shape_id].first = plane; + + if (is_horizontal) { + horiz_pt_cnt += segpts.size(); + } + } else { // is_wall + wall_pt_cnt += region.inliers.size(); + } + if (is_horizontal) + ++horiz_roofplane_cnt; + else if (!is_wall && !is_horizontal) + ++slant_roofplane_cnt; + } + + } else { // use_ransac == true + + // Instantiate shape detection engine. + Efficient_ransac ransac; + // Provide input data. + ransac.set_input(pnl_points); + // Register planar shapes via template method. + ransac.add_shape_factory(); + + // Set parameters for shape detection. + Efficient_ransac::Parameters parameters; + // Set probability to miss the largest primitive at each iteration. + parameters.probability = metrics_probability_ransac; + // Detect shapes with at least 200 points. + parameters.min_points = metrics_plane_min_points; + // Set maximum Euclidean distance between a point and a shape. + parameters.epsilon = metrics_plane_epsilon; + // Set maximum Euclidean distance between points to be clustered. + parameters.cluster_epsilon = metrics_cluster_epsilon_ransac; + // Set maximum normal deviation. + // 0.9 < dot(surface_normal, point_normal); + parameters.normal_threshold = metrics_plane_normal_threshold; + // Detect shapes. + ransac.detect(parameters); + // Print number of detected shapes. + total_plane_cnt = ransac.shapes().end() - ransac.shapes().begin(); + + unsigned shape_id = 0; + for(auto shape: ransac.shapes()){ + + RansacPlane* ransac_plane = dynamic_cast(shape.get()); + Plane plane = static_cast(*ransac_plane); + Vector n = plane.orthogonal_vector(); + // this dot product is close to 0 for vertical planes + auto horizontality = CGAL::abs(n*Vector(0,0,1)); + bool is_wall = horizontality < metrics_is_wall_threshold; + bool is_horizontal = horizontality > metrics_is_horizontal_threshold; + // put slanted surface points at index -1 if we care only about horzontal surfaces + if (!is_wall) { + ++shape_id; + planes.push_back(plane); + std::vector segpts; + for (auto& i : shape->indices_of_assigned_points()) { + segpts.push_back(boost::get<0>(pnl_points[i])); + roof_elevations.push_back(float(boost::get<0>(pnl_points[i]).z())); + boost::get<2>(pnl_points[i]) = shape_id; + boost::get<3>(pnl_points[i]) = is_wall; + boost::get<9>(pnl_points[i]) = is_horizontal; + } + total_pt_cnt += segpts.size(); + pts_per_roofplane[shape_id].second = segpts; + pts_per_roofplane[shape_id].first = plane; + + if (is_horizontal) { + horiz_pt_cnt += segpts.size(); + } + } else { // is_wall + wall_pt_cnt += shape->indices_of_assigned_points().size(); } - total_pt_cnt += segpts.size(); - if (!only_horizontal || - (only_horizontal && is_horizontal)) { - pts_per_roofplane[region.get_region_id()].second = segpts; - pts_per_roofplane[region.get_region_id()].first = plane; - } else if (!is_horizontal) { - pts_per_roofplane[-1].second.insert( - pts_per_roofplane[-1].second.end(), - segpts.begin(), - segpts.end() - ); - } - if (is_horizontal) { - horiz_pt_cnt += segpts.size(); + if (is_horizontal) + ++horiz_roofplane_cnt; + else if (!is_wall && !is_horizontal) + ++slant_roofplane_cnt; + + } + } + + vec1i plane_id, is_wall, is_horizontal; + for(auto& p : pnl_points) { + auto pid = boost::get<2>(p); + if (pid==0) ++unsegmented_pt_cnt; + plane_id.push_back(pid); + is_wall.push_back(boost::get<3>(p)); + is_horizontal.push_back(boost::get<9>(p)); + } + + // Plane regularisation + std::cout << "\nN planes before: " << pts_per_roofplane.size() << std::endl; + + // START Regularize detected planes. + if (regularize_parallelism_ || + regularize_orthogonality_ || + regularize_coplanarity_ || + regularize_axis_symmetry_) { + CGAL::Shape_regularization::Planes::regularize_planes( + planes, + pnl_points, + CGAL::parameters:: + plane_map(Custom_plane_map()). + point_map(Point_map()). + plane_index_map(Custom_plane_index_map(&pnl_points)). + maximum_angle(maximum_angle_). + maximum_offset(maximum_offset_). + regularize_parallelism(regularize_parallelism_). + regularize_orthogonality(regularize_orthogonality_). + regularize_coplanarity(regularize_coplanarity_). + regularize_axis_symmetry(regularize_axis_symmetry_) + // symmetry_direction(symmetry_direction_) + ); + + std::unordered_map, PlaneHash> plane_merge_map; + size_t pt_i=0; + for (auto& p :pnl_points) { + // std::cout << "pt_i=" << pt_i << std::endl; + auto pid = boost::get<2>(p); + // boost::get<2>(p) = 0; + // std::cout << "pid=" << pid << std::endl; + if (pid > 0){ + const auto& pl = planes[pid-1]; + plane_merge_map[pl].push_back( pt_i ); } - } else { // is_wall - wall_pt_cnt = region.inliers.size(); + ++pt_i; } - if (is_horizontal) - ++horiz_roofplane_cnt; - else if (!is_wall && !is_horizontal) - ++slant_roofplane_cnt; - - for (size_t& i : region.inliers) { - boost::get<2>(pnl_points[i]) = region.get_region_id(); - boost::get<3>(pnl_points[i]) = is_wall; - boost::get<9>(pnl_points[i]) = is_horizontal; + std::cout << "plane_merge_map.size=" << plane_merge_map.size() << std::endl; + pts_per_roofplane.clear(); + + int plane_cnt = 1; + for(auto& [plane, pt_i_vec] : plane_merge_map) { + Vector n = plane.orthogonal_vector(); + // this dot product is close to 0 for vertical planes + auto horizontality = CGAL::abs(n*Vector(0,0,1)); + bool is_wall = horizontality < metrics_is_wall_threshold; + + if (!is_wall) { + std::vector ptvec; + ptvec.reserve(pt_i_vec.size()); + for (auto& pt_i : pt_i_vec) { + boost::get<2>(pnl_points[pt_i]) = plane_cnt; + plane_id[pt_i] = plane_cnt; + ptvec.push_back(boost::get<0>(pnl_points[pt_i])); + } + pts_per_roofplane[plane_cnt] = std::make_pair(plane, ptvec); + ++plane_cnt; + } } + std::cout << "N planes after: " << plane_cnt-1 << std::endl; } + // END Regularize detected planes. + + AdjacencyFinder adj_finder(pnl_points, metrics_plane_k); + output("plane_adj").set(adj_finder.adjacencies); + bool b_is_horizontal = float(horiz_pt_cnt)/float(total_pt_cnt) > horiz_min_count; // int roof_type=-2; // as built: -2=undefined; -1=no pts; 0=LOD1, 1=LOD1.3, 2=LOD2 std::string roof_type = "no planes"; - if (R.regions.size()==0) { + if (total_plane_cnt==0) { // roof_type=-1; roof_type = "no points"; } else if (horiz_roofplane_cnt==1 && slant_roofplane_cnt==0){ @@ -180,21 +453,13 @@ namespace geoflow::nodes::stepedge { output("roof_pt_cnt").set((int)total_pt_cnt); output("wall_pt_cnt").set((int)wall_pt_cnt); - vec1i plane_id, is_wall, is_horizontal; - for(auto& p : pnl_points) { - auto pid = boost::get<2>(p); - if (pid==0) ++unsegmented_pt_cnt; - plane_id.push_back(pid); - is_wall.push_back(boost::get<3>(p)); - is_horizontal.push_back(boost::get<9>(p)); - } output("unsegmented_pt_cnt").set((int)unsegmented_pt_cnt); output("pts_per_roofplane").set(pts_per_roofplane); output("plane_id").set(plane_id); output("is_wall").set(is_wall); output("is_horizontal").set(is_horizontal); - output("plane_adj").set(R.adjacencies); + } } \ No newline at end of file diff --git a/src/heightfield_nodes.cpp b/src/heightfield_nodes.cpp index 5b96109..65d1fb8 100644 --- a/src/heightfield_nodes.cpp +++ b/src/heightfield_nodes.cpp @@ -15,10 +15,20 @@ // along with this program. If not, see . // triangulation #include +#include #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include "Raster.h" +#include "point_edge.h" #include "stepedge_nodes.hpp" #include "pip_util.hpp" @@ -562,6 +572,138 @@ namespace geoflow::nodes::stepedge { output("image").set(I); } + void rasterise_ring(LinearRing& polygon, RasterTools::Raster& r) { + typedef double FT; + typedef CGAL::Simple_cartesian K; + + K::Plane_3 plane; + std::vector pts; + for (auto& p : polygon) { + pts.push_back(K::Point_3(p[0], p[1], p[2])); + } + // fit plane to polygon pts + linear_least_squares_fitting_3(pts.begin(),pts.end(),plane,CGAL::Dimension_tag<0>()); + + auto box = polygon.box(); + auto bb_min = box.min(); + auto bb_max = box.max(); + auto cr_min = r.getColRowCoord(bb_min[0], bb_min[1]); + auto cr_max = r.getColRowCoord(bb_max[0], bb_max[1]); + + auto points_inside = r.rasterise_polygon(polygon, cr_min, cr_max); + for (auto& p : points_inside) { + float z_interpolate = -plane.a()/plane.c() * p[0] - plane.b()/plane.c()*p[1] - plane.d()/plane.c(); + r.add_point(p[0], p[1], z_interpolate, RasterTools::MAX); + } + + } + + void calculate_h_attr(gfSingleFeatureInputTerminal& roofparts, gfMultiFeatureOutputTerminal& h_attr, RasterTools::Raster& r_lod22, float z_offset) { + h_attr.add_vector("h_min", typeid(float)); + h_attr.add_vector("h_max", typeid(float)); + h_attr.add_vector("h_50p", typeid(float)); + h_attr.add_vector("h_70p", typeid(float)); + for (size_t i=0; i< roofparts.size(); ++i) { + auto polygon = roofparts.get(i); + auto box = polygon.box(); + auto bb_min = box.min(); + auto bb_max = box.max(); + auto cr_min = r_lod22.getColRowCoord(bb_min[0], bb_min[1]); + auto cr_max = r_lod22.getColRowCoord(bb_max[0], bb_max[1]); + auto part_points = r_lod22.rasterise_polygon(polygon, cr_min, cr_max, false); + + if(part_points.size()==0) { + part_points.insert(part_points.begin(), polygon.begin(), polygon.end()); + } + std::sort(part_points.begin(), part_points.end(), [](auto& p1, auto& p2) { + return p1[2] < p2[2]; + }); + + size_t datasize = part_points.size(); + int elevation_id = std::floor(0.5*float(datasize-1)); + h_attr.sub_terminal("h_50p").push_back(part_points[elevation_id][2] + z_offset); + elevation_id = std::floor(0.7*float(datasize-1)); + h_attr.sub_terminal("h_70p").push_back(part_points[elevation_id][2] + z_offset); + h_attr.sub_terminal("h_min").push_back(part_points[0][2] + z_offset); + h_attr.sub_terminal("h_max").push_back(part_points[datasize-1][2] + z_offset); + } + } + + typedef CGAL::Simple_cartesian CF; + CF::Vector_3 calculate_normal_cf(const LinearRing& ring) + { + float x=0, y=0, z=0; + for (size_t i = 0; i < ring.size(); ++i) { + const auto &curr = ring[i]; + const auto &next = ring[(i + 1) % ring.size()]; + x += (curr[1] - next[1]) * (curr[2] + next[2]); + y += (curr[2] - next[2]) * (curr[0] + next[0]); + z += (curr[0] - next[0]) * (curr[1] + next[1]); + } + CF::Vector_3 n(x, y, z); + return n / CGAL::approximate_sqrt(n.squared_length()); + } + + static constexpr double pi = 3.14159265358979323846; + static const CF::Vector_3 up = CF::Vector_3(0,0,1); + + void RoofPartition3DBAGRasteriseNode::process(){ + auto& lod12_roofparts = input("lod12_roofparts");//.get(); + auto& lod13_roofparts = input("lod13_roofparts");//.get(); + auto& lod22_roofparts = input("lod22_roofparts");//.get(); + + auto& lod12_hattr = poly_output("lod12_hattr"); + auto& lod13_hattr = poly_output("lod13_hattr"); + auto& lod22_hattr = poly_output("lod22_hattr"); + + Box box; + for (size_t i=0; i< lod22_roofparts.size(); ++i) { + auto rpart = lod22_roofparts.get(i); + box.add(rpart.box()); + } + auto boxmin = box.min(); + auto boxmax = box.max(); + + RasterTools::Raster r_lod22 = RasterTools::Raster(cellsize, boxmin[0]-0.5, boxmax[0]+0.5, boxmin[1]-0.5, boxmax[1]+0.5); + r_lod22.prefill_arrays(RasterTools::MAX); + for (size_t i=0; i< lod22_roofparts.size(); ++i) { + auto ring = lod22_roofparts.get(i); + rasterise_ring(ring, r_lod22); + } + auto z_offset = (*manager.data_offset())[2]; + calculate_h_attr(lod12_roofparts, lod12_hattr, r_lod22, z_offset); + calculate_h_attr(lod13_roofparts, lod13_hattr, r_lod22, z_offset); + calculate_h_attr(lod22_roofparts, lod22_hattr, r_lod22, z_offset); + + // compute inclination and azimuth. Both in degrees. + lod22_hattr.add_vector("b3_hellingshoek", typeid(float)); + lod22_hattr.add_vector("b3_azimut", typeid(float)); + for (size_t i=0; i(i); + auto n = calculate_normal_cf(ring); + float azimuth, slope; + if (std::isnan(n.x()) || std::isnan(n.y()) || std::isnan(n.z())){ + azimuth = std::numeric_limits::quiet_NaN(); + slope = std::numeric_limits::quiet_NaN(); + } else { + slope = CGAL::to_double(CGAL::approximate_angle(n, up)); + + // calculate azimuth from arctan2 (https://en.cppreference.com/w/cpp/numeric/math/atan2) + // ie. subtract pi/2, multiply by -1 and then add 2 pi if result is negative (4th quadrant) + azimuth = -1 * ( std::atan2(n.y(), n.x()) - pi/2 ); + if (azimuth<0) { + azimuth = 2*pi + azimuth; + } + + // convert to degrees + azimuth = azimuth * (180/pi); + } + // push attributes + lod22_hattr.sub_terminal("b3_hellingshoek").push_back(slope); + lod22_hattr.sub_terminal("b3_azimut").push_back(azimuth); + } + } + void SegmentRasteriseNode::rasterise_input(gfSingleFeatureInputTerminal& input_triangles, RasterTools::Raster& r, size_t& data_pixel_cnt) { typedef CGAL::Exact_predicates_inexact_constructions_kernel K; for(size_t i=0; i< input_triangles.size(); ++i) { diff --git a/src/line_regulariser.cpp b/src/line_regulariser.cpp index 68093e0..29342f3 100644 --- a/src/line_regulariser.cpp +++ b/src/line_regulariser.cpp @@ -151,7 +151,12 @@ namespace linereg { lhs_hhandles.erase(i++); } else { // dereference 3 times! iterator->shared_ptr->heap_handle->heap_value Notice -> is not implemented on the heap_handle, so we must use * for the last dereference here - (***i).dist = (***i).clusters.first->distance((***i).clusters.second.get()); + try { + (***i).dist = (***i).clusters.first->distance((***i).clusters.second.get()); + } catch (CGAL::Uncertain_conversion_exception e) { + // this happes sometimes, assuming here that this means the distance was close to 0 + (***i).dist = 0; + } distances.update(*(*i)); ++i; } diff --git a/src/polygon_triangulate.cpp b/src/polygon_triangulate.cpp index 874828d..3b09ac4 100644 --- a/src/polygon_triangulate.cpp +++ b/src/polygon_triangulate.cpp @@ -13,6 +13,7 @@ // You should have received a copy of the GNU Affero General Public License // along with this program. If not, see . +#include #include #include @@ -20,6 +21,7 @@ #include #include +#include "point_edge.h" #include "polygon_util.hpp" #include "cdt_util.hpp" @@ -33,8 +35,7 @@ typedef CGAL::Plane_3 Plane_3; glm::vec3 calculate_normal(const LinearRing ring) { glm::vec3 normal(0, 0, 0); - for (size_t i = 0; i < ring.size(); ++i) - { + for (size_t i = 0; i < ring.size(); ++i) { const auto &curr = ring[i]; const auto &next = ring[(i + 1) % ring.size()]; normal[0] += (curr[1] - next[1]) * (curr[2] + next[2]); @@ -44,6 +45,17 @@ glm::vec3 calculate_normal(const LinearRing ring) return glm::normalize(normal); } +double calculate_volume(const TriangleCollection& triangle_collection) { + double sum = 0; + for(const auto& t : triangle_collection) { + auto a = Vector(t[0][0], t[0][1], t[0][2]); + auto b = Vector(t[1][0], t[1][1], t[1][2]); + auto c = Vector(t[2][0], t[2][1], t[2][2]); + sum += CGAL::scalar_product(a, CGAL::cross_product(b, c)); + } + return sum/6; +} + // void mark_domains(CDT& cdt) { // std::list explorables; @@ -99,9 +111,14 @@ void PolygonTriangulatorNode::triangulate_polygon(LinearRing& poly, vec3f& norma float dupe_threshold = (float) std::pow(10,-dupe_threshold_exp); if (is_degenerate(poly, dupe_threshold)) { - std::cout << "skipping ring with duplicates\n"; - // dupe_rings.push_back(poly); - return; + LinearRing new_poly = fix_duplicates(poly, dupe_threshold); + if(is_degenerate(new_poly, dupe_threshold)) { + std::cout << "skipping ring with duplicates\n"; + // dupe_rings.push_back(poly); + return; + } + std::cout << "fixed ring with duplicates\n"; + poly = new_poly; } auto normal = calculate_normal(poly); if (std::isnan(normal.x) || std::isnan(normal.y) || std::isnan(normal.z)){ @@ -175,6 +192,7 @@ void PolygonTriangulatorNode::process() vec3f normals; // vec1f values_out; vec1i ring_ids; + auto& volumes = output("volumes"); // vec1i nesting_levels; // SegmentCollection edges; // vec1i edges_constr; @@ -188,12 +206,15 @@ void PolygonTriangulatorNode::process() triangulate_polygon(poly_3d, normals, tc, ri, ring_ids); triangles.insert(triangles.end(), tc.begin(), tc.end()); } + volumes.push_back((float)calculate_volume(triangles)); multitrianglecols.push_back(triangles); + output("multi_triangle_collections").set(multitrianglecols); } else if (rings.is_connected_type(typeid(std::unordered_map))) { // We are processing a building part here. We get a building part when we // cut a footprint into parts because of cutting off the underground part. for (size_t mi = 0; mi < rings.size(); ++mi) { auto meshmap = rings.get>(mi); + double volume_sum = 0; for(auto& [sid, mesh] : meshmap) { TriangleCollection mesh_triangles; AttributeMap mesh_attributes; @@ -211,31 +232,48 @@ void PolygonTriangulatorNode::process() multitrianglecols.push_back(mesh_triangles); multitrianglecols.push_back(mesh_attributes); multitrianglecols.building_part_ids_.push_back(sid); + volume_sum += calculate_volume(mesh_triangles); } + volumes.push_back((float)volume_sum); } + output("multi_triangle_collections").set(multitrianglecols); } else if (rings.is_connected_type(typeid(Mesh))) { - auto mesh = rings.get(); - TriangleCollection mesh_triangles; - AttributeMap mesh_attributes; - std::vector tri_labels; - for (size_t ri = 0; ri(mi); + TriangleCollection mesh_triangles; + AttributeMap mesh_attributes; + std::vector tri_labels; + for (size_t ri = 0; ri. #pragma once +#include #include #include "arrangement.hpp" @@ -541,7 +542,6 @@ namespace geoflow::nodes::stepedge { // }; class DetectPlanesNode:public Node { - bool only_horizontal = true; float horiz_min_count = 0.95; int metrics_normal_k = 5; int metrics_plane_k = 15; @@ -549,9 +549,21 @@ namespace geoflow::nodes::stepedge { float metrics_plane_epsilon = 0.2; float metrics_plane_normal_threshold = 0.75; float metrics_is_horizontal_threshold = 0.97; + float metrics_probability_ransac = 0.05; + float metrics_cluster_epsilon_ransac = 0.3; float metrics_is_wall_threshold = 0.3; int n_refit = 5; + bool use_ransac = false; // float roof_percentile=0.5; + + // plane regularisation + float maximum_angle_ = 25; + float maximum_offset_ = 0.5; + bool regularize_parallelism_ = false; + bool regularize_orthogonality_ = false; + bool regularize_coplanarity_ = false; + bool regularize_axis_symmetry_ = false; + public: using Node::Node; void init() override { @@ -559,7 +571,6 @@ namespace geoflow::nodes::stepedge { add_output("plane_id", typeid(vec1i)); add_output("is_wall", typeid(vec1i)); add_output("is_horizontal", typeid(vec1i)); - add_output("planes", typeid(Plane)); add_output("pts_per_roofplane", typeid(IndexedPlanesWithPoints )); @@ -576,16 +587,25 @@ namespace geoflow::nodes::stepedge { add_output("total_roofplane_cnt", typeid(int)); add_output("plane_adj", typeid(std::map>)); - add_param(ParamBool(only_horizontal, "only_horizontal", "Output only horizontal planes")); + add_param(ParamBool(use_ransac, "use_ransac", "Use ransac instead of region growing plane detection")); add_param(ParamFloat(horiz_min_count, "horiz_min_count", "Mininmal point count for horizontal planes")); add_param(ParamInt(metrics_normal_k, "metrics_normal_k", "Number of neighbours used for normal estimation")); add_param(ParamInt(metrics_plane_k, "metrics_plane_k", "Number of neighbours used during region growing plane detection")); add_param(ParamInt(metrics_plane_min_points, "metrics_plane_min_points", "Minimum number of points in a plane")); add_param(ParamFloat(metrics_plane_epsilon, "metrics_plane_epsilon", "Plane epsilon")); + add_param(ParamFloat(metrics_cluster_epsilon_ransac, "metrics_cluster_epsilon_ransac", "Cluster epsilon RANSAC only")); + add_param(ParamFloat(metrics_probability_ransac, "metrics_probability_ransac", "Probability RANSAC only")); add_param(ParamFloat(metrics_plane_normal_threshold, "metrics_plane_normal_threshold", "Plane normal angle threshold")); add_param(ParamFloat(metrics_is_horizontal_threshold, "metrics_is_horizontal_threshold", "Threshold for horizontal plane detection (expressed as angle wrt unit verctor in +z direction)")); add_param(ParamFloat(metrics_is_wall_threshold, "metrics_is_wall_threshold", "Wall angle thres")); add_param(ParamInt(n_refit, "n_refit", "Refit every n points")); + + add_param(ParamBool(regularize_parallelism_, "regularize_parallelism", "regularizeparallelism")); + add_param(ParamBool(regularize_orthogonality_, "regularize_orthogonality", "regularizeorthogonality")); + add_param(ParamBool(regularize_coplanarity_, "regularize_coplanarity", "regularizecoplanarity")); + add_param(ParamBool(regularize_axis_symmetry_, "regularize_axis_symmetry", "regularize_axis_symmetry")); + add_param(ParamFloat(maximum_angle_, "maximum_angle", "Plane regularisation: maximum allowed angle in degrees between plane normals used for parallelism, orthogonality, and axis symmetry")); + add_param(ParamFloat(maximum_offset_, "maximum_offset", "Plane regularisation: maximum allowed orthogonal distance between two parallel planes such that they are considered to be coplanar")); // add_param(ParamBoundedFloat(roof_percentile, 0, 1, "roof_percentile", "Roof elevation percentile")); } // void before_gui(){ @@ -920,6 +940,26 @@ namespace geoflow::nodes::stepedge { }; + class RoofPartition3DBAGRasteriseNode:public Node { + float cellsize = 0.5; + + public: + using Node::Node; + void init() override { + add_input("lod12_roofparts", typeid(geoflow::LinearRing)); + add_input("lod13_roofparts", typeid(geoflow::LinearRing)); + add_input("lod22_roofparts", typeid(geoflow::LinearRing)); + + // add_output("values", typeid(vec1f)); + add_poly_output("lod12_hattr", {typeid(float)}); + add_poly_output("lod13_hattr", {typeid(float)}); + add_poly_output("lod22_hattr", {typeid(float)}); + + add_param(ParamBoundedFloat(cellsize, 0, 50, "cellsize", "cellsize")); + } + void process() override; + }; + class BuildingRasteriseNode:public Node { float cellsize = 0.5; bool use_tin = false; @@ -1072,6 +1112,7 @@ namespace geoflow::nodes::stepedge { class PolygonTriangulatorNode:public Node { int dupe_threshold_exp = 6; bool output_all_triangles = false; + bool output_mtc_for_every_input = false; void triangulate_polygon(LinearRing& ring, vec3f& normals, TriangleCollection& triangles, size_t& ring_id, vec1i& ring_ids); public: @@ -1082,6 +1123,7 @@ namespace geoflow::nodes::stepedge { add_output("triangles", typeid(TriangleCollection)); add_output("multi_triangle_collections", typeid(MultiTriangleCollection)); add_output("normals", typeid(vec3f)); + add_output("volumes", typeid(float)); add_output("ring_ids", typeid(vec1i)); // add_output("nesting_levels", typeid(vec1i)); @@ -1089,6 +1131,7 @@ namespace geoflow::nodes::stepedge { // add_output("edges_constr", typeid(vec1i)); add_param(ParamBool(output_all_triangles, "output_all_triangles", "Also output triangles in holes and outside convex hull.")); + add_param(ParamBool(output_mtc_for_every_input, "output_mtc_for_every_input", "Output a MultiTriangleCollection for every input. Otherwise aggregate all inputs into one MultiTriangleCollection. Only applies to Mesh type inputs.")); add_param(ParamInt(dupe_threshold_exp, "dupe_threshold_exp", "Dupe tolerance exponent")); } diff --git a/src/tinsimp.cpp b/src/tinsimp.cpp new file mode 100644 index 0000000..de10aaa --- /dev/null +++ b/src/tinsimp.cpp @@ -0,0 +1,227 @@ +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "tinsimp.hpp" + +namespace tinsimp { + +void mark_domains(CDT& ct, + CDT::Face_handle start, + int index, + std::list& border) { + if (start->info().nesting_level != -1) { + return; + } + std::list queue; + queue.push_back(start); + while (!queue.empty()) { + CDT::Face_handle fh = queue.front(); + queue.pop_front(); + if (fh->info().nesting_level == -1) { + fh->info().nesting_level = index; + for (int i = 0; i < 3; i++) { + CDT::Edge e(fh, i); + CDT::Face_handle n = fh->neighbor(i); + if (n->info().nesting_level == -1) { + if (ct.is_constrained(e)) border.push_back(e); + else queue.push_back(n); + } + } + } + } +} +/** +* mark the triangles that are inside the original 2D polygon. +* explore set of facets connected with non constrained edges, +* and attribute to each such set a nesting level. +* start from facets incident to the infinite vertex, with a nesting +* level of 0. Then recursively consider the non-explored facets incident +* to constrained edges bounding the former set and increase the nesting level by 1. +* facets in the domain are those with an odd nesting level. +*/ +void mark_domains(CDT& cdt) { + // for (CDT::All_faces_iterator it = cdt.all_faces_begin(); it != cdt.all_faces_end(); ++it) { + // it->info().nesting_level = -1; + // } + std::list border; + mark_domains(cdt, cdt.infinite_face(), 0, border); + while (!border.empty()) { + CDT::Edge e = border.front(); + border.pop_front(); + CDT::Face_handle n = e.first->neighbor(e.second); + if (n->info().nesting_level == -1) { + mark_domains(cdt, n, e.first->info().nesting_level + 1, border); + } + } +} + +struct PointXYHash { + std::size_t operator()(Point const& p) const noexcept { + std::size_t h1 = std::hash{}(p.x()); + std::size_t h2 = std::hash{}(p.y()); + return h1 ^ (h2 << 1); + } +}; +struct PointXYEqual { + bool operator()(Point const& p1, Point const& p2) const noexcept { + auto ex = p1.x() == p2.x(); + auto ey = p1.y() == p2.y(); + return ex && ey; + } +}; + +inline double compute_error(Point &p, CDT::Face_handle &face); + +//--- TIN Simplification +// Greedy insertion/incremental refinement algorithm adapted from "Fast polygonal approximation of terrain and height fields" by Garland, Michael and Heckbert, Paul S. +inline double compute_error(Point &p, CDT::Face_handle &face) { + if(!face->info().plane) + face->info().plane = new CGAL::Plane_3( + face->vertex(0)->point(), + face->vertex(1)->point(), + face->vertex(2)->point()); + if(!face->info().points_inside) + face->info().points_inside = new std::vector(); + + auto plane = face->info().plane; + auto interpolate = -plane->a()/plane->c() * p.x() - plane->b()/plane->c()*p.y() - plane->d()/plane->c(); + double error = std::fabs(interpolate - p.z()); + return error; +} + +// void greedy_insert(CDT &T, const std::vector> &pts, double threshold) { +// // Convert all elevation points to CGAL points +// std::vector cpts; +// cpts.reserve(pts.size()); +// for (auto& p : pts) { +// cpts.push_back(Point(p[0], p[1], p[2])); +// } + +// greedy_insert(T, cpts, threshold); +// } + +void greedy_insert(CDT &T, std::vector &cpts, double threshold, size_t minpts) { + // assumes all lidar points are inside a triangle + Heap heap; + + // compute initial point errors, build heap, store point indices in triangles + { + std::unordered_set set; + for (int i = 0; i < cpts.size(); i++) { + auto p = cpts[i]; + CDT::Locate_type lt; + int li; + auto not_duplicate = set.insert(p).second; + if(not_duplicate){ + CDT::Face_handle face = T.locate(p, lt, li); + if (lt == CDT::EDGE || lt == CDT::FACE) { + auto e = compute_error(p, face); + auto handle = heap.push(point_error(i, e)); + face->info().points_inside->push_back(handle); + } + } + } + } + std::cout << "prepared tinsimp...\n"; + + // insert points, update errors of affected triangles until threshold error is reached + size_t insert_cnt = 0; + // while ( (!heap.empty() && heap.top().error > threshold) || insert_cnt < minpts ) { + while (!heap.empty() && heap.top().error > threshold) { + // get top element (with largest error) from heap + auto maxelement = heap.top(); + auto max_p = cpts[maxelement.index]; + + // get triangles that will change after inserting this max_p + std::vector faces; + T.get_conflicts(max_p, std::back_inserter(faces)); + + // insert max_p in triangulation + auto face_hint = faces[0]; + auto v = T.insert(max_p, face_hint); + face_hint = v->face(); + ++insert_cnt; + + // update clear info of triangles that just changed, collect points that were inside these triangles + std::vector points_to_update; + for (auto face : faces) { + if (face->info().plane) { + delete face->info().plane; + face->info().plane = nullptr; + } + if (face->info().points_inside) { + for (auto h : *face->info().points_inside) { + if (maxelement.index != (*h).index) + points_to_update.push_back(h); + } + std::vector().swap((*face->info().points_inside)); + } + } + + // remove the point we just inserted in the triangulation from the heap + heap.pop(); + + // update the errors of affected elevation points + for (auto curelement : points_to_update) { + auto p = cpts[(*curelement).index]; + //auto containing_face = T.locate(p, face_hint); + CDT::Locate_type lt; + int li; + CDT::Face_handle containing_face = T.locate(p, lt, li, face_hint); + if (lt == CDT::EDGE || lt == CDT::FACE) { + const double e = compute_error(p, containing_face); + const point_error new_pe = point_error((*curelement).index, e); + heap.update(curelement, new_pe); + containing_face->info().points_inside->push_back(curelement); + } + } + } + + // insert more points so we can guarantee the minpts number of interior points + if (heap.size() < minpts) minpts = heap.size(); + if(insert_cnt < minpts) { + std::list> remaining_pts; + for (auto hit = heap.ordered_begin(); hit != heap.ordered_end(); ++hit) { + auto& p = cpts[(*hit).index]; + remaining_pts.push_back(p); + } + + auto rand = CGAL::Random(13374269); + while (insert_cnt < minpts) { + auto it = remaining_pts.begin(); + auto r = rand.get_int(0, remaining_pts.size()); + while (r > 0) { + ++it; + --r; + } + T.insert(*it); + ++insert_cnt; + remaining_pts.erase(it); + } + } + + //cleanup the stuff I put in face info of triangles + for (CDT::Finite_faces_iterator fit = T.finite_faces_begin(); + fit != T.finite_faces_end(); ++fit) { + if (fit->info().plane) { + delete fit->info().plane; + fit->info().plane = nullptr; + } + if (fit->info().points_inside) { + delete fit->info().points_inside; + fit->info().points_inside = nullptr; + } + } + +} + +} \ No newline at end of file diff --git a/src/tinsimp.hpp b/src/tinsimp.hpp new file mode 100644 index 0000000..cbb5e93 --- /dev/null +++ b/src/tinsimp.hpp @@ -0,0 +1,55 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace tinsimp { + +// fibonacci heap for greedy insertion code +struct point_error { + point_error(size_t i, double e) : index(i), error(e){} + size_t index; + double error; + size_t line_id; + + bool operator<(point_error const & rhs) const + { + return error < rhs.error; + } +}; +typedef boost::heap::fibonacci_heap Heap; +typedef Heap::handle_type heap_handle; + +typedef CGAL::Exact_predicates_inexact_constructions_kernel K; +typedef CGAL::Projection_traits_xy_3 Gt; +typedef CGAL::Triangulation_vertex_base_with_id_2 Vb; +struct FaceInfo2 +{ + FaceInfo2() {} + std::vector* points_inside = nullptr; + CGAL::Plane_3* plane = nullptr; + int nesting_level = -1; + bool in_domain() { + return nesting_level % 2 == 1; + } +}; +typedef CGAL::Triangulation_face_base_with_info_2 Fbb; +typedef CGAL::Constrained_triangulation_face_base_2 Fb; +typedef CGAL::Triangulation_data_structure_2 Tds; +typedef CGAL::Exact_predicates_tag Itag; +typedef CGAL::Constrained_Delaunay_triangulation_2 CDT; +typedef CGAL::Constrained_triangulation_plus_2 CT; +typedef CDT::Point Point; + +// void greedy_insert(CDT &T, const std::vector> &pts, double threshold); +void greedy_insert(CDT &T, std::vector &cpts, double threshold, size_t minpts=0); +void mark_domains(CDT& cdt); +} \ No newline at end of file diff --git a/thirdparty/region-grower b/thirdparty/region-grower index 3f824f2..9c12bf2 160000 --- a/thirdparty/region-grower +++ b/thirdparty/region-grower @@ -1 +1 @@ -Subproject commit 3f824f2aaa63317dab5470bbe39b98a723f99149 +Subproject commit 9c12bf2bfdfe81eca4f807c305ae5059da941229