From 3771fa1d169f792052f8b563e77b5e5309bd7bbd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gr=C3=A9goire=20Grzeczkowicz?= Date: Tue, 27 Dec 2022 11:08:25 +0000 Subject: [PATCH] Refacto code --- CMakeLists.txt | 2 +- code.cpp | 1341 +++++++++++++++++++++++++++++++++ header.hpp | 95 +++ label.cpp => label.hpp | 7 +- main.cpp | 1626 +--------------------------------------- raster.cpp | 166 ++++ raster.hpp | 56 ++ 7 files changed, 1675 insertions(+), 1618 deletions(-) create mode 100644 code.cpp create mode 100644 header.hpp rename label.cpp => label.hpp (89%) create mode 100644 raster.cpp create mode 100644 raster.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 6c776f1..f6b4a9a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -50,7 +50,7 @@ endif() # Creating entries for target: compute-LOD2 # ############################ -add_executable( compute-LOD2 main.cpp ) +add_executable( compute-LOD2 main.cpp raster.cpp code.cpp) add_to_cached_list( CGAL_EXECUTABLE_TARGETS compute-LOD2 ) diff --git a/code.cpp b/code.cpp new file mode 100644 index 0000000..45877c7 --- /dev/null +++ b/code.cpp @@ -0,0 +1,1341 @@ +#include "header.hpp" +#include "raster.hpp" + +std::list get_LOD0_from_shapefile(char *path) { + GDALAllRegister(); + + GDALDataset *dataset; + + std::list polygons; + + dataset = (GDALDataset*) GDALOpenEx(path, GDAL_OF_VECTOR, NULL, NULL, NULL ); + if( dataset == NULL ) { + std::cerr << "Unable to open " << path << "." << std::endl; + return polygons; + } + + for( OGRLayer *layer: dataset->GetLayers() ) { + for( const auto& feature: *layer ) { + if ((*feature)["num_class"].GetInteger() == 4) { + // Get only the buildings + OGRGeometry *geometry = feature->GetGeometryRef(); + + if (wkbFlatten(geometry->getGeometryType()) == wkbPolygon) { + + OGRPolygon *shp_polygon = geometry->toPolygon(); + Polygon cgal_polygon; + std::istringstream wkt(shp_polygon->exportToWkt()); + CGAL::IO::read_polygon_WKT (wkt, cgal_polygon); + polygons.push_back(cgal_polygon); + + } else if (wkbFlatten(geometry->getGeometryType()) == wkbMultiPolygon) { + + OGRMultiPolygon *shp_multi_polygon = geometry->toMultiPolygon(); + for (OGRPolygon *shp_polygon: *shp_multi_polygon) { + Polygon cgal_polygon; + std::istringstream wkt(shp_polygon->exportToWkt()); + CGAL::IO::read_polygon_WKT (wkt, cgal_polygon); + polygons.push_back(cgal_polygon); + } + + } + } + } + } + + return polygons; +} + +float single_face_cost(const Raster &raster, const Point_3 &p0, const Point_3 &p1, const Point_3 &p2) { + float nz = ((-p0.x() + p1.x()) * (-p0.y() + p2.y()) - (-p0.x() + p2.x()) * (-p0.y() + p1.y())); + if (nz == 0) { + // flat triangle + return 0; + } + + std::list> pixels = raster.triangle_to_pixel(p0, p1, p2); + + // Entropy + int face_label[LABELS.size()] = {0}; + int sum_face_label = 0; + for (auto pixel : pixels) { + if (raster.land_cover[pixel.second][pixel.first] > -1) { + sum_face_label++; + face_label[raster.land_cover[pixel.second][pixel.first]]++; + } + } + float entropy = 0; + if (sum_face_label > 0) { + for (int i = 0; i < LABELS.size(); i++) { + if (face_label[i] > 0) { + entropy += ((float) face_label[i])*log((float) face_label[i]); + } + } + entropy = log((float) sum_face_label) - entropy/((float) sum_face_label); + } + + // Least squares + float least_squares = 0; + float least = 0; + if (pixels.size() != 0) { + K::Plane_3 plane(p0, p1, p2); + for (auto pixel : pixels) { + float px = 0.5 + pixel.first; + float py = 0.5 + pixel.second; + float pz = - (plane.a() * px + plane.b() * py + plane.d()) / plane.c(); + least_squares += pow(raster.dsm[pixel.second][pixel.first] - pz,2); + least += abs(raster.dsm[pixel.second][pixel.first] - pz); + } + least_squares /= pixels.size(); + least /= pixels.size(); + } + + // Verticality + float verticality = 0; + if (pixels.size() == 0) { + float surface = pow(K::Triangle_3(p0, p1, p2).squared_area (), 0.5); + verticality = abs(nz)/(2*surface); + } + + // Eccentricity + Point_3 S = CGAL::centroid(p0,p1,p2); + float M = (pow(p2.x()-S.x(),2) + pow(p2.y()-S.y(),2) + pow(p2.z()-S.z(),2) + (pow(p1.x()-p0.x(),2) + pow(p1.y()-p0.y(),2) + pow(p1.z()-p0.z(),2))/3)/4; + float N = sqrtf(pow((p2.y()-S.y())*(p1.z()-p0.z())-(p2.z()-S.z())*(p1.y()-p0.y()),2) + pow((p2.z()-S.z())*(p1.x()-p0.x())-(p2.x()-S.x())*(p1.z()-p0.z()),2) + pow((p2.x()-S.x())*(p1.y()-p0.y())-(p2.y()-S.y())*(p1.x()-p0.x()),2))/(4 * sqrtf(3)); + float eccentricity = M*M - 4*N*N; + eccentricity = sqrtf(1-(M-eccentricity)/(M+eccentricity)); + + return 0 * least_squares + 1 * least + 1 * entropy + 0 * verticality + 0 * eccentricity; +} + +float face_cost(const Raster &raster, const Point_3 &p0, const Point_3 &p1, const Point_3 &p2) { + float surface = pow(K::Triangle_3(p0, p1, p2).squared_area (), 0.5); + return surface * (1 + single_face_cost(raster, p0, p1, p2)); +} + +Point_3 best_point(const Raster &raster, K::FT x, K::FT y, const SMS::Edge_profile& profile) { + + float z; + float d = 0; + float D = 0; + std::list> values; + float t = 0; + int count = 0; + + //Foreach face + for (auto he : CGAL::halfedges_around_source(profile.v0(), profile.surface_mesh())) { + if (he != profile.v0_v1() && he != profile.v0_vR()) { + Point_3 A = get(profile.vertex_point_map(),CGAL::target(he, profile.surface_mesh())); + Point_3 B = get(profile.vertex_point_map(),CGAL::target(CGAL::next(he, profile.surface_mesh()), profile.surface_mesh())); + float i2 = ((-A.x() + B.x()) * (-A.y() + y) - (-A.x() + x) * (-A.y() + B.y())); + if (i2 != 0) { + for (auto pixel : raster.triangle_to_pixel(A, B, Point_3(x, y, 0))) { //for each pixel + float px = 0.5 + pixel.first; + float py = 0.5 + pixel.second; + float pz = raster.dsm[pixel.second][pixel.first]; + + float i1 = ((A.x() - B.x()) * (-A.y() + py) + (-A.x() + px) * (-A.y() + B.y())); + + d += i1 * (((A.x() - px) * (A.y() * B.z() - A.z() * B.y() + A.z() * y - B.z() * y) + (A.y() - py) * (-A.x() * B.z() + A.z() * B.x() - A.z() * x + B.z() * x) + (A.z() - pz) * i2) / (i2 * i2)); + D += (i1 * i1) / (i2 * i2); + values.push_back(std::pair(((A.x() - px) * (A.y() * B.z() - A.z() * B.y() + A.z() * y - B.z() * y) + (A.y() - py) * (-A.x() * B.z() + A.z() * B.x() - A.z() * x + B.z() * x) + (A.z() - pz) * i2) / i1, abs(i1/i2))); + t += abs(i1/i2); + count ++; + } + } + } + } + for (auto he : CGAL::halfedges_around_source(profile.v1(), profile.surface_mesh())) { + if (he != profile.v1_v0() && he != profile.v1_vL()) { + Point_3 A = get(profile.vertex_point_map(),CGAL::target(he, profile.surface_mesh())); + Point_3 B = get(profile.vertex_point_map(),CGAL::target(CGAL::next(he, profile.surface_mesh()), profile.surface_mesh())); + float i2 = ((-A.x() + B.x()) * (-A.y() + y) - (-A.x() + x) * (-A.y() + B.y())); + if (i2 != 0) { + for (auto pixel : raster.triangle_to_pixel(A, B, Point_3(x, y, 0))) { //for each pixel + float px = 0.5 + pixel.first; + float py = 0.5 + pixel.second; + float pz = raster.dsm[pixel.second][pixel.first]; + + float i1 = ((A.x() - B.x()) * (-A.y() + py) + (-A.x() + px) * (-A.y() + B.y())); + + d += i1 * (((A.x() - px) * (A.y() * B.z() - A.z() * B.y() + A.z() * y - B.z() * y) + (A.y() - py) * (-A.x() * B.z() + A.z() * B.x() - A.z() * x + B.z() * x) + (A.z() - pz) * i2) / (i2 * i2)); + D += (i1 * i1) / (i2 * i2); + values.push_back(std::pair(((A.x() - px) * (A.y() * B.z() - A.z() * B.y() + A.z() * y - B.z() * y) + (A.y() - py) * (-A.x() * B.z() + A.z() * B.x() - A.z() * x + B.z() * x) + (A.z() - pz) * i2) / i1, abs(i1/i2))); + t += abs(i1/i2); + count ++; + } + } + } + } + + if (count > 0) { + z = d / D; + + values.sort([](std::pair a, std::pair b) { + return a.first > b.first; + }); + + auto p = values.begin(); + float s = 0; + while (s+p->second < t/2) { + s += p->second; + p++; + } + z = p->first; + + } else { + if (profile.p1().x() != profile.p0().x()) { + z = (x - profile.p0().x())/(profile.p1().x() - profile.p0().x())*(profile.p1().z() - profile.p0().z()); + } else { + z = (y - profile.p0().y())/(profile.p1().y() - profile.p0().y())*(profile.p1().z() - profile.p0().z()); + } + } + + return Point_3(x,y,z); +} + +class Custom_placement { + const Raster &raster; + + public: + Custom_placement (const Raster &raster) : raster(raster) {} + + boost::optional::Point> operator()(const SMS::Edge_profile& profile) const { + typedef boost::optional::Point> result_type; + + K::Vector_3 vector(profile.p0(), profile.p1()); + Point_3 p[5] = {profile.p0(), profile.p0() + 0.25 * vector, profile.p0() + 0.5 * vector, profile.p0() + 0.75 * vector, profile.p1()}; + float cost[5] = {0}; + + /*for (int j = 0; j < 5; j++) { + p[j] = best_point(raster, p[j].x(), p[j].y(), profile); + }*/ + + for (auto he : CGAL::halfedges_around_source(profile.v0(), profile.surface_mesh())) { + if (he != profile.v0_v1() && he != profile.v0_vR()) { + Point_3 A = get(profile.vertex_point_map(),CGAL::target(he, profile.surface_mesh())); + Point_3 B = get(profile.vertex_point_map(),CGAL::target(CGAL::next(he, profile.surface_mesh()), profile.surface_mesh())); + for (int j = 0; j < 5; j++) { + cost[j] += face_cost(raster, A, B, p[j]); + } + } + } + for (auto he : CGAL::halfedges_around_source(profile.v1(), profile.surface_mesh())) { + if (he != profile.v1_v0() && he != profile.v1_vL()) { + Point_3 A = get(profile.vertex_point_map(),CGAL::target(he, profile.surface_mesh())); + Point_3 B = get(profile.vertex_point_map(),CGAL::target(CGAL::next(he, profile.surface_mesh()), profile.surface_mesh())); + for (int j = 0; j < 5; j++) { + cost[j] += face_cost(raster, A, B, p[j]); + } + } + } + + for (int i = 0; i < 2; i++) { + int min_cost = std::min_element(cost, cost + 5) - cost; + + if (min_cost == 0 || min_cost == 1) { + p[4] = p[2]; + cost[4] = cost[2]; + p[2] = p[1]; + cost[2] = cost[1]; + } else if (min_cost == 2) { + p[0] = p[1]; + cost[0] = cost[1]; + p[4] = p[3]; + cost[4] = cost[3]; + } else { + p[0] = p[2]; + cost[0] = cost[2]; + p[2] = p[3]; + cost[2] = cost[3]; + } + + vector = K::Vector_3(p[0], p[4]); + p[1] = p[0] + 0.25 * vector; + p[3] = p[0] + 0.75 * vector; + //p[1] = best_point(raster, p[1].x(), p[1].y(), profile); + //p[3] = best_point(raster, p[1].x(), p[1].y(), profile); + + cost[1] = 0; + cost[3] = 0; + + for (auto he : CGAL::halfedges_around_source(profile.v0(), profile.surface_mesh())) { + if (he != profile.v0_v1() && he != profile.v0_vR()) { + Point_3 A = get(profile.vertex_point_map(),CGAL::target(he, profile.surface_mesh())); + Point_3 B = get(profile.vertex_point_map(),CGAL::target(CGAL::next(he, profile.surface_mesh()), profile.surface_mesh())); + cost[1] += face_cost(raster, A, B, p[1]); + cost[3] += face_cost(raster, A, B, p[3]); + } + } + for (auto he : CGAL::halfedges_around_source(profile.v1(), profile.surface_mesh())) { + if (he != profile.v1_v0() && he != profile.v1_vL()) { + Point_3 A = get(profile.vertex_point_map(),CGAL::target(he, profile.surface_mesh())); + Point_3 B = get(profile.vertex_point_map(),CGAL::target(CGAL::next(he, profile.surface_mesh()), profile.surface_mesh())); + cost[1] += face_cost(raster, A, B, p[1]); + cost[3] += face_cost(raster, A, B, p[3]); + } + } + } + + int min_cost = std::min_element(cost, cost + 5) - cost; + + //std::cout << "Placement: (" << profile.p0() << ") - (" << profile.p1() << ") -> (" << p[min_cost] << ")\n"; + Point_3 placement = p[min_cost]; + + for (auto he : CGAL::halfedges_around_source(profile.v0(), profile.surface_mesh())) { + if (he != profile.v0_v1() && he != profile.v0_vR()) { + Point_3 A = get(profile.vertex_point_map(),CGAL::target(he, profile.surface_mesh())); + Point_3 B = get(profile.vertex_point_map(),CGAL::target(CGAL::next(he, profile.surface_mesh()), profile.surface_mesh())); + if (CGAL::orientation(Point_2(A.x(),A.y()), Point_2(B.x(),B.y()), Point_2(profile.p0().x(),profile.p0().y())) != CGAL::orientation(Point_2(A.x(),A.y()), Point_2(B.x(),B.y()), Point_2(placement.x(),placement.y()))) { + return result_type(); + } + } + } + for (auto he : CGAL::halfedges_around_source(profile.v1(), profile.surface_mesh())) { + if (he != profile.v1_v0() && he != profile.v1_vL()) { + Point_3 A = get(profile.vertex_point_map(),CGAL::target(he, profile.surface_mesh())); + Point_3 B = get(profile.vertex_point_map(),CGAL::target(CGAL::next(he, profile.surface_mesh()), profile.surface_mesh())); + if (CGAL::orientation(Point_2(A.x(),A.y()), Point_2(B.x(),B.y()), Point_2(profile.p1().x(),profile.p1().y())) != CGAL::orientation(Point_2(A.x(),A.y()), Point_2(B.x(),B.y()), Point_2(placement.x(),placement.y()))) { + return result_type(); + } + } + } + + return result_type(placement); + } +}; + +class Custom_cost { + const Raster &raster; + + public: + Custom_cost (const Raster &raster) : raster(raster) {} + + boost::optional::FT> operator()(const SMS::Edge_profile& profile, const boost::optional::Point>& placement) const { + typedef boost::optional::FT> result_type; + + if (placement) { + + float old_cost = 0; + float new_cost = 0; + + SMS::Edge_profile::Triangle_vector triangles = profile.triangles(); + for (auto triange: triangles) { + Point_3 A = get(profile.vertex_point_map(),triange.v0); + Point_3 B = get(profile.vertex_point_map(),triange.v1); + Point_3 C = get(profile.vertex_point_map(),triange.v2); + old_cost += face_cost(raster, A, B, C); + } + + Point_3 C = *placement; + for (auto he : CGAL::halfedges_around_source(profile.v0(), profile.surface_mesh())) { + if (he != profile.v0_v1() && he != profile.v0_vR()) { + Point_3 A = get(profile.vertex_point_map(),CGAL::target(he, profile.surface_mesh())); + Point_3 B = get(profile.vertex_point_map(),CGAL::target(CGAL::next(he, profile.surface_mesh()), profile.surface_mesh())); + new_cost += face_cost(raster, A, B, C); + } + } + for (auto he : CGAL::halfedges_around_source(profile.v1(), profile.surface_mesh())) { + if (he != profile.v1_v0() && he != profile.v1_vL()) { + Point_3 A = get(profile.vertex_point_map(),CGAL::target(he, profile.surface_mesh())); + Point_3 B = get(profile.vertex_point_map(),CGAL::target(CGAL::next(he, profile.surface_mesh()), profile.surface_mesh())); + new_cost += face_cost(raster, A, B, C); + } + } + + //std::cout << "Cost: " << (new_cost - old_cost) << "\n"; + + return result_type(new_cost - old_cost); + } + + return result_type(); + } +}; + +class Cost_stop_predicate { + public: + + Cost_stop_predicate(const float cost) : cost(cost) {} + + bool operator()(const SMS::Edge_profile::FT & current_cost, const SMS::Edge_profile &, const SMS::Edge_profile::edges_size_type, const SMS::Edge_profile::edges_size_type) const { + return current_cost > cost; + } + + private: + const float cost; +}; + +void save_mesh(const Surface_mesh &mesh, const Raster &raster, const char *filename) { + typedef CGAL::Surface_mesh::Point_3> OutputMesh; + OutputMesh output_mesh; + + std::unordered_map::face_descriptor, boost::graph_traits::face_descriptor> f2f; + CGAL::copy_face_graph (mesh, output_mesh, CGAL::parameters::face_to_face_output_iterator(std::inserter(f2f, f2f.end()))); + + // Label + OutputMesh::Property_map output_label; + bool created; + boost::tie(output_label, created) = output_mesh.add_property_map("label",0); + assert(created); + + Surface_mesh::Property_map label; + bool has_label; + boost::tie(label, has_label) = mesh.property_map("label"); + if (has_label) { + for (auto face : mesh.faces()) { + output_label[f2f[face]] = label[face]; + } + } + + // Color + OutputMesh::Property_map red; + OutputMesh::Property_map green; + OutputMesh::Property_map blue; + boost::tie(red, created) = output_mesh.add_property_map("red",0); + assert(created); + boost::tie(green, created) = output_mesh.add_property_map("green",0); + assert(created); + boost::tie(blue, created) = output_mesh.add_property_map("blue",0); + assert(created); + + // Entropy + OutputMesh::Property_map quality; + boost::tie(quality, created) = output_mesh.add_property_map("quality",0); + assert(created); + + Surface_mesh::Property_map path; + bool has_path; + boost::tie(path, has_path) = mesh.property_map("path"); + if (has_path) { + for (auto face : mesh.faces()) { + quality[f2f[face]] = path[face]; + } + } + + for (auto face : output_mesh.faces()) { + if (!has_label) { + int face_label[LABELS.size()] = {0}; + + CGAL::Vertex_around_face_iterator vbegin, vend; + boost::tie(vbegin, vend) = vertices_around_face(output_mesh.halfedge(face), output_mesh); + for (auto pixel : raster.triangle_to_pixel(output_mesh.point(*(vbegin++)), output_mesh.point(*(vbegin++)), output_mesh.point(*(vbegin++)))) { + if (raster.land_cover[pixel.second][pixel.first] > -1) { + face_label[raster.land_cover[pixel.second][pixel.first]]++; + } + } + + auto argmax = std::max_element(face_label, face_label+LABELS.size()); + output_label[face] = argmax - face_label; + } + + red[face] = LABELS[output_label[face]].red; + green[face] = LABELS[output_label[face]].green; + blue[face] = LABELS[output_label[face]].blue; + + if (!has_path) { + CGAL::Vertex_around_face_iterator vbegin, vend; + boost::tie(vbegin, vend) = vertices_around_face(output_mesh.halfedge(face), output_mesh); + auto pa = output_mesh.point(*(vbegin++)); + auto pb = output_mesh.point(*(vbegin++)); + auto pc = output_mesh.point(*(vbegin++)); + quality[face] = single_face_cost(raster, Point_3(pa.x(), pa.y(), pa.z()), Point_3(pb.x(), pb.y(), pb.z()), Point_3(pc.x(), pc.y(), pc.z())); + } + } + + double min_x, min_y; + raster.grid_to_coord(0, 0, min_x, min_y); + + char *temp; + const char *options_json[] = { "MULTILINE=NO", NULL }; + raster.get_crs().exportToPROJJSON(&temp, options_json); + std::string crs_as_json(temp); + CPLFree(temp); + + crs_as_json = regex_replace(crs_as_json, std::regex(",\"id\":\\{\"authority\":\"EPSG\",\"code\":[^\\}]+\\}"), ""); + crs_as_json = regex_replace(crs_as_json, std::regex("Lambert-93"), "Custom"); + + std::smatch matches; + regex_search(crs_as_json, matches, std::regex("\"name\":\"Northing at false origin\",\"value\":([^,]+),")); + crs_as_json = regex_replace(crs_as_json, std::regex("\"name\":\"Northing at false origin\",\"value\":([^,]+),"), "\"name\":\"Northing at false origin\",\"value\":" + std::to_string(std::stoi(matches[1]) - min_y) + ","); + + regex_search(crs_as_json, matches, std::regex("\"name\":\"Easting at false origin\",\"value\":([^,]+),")); + crs_as_json = regex_replace(crs_as_json, std::regex("\"name\":\"Easting at false origin\",\"value\":([^,]+),"), "\"name\":\"Easting at false origin\",\"value\":" + std::to_string(std::stoi(matches[1]) - min_x) + ","); + + OGRSpatialReference output_crs; + output_crs.SetFromUserInput(crs_as_json.c_str()); + output_crs.AutoIdentifyEPSG(); + + /*const char *options_wkt[] = { "MULTILINE=NO", "FORMAT=WKT2", NULL }; + output_crs.exportToWkt(&temp, options_wkt);*/ output_crs.exportToProj4(&temp); // WKT format is too long for MeshLab + std::string crs_as_wkt(temp); + CPLFree(temp); + + for(auto vertex : output_mesh.vertices()) { + auto point = output_mesh.point(vertex); + double x, y; + raster.grid_to_coord((float) point.x(), (float) point.y(), x, y); + output_mesh.point(vertex) = CGAL::Simple_cartesian::Point_3(x-min_x, y-min_y, (double) point.z()); + } + + std::ofstream mesh_ofile (filename, std::ios_base::binary); + CGAL::IO::set_binary_mode (mesh_ofile); + CGAL::IO::write_PLY (mesh_ofile, output_mesh, "crs " + crs_as_wkt); + mesh_ofile.close(); +} + + +struct My_visitor : SMS::Edge_collapse_visitor_base { + private: + int i_collecte = 0; + const Surface_mesh *mesh; + const Raster *raster; + std::chrono::time_point start_collecte; + std::chrono::time_point start_collapse; + bool output[5] = {false}; + + public: + My_visitor(const Surface_mesh *mesh, const Raster *raster) : mesh(mesh), raster(raster) {} + + void OnStarted (Surface_mesh &mesh) { + start_collecte = std::chrono::system_clock::now(); + } + + void OnCollected(const SMS::Edge_profile& profile, const boost::optional< SMS::Edge_profile::FT >& cost) { + start_collapse = std::chrono::system_clock::now(); + i_collecte++; + if (i_collecte%1000 == 0) { + std::chrono::duration diff = start_collapse - start_collecte; + std::cout << "\rCollecte: " << i_collecte << "/" << mesh->number_of_edges() << " (" << ((int) (((float) i_collecte)/mesh->number_of_edges()*100)) << "%)" << " still " << (((float) mesh->number_of_edges() - i_collecte) * diff.count() / i_collecte) << "s" << " (" << (((float) i_collecte) / diff.count()) << " op/s)" << std::flush; + } + } + + void OnSelected (const SMS::Edge_profile &profile, boost::optional< SMS::Edge_profile::FT > cost, const SMS::Edge_profile::edges_size_type initial_edge_count, const SMS::Edge_profile::edges_size_type current_edge_count) { + if (current_edge_count%100 == 0) { + auto end = std::chrono::system_clock::now(); + std::chrono::duration diff = end - start_collapse; + std::cout << "\rCollapse: " << (initial_edge_count-current_edge_count) << "/" << initial_edge_count << " (" << ((int) (((float) (initial_edge_count-current_edge_count))/initial_edge_count*100)) << "%)" << " still " << (((float) current_edge_count) * diff.count() / (initial_edge_count-current_edge_count)) << "s" << " (" << (((float) (initial_edge_count-current_edge_count)) / diff.count()) << " op/s)"; + if (cost) { + std::cout << " - cost: " << *cost << " " << std::flush; + } + } + + if (cost) { + if(*cost > 1e-4 && !output[0]) { + output[0] = true; + save_mesh(*mesh,*raster,"mesh-1e-4.ply"); + } else if(*cost > 0 && !output[1]) { + output[1] = true; + save_mesh(*mesh,*raster,"mesh-0.ply"); + } + if(current_edge_count <= 100000 && !output[2]) { + output[2] = true; + save_mesh(*mesh,*raster,"mesh-100000.ply"); + } else if(current_edge_count <= 10000 && !output[3]) { + output[3] = true; + save_mesh(*mesh,*raster,"mesh-10000.ply"); + } else if(current_edge_count <= 5000 && !output[4]) { + output[4] = true; + save_mesh(*mesh,*raster,"mesh-5000.ply"); + } else if(current_edge_count <= 1000000 && !output[5]) { + output[5] = true; + save_mesh(*mesh,*raster,"mesh-1000000.ply"); + } + } + + } + +}; + +std::tuple compute_meshes(const Raster &raster) { + + std::cout << "Terrain mesh" << std::endl; + Surface_mesh terrain_mesh; + // Add points + std::vector> terrain_vertex_index(raster.ySize, std::vector(raster.xSize, Surface_mesh::Vertex_index())); + for (int L = 0; L < raster.ySize; L++) { + for (int P = 0; P < raster.xSize; P++) { + terrain_vertex_index[L][P] = terrain_mesh.add_vertex(Point_3(0.5 + P, 0.5 + L, raster.dtm[L][P])); + } + } + std::cout << "Point added" << std::endl; + // Add faces + for (int L = 0; L < raster.ySize-1; L++) { + for (int P = 0; P < raster.xSize-1; P++) { + if (pow(raster.dtm[L][P]-raster.dtm[L+1][P+1], 2) < pow(raster.dtm[L+1][P]-raster.dtm[L][P+1], 2)) { + terrain_mesh.add_face(terrain_vertex_index[L][P], terrain_vertex_index[L+1][P+1], terrain_vertex_index[L+1][P]); + terrain_mesh.add_face(terrain_vertex_index[L][P], terrain_vertex_index[L][P+1], terrain_vertex_index[L+1][P+1]); + } else { + terrain_mesh.add_face(terrain_vertex_index[L][P], terrain_vertex_index[L][P+1], terrain_vertex_index[L+1][P]); + terrain_mesh.add_face(terrain_vertex_index[L][P+1], terrain_vertex_index[L+1][P+1], terrain_vertex_index[L+1][P]); + } + } + } + std::cout << "Faces added" << std::endl; + + save_mesh(terrain_mesh, raster, "initial-terrain-mesh.ply"); + + SMS::edge_collapse(terrain_mesh, Cost_stop_predicate(10)); + std::cout << "Terrain mesh simplified" << std::endl; + + save_mesh(terrain_mesh, raster, "terrain-mesh.ply"); + + std::cout << "Surface mesh" << std::endl; + Surface_mesh mesh; + // Add points + std::vector> vertex_index(raster.ySize, std::vector(raster.xSize, Surface_mesh::Vertex_index())); + for (int L = 0; L < raster.ySize; L++) { + for (int P = 0; P < raster.xSize; P++) { + vertex_index[L][P] = mesh.add_vertex(Point_3(0.5 + P, 0.5 + L, raster.dsm[L][P])); + } + } + std::cout << "Point added" << std::endl; + // Add faces + for (int L = 0; L < raster.ySize-1; L++) { + for (int P = 0; P < raster.xSize-1; P++) { + if (pow(raster.dsm[L][P]-raster.dsm[L+1][P+1], 2) < pow(raster.dsm[L+1][P]-raster.dsm[L][P+1], 2)) { + mesh.add_face(vertex_index[L][P], vertex_index[L+1][P+1], vertex_index[L+1][P]); + mesh.add_face(vertex_index[L][P], vertex_index[L][P+1], vertex_index[L+1][P+1]); + } else { + mesh.add_face(vertex_index[L][P], vertex_index[L][P+1], vertex_index[L+1][P]); + mesh.add_face(vertex_index[L][P+1], vertex_index[L+1][P+1], vertex_index[L+1][P]); + } + } + } + std::cout << "Faces added" << std::endl; + + save_mesh(mesh, raster, "initial-mesh.ply"); + + Cost_stop_predicate stop(10); + //SMS::Count_stop_predicate stop(1000); + Custom_cost cf(raster); + Custom_placement pf(raster); + int r = SMS::edge_collapse(mesh, stop, CGAL::parameters::get_cost(cf).get_placement(pf).visitor(My_visitor(&mesh, &raster))); + std::cout << "\rMesh simplified " << std::endl; + + save_mesh(mesh, raster, "final-mesh.ply"); + + return std::make_tuple(terrain_mesh, mesh); +} + +void add_label(const Raster &raster, Surface_mesh &mesh) { + Surface_mesh::Property_map label; + bool created; + boost::tie(label, created) = mesh.add_property_map("label",0); + assert(created); + + for (auto face : mesh.faces()) { + int face_label[LABELS.size()] = {0}; + int sum_face_label = 0; + + CGAL::Vertex_around_face_iterator vbegin, vend; + boost::tie(vbegin, vend) = vertices_around_face(mesh.halfedge(face), mesh); + for (auto pixel : raster.triangle_to_pixel(mesh.point(*(vbegin++)), mesh.point(*(vbegin++)), mesh.point(*(vbegin++)))) { + if (raster.land_cover[pixel.second][pixel.first] > -1) { + sum_face_label++; + face_label[raster.land_cover[pixel.second][pixel.first]]++; + } + } + + auto argmax = std::max_element(face_label, face_label+LABELS.size()); + label[face] = argmax - face_label; + } +} + +void change_vertical_faces(Surface_mesh &mesh, const Raster &raster) { + Surface_mesh::Property_map label; + bool has_label; + boost::tie(label, has_label) = mesh.property_map("label"); + assert(has_label); + + std::unordered_map new_label; + std::list remove_face; + + for (auto face : mesh.faces()) { + new_label[face] = label[face]; + CGAL::Vertex_around_face_iterator vbegin, vend; + boost::tie(vbegin, vend) = vertices_around_face(mesh.halfedge(face), mesh); + auto p0 = mesh.point(*(vbegin++)); + auto p1 = mesh.point(*(vbegin++)); + auto p2 = mesh.point(*(vbegin++)); + //TODO : use coords and not grid + float nz = ((-p0.x() + p1.x()) * (-p0.y() + p2.y()) - (-p0.x() + p2.x()) * (-p0.y() + p1.y())); + float surface = pow(K::Triangle_3(p0, p1, p2).squared_area(), 0.5); + if (abs(nz)/(2*surface) < 0.5) { + /*CGAL::Face_around_face_iterator fbegin, fend; + for(boost::tie(fbegin, fend) = faces_around_face(mesh.halfedge(face), mesh); fbegin != fend && new_label[face] != 4; ++fbegin) { + if (*fbegin != boost::graph_traits::null_face()) { + if (label[*fbegin] == 4) { + //Building + new_label[face] = 4; + } else if (label[*fbegin] == 5 && new_label[face] != 4) { + //High vegetation + new_label[face] = 5; + } + CGAL::Face_around_face_iterator fbegin2, fend2; + for(boost::tie(fbegin2, fend2) = faces_around_face(mesh.halfedge(*fbegin), mesh); fbegin2 != fend2 && new_label[face] != 4; ++fbegin2) { + if (*fbegin2 != boost::graph_traits::null_face()) { + if (label[*fbegin2] == 4) { + //Building + new_label[face] = 4; + } else if (label[*fbegin2] == 5 && new_label[face] != 4) { + //High vegetation + new_label[face] = 5; + } + } + } + } + }*/ + + int face_label[LABELS.size()] = {0}; + int sum_face_label = 0; + + CGAL::Vertex_around_face_iterator vbegin, vend; + boost::tie(vbegin, vend) = vertices_around_face(mesh.halfedge(face), mesh); + for (auto pixel : raster.triangle_to_pixel(mesh.point(*(vbegin++)), mesh.point(*(vbegin++)), mesh.point(*(vbegin++)))) { + if (raster.land_cover[pixel.second][pixel.first] > -1) { + sum_face_label++; + face_label[raster.land_cover[pixel.second][pixel.first]]++; + } + } + + if (face_label[4] > 0) { + //Building + new_label[face] = 4; + } else if (face_label[5] > 0) { + //High vegetation + new_label[face] = 5; + } + + if (new_label[face] != 4 && new_label[face] != 5) { + new_label[face] = 0; + } + } + } + + for (auto &face_label: new_label) { + label[face_label.first] = face_label.second; + } + +} + +void set_path(Surface_mesh &mesh, + Surface_mesh::Property_map &path, + Surface_mesh::Property_map &label, + std::vector> &paths, + Surface_mesh::Face_index face, + int path_id) { + path[face] = path_id; + paths[path_id].push_back(face); + CGAL::Face_around_face_iterator fbegin, fend; + for(boost::tie(fbegin, fend) = faces_around_face(mesh.halfedge(face), mesh); fbegin != fend; ++fbegin) { + if (*fbegin != boost::graph_traits::null_face()) { + if (label[face] == label[*fbegin] && path[*fbegin] == -1) { + set_path(mesh, path, label, paths, *fbegin, path_id); + } + } + } +} + +std::vector> compute_path(Surface_mesh &mesh) { + Surface_mesh::Property_map label; + bool has_label; + boost::tie(label, has_label) = mesh.property_map("label"); + assert(has_label); + + Surface_mesh::Property_map path; + bool created; + boost::tie(path, created) = mesh.add_property_map("path", -1); + assert(created); + + std::vector> paths; + + for (auto face: mesh.faces()) { + if (path[face] == -1) { + paths.push_back(std::list{}); + set_path(mesh, path, label, paths, face, paths.size()-1); + } + } + + return paths; +} + +template +CGAL::Polygon_with_holes_2 polygon(Face_handle face) { + + typedef typename Face_handle::value_type::Vertex::Point::R Kernel; + + CGAL::Polygon_2 border; + auto curr = face->outer_ccb(); + do { + auto p = curr->target()->point(); + border.push_back(p); + } while (++curr != face->outer_ccb()); + + std::list> holes; + for(auto hole = face->holes_begin(); hole != face->holes_end(); hole++) { + auto curr = *hole; + CGAL::Polygon_2 hole_polygon; + do { + auto p = curr->target()->point(); + hole_polygon.push_back(p); + } while (++curr != *hole); + holes.push_back(hole_polygon); + } + + return CGAL::Polygon_with_holes_2(border, holes.begin(), holes.end()); +} + + +std::map> compute_path_polygon(const Surface_mesh &mesh, const std::vector> &paths, const Raster &raster) { + Surface_mesh::Property_map path; + bool has_path; + boost::tie(path, has_path) = mesh.property_map("path"); + assert(has_path); + + Surface_mesh::Property_map label; + bool has_label; + boost::tie(label, has_label) = mesh.property_map("label"); + assert(has_label); + + std::map> path_polygon; + + typedef CGAL::Face_filtered_graph Filtered_graph; + for (int i = 0; i < paths.size(); i++) { + + Filtered_graph filtered_sm(mesh, i, path); + + if (filtered_sm.number_of_faces() > 3) { + + int lab = label[*(CGAL::faces(filtered_sm).first)]; + if (lab == 3 || lab == 8 || lab == 9) { + + Arrangement_2 arr; + std::map point_map; + for (auto edge: CGAL::edges(filtered_sm)) { + if (CGAL::is_border (edge, filtered_sm)) { + auto p0 = mesh.point(CGAL::source(edge, filtered_sm)); + auto p1 = mesh.point(CGAL::target(edge, filtered_sm)); + insert_non_intersecting_curve(arr, Traits_2::X_monotone_curve_2(Exact_predicates_kernel::Point_2(p0.x(), p0.y()), Exact_predicates_kernel::Point_2(p1.x(), p1.y()))); + } + } + + path_polygon[i] = polygon((*(arr.unbounded_face()->holes_begin()))->twin()->face()); + + { // Arrangement + Surface_mesh skeleton; + + std::map v_map; + for (auto v = arr.vertices_begin(); v != arr.vertices_end(); v++) { + auto p = v->point(); + auto z = raster.dsm[int(p.y())][int(p.x())]; + v_map[v] = skeleton.add_vertex(Point_3((float) p.x(), (float) p.y(), z)); + } + + for (auto he = arr.edges_begin(); he != arr.edges_end(); ++he ) { + skeleton.add_edge(v_map[he->source()], v_map[he->target()]); + } + + Surface_mesh::Property_map edge_prop; + bool created; + boost::tie(edge_prop, created) = skeleton.add_property_map("prop",0); + assert(created); + + double min_x, min_y; + raster.grid_to_coord(0, 0, min_x, min_y); + + for(auto vertex : skeleton.vertices()) { + auto point = skeleton.point(vertex); + double x, y; + raster.grid_to_coord((float) point.x(), (float) point.y(), x, y); + skeleton.point(vertex) = Point_3(x-min_x, y-min_y, point.z()); + } + + std::stringstream skeleton_name; + skeleton_name << "arr_" << lab << "_" << i << ".ply"; + std::ofstream mesh_ofile (skeleton_name.str().c_str()); + CGAL::IO::write_PLY (mesh_ofile, skeleton); + mesh_ofile.close(); + } + + Surface_mesh part_mesh; + CGAL::copy_face_graph(filtered_sm, part_mesh); + std::stringstream name; + name << "part_mesh_" << lab << "_" << i << ".ply"; + save_mesh(part_mesh, raster, name.str().c_str()); + + } + } + } + + return path_polygon; + +} + + +std::map>> compute_medial_axes(const Surface_mesh &mesh, const std::vector> &paths, const std::map> &path_polygon, const Raster &raster) { + Surface_mesh::Property_map label; + bool has_label; + boost::tie(label, has_label) = mesh.property_map("label"); + assert(has_label); + + std::map>> medial_axes; + + for (int i = 0; i < paths.size(); i++) { + + if (path_polygon.count(i) > 0) { + + int lab = label[paths.at(i).front()]; + + auto poly = path_polygon.at(i); + + poly = CGAL::Polyline_simplification_2::simplify( + poly, + CGAL::Polyline_simplification_2::Squared_distance_cost(), + CGAL::Polyline_simplification_2::Stop_above_cost_threshold(pow(raster.coord_distance_to_grid_distance(1.5),2)) + ); + + auto iss = CGAL::create_interior_straight_skeleton_2(poly, Exact_predicates_kernel()); + medial_axes[i] = CGAL::convert_straight_skeleton_2>(*iss); + + { // Skeleton + Surface_mesh skeleton; + + std::map v_map; + for (auto v = iss->vertices_begin(); v != iss->vertices_end(); v++) { + auto p = v->point(); + auto z = raster.dsm[int(p.y())][int(p.x())]; + v_map[v->id()] = skeleton.add_vertex(Point_3((float) p.x(), (float) p.y(), z)); + } + + for (auto he = iss->halfedges_begin(); he != iss->halfedges_end(); ++he ) { + skeleton.add_edge(v_map[he->vertex()->id()], v_map[he->opposite()->vertex()->id()]); + } + + Surface_mesh::Property_map edge_prop; + bool created; + boost::tie(edge_prop, created) = skeleton.add_property_map("prop",0); + assert(created); + + double min_x, min_y; + raster.grid_to_coord(0, 0, min_x, min_y); + + for(auto vertex : skeleton.vertices()) { + auto point = skeleton.point(vertex); + double x, y; + raster.grid_to_coord((float) point.x(), (float) point.y(), x, y); + skeleton.point(vertex) = Point_3(x-min_x, y-min_y, point.z()); + } + + std::stringstream skeleton_name; + skeleton_name << "skeleton_" << lab << "_" << i << ".ply"; + std::ofstream mesh_ofile (skeleton_name.str().c_str()); + CGAL::IO::write_PLY (mesh_ofile, skeleton); + mesh_ofile.close(); + } + + { // Path + Surface_mesh skeleton; + + std::map v_map; + for (auto v = iss->vertices_begin(); v != iss->vertices_end(); v++) { + if (v->is_skeleton()) { + auto p = v->point(); + auto z = raster.dsm[int(p.y())][int(p.x())]; + v_map[v->id()] = skeleton.add_vertex(Point_3((float) p.x(), (float) p.y(),z)); + } + } + + for (auto he = iss->halfedges_begin(); he != iss->halfedges_end(); ++he ) { + if (he->is_inner_bisector()) { + auto v0 = v_map.find(he->vertex()->id()); + auto v1 = v_map.find(he->opposite()->vertex()->id()); + if (v0 != v_map.end() && v1 != v_map.end() && v0->second != v1->second) { + skeleton.add_edge(v_map[he->vertex()->id()], v_map[he->opposite()->vertex()->id()]); + } + } + } + + bool created; + Surface_mesh::Property_map edge_prop; + boost::tie(edge_prop, created) = skeleton.add_property_map("prop",0); + assert(created); + Surface_mesh::Property_map red; + boost::tie(red, created) = skeleton.add_property_map("red", LABELS[lab].red); + assert(created); + Surface_mesh::Property_map green; + boost::tie(green, created) = skeleton.add_property_map("green", LABELS[lab].green); + assert(created); + Surface_mesh::Property_map blue; + boost::tie(blue, created) = skeleton.add_property_map("blue", LABELS[lab].blue); + assert(created); + + double min_x, min_y; + raster.grid_to_coord(0, 0, min_x, min_y); + + for(auto vertex : skeleton.vertices()) { + auto point = skeleton.point(vertex); + double x, y; + raster.grid_to_coord((float) point.x(), (float) point.y(), x, y); + skeleton.point(vertex) = Point_3(x-min_x, y-min_y, point.z()); + } + + std::stringstream skeleton_name; + skeleton_name << "path_" << lab << "_" << i << ".ply"; + std::ofstream mesh_ofile (skeleton_name.str().c_str()); + CGAL::IO::write_PLY (mesh_ofile, skeleton); + mesh_ofile.close(); + } + + } + } + + return medial_axes; + +} + +std::set> link_paths(const Surface_mesh &mesh, const std::vector> &paths, const std::map> &path_polygon, const std::map>> &medial_axes, const Raster &raster) { + + // Get label property + Surface_mesh::Property_map label; + bool has_label; + boost::tie(label, has_label) = mesh.property_map("label"); + assert(has_label); + + std::set> result; + + for (int selected_label: {3, 8, 9}) { + // List path with selected label + std::list same_label_paths; + for (int i = 0; i < paths.size(); i++) { + if (label[paths[i].front()] == selected_label && medial_axes.count(i) == 1) { + same_label_paths.push_back(i); + } + } + + for (int path1: same_label_paths) { + for (int path2: same_label_paths) { + if (path1 < path2) { + + std::map::Vertex_handle, CGAL::Straight_skeleton_2::Vertex_handle>, K::FT> distance_v1v2; + std::map::Vertex_handle, CGAL::Straight_skeleton_2::Halfedge_handle>, std::pair> distance_v1h2; + std::map::Vertex_handle, CGAL::Straight_skeleton_2::Halfedge_handle>, std::pair> distance_v2h1; + + // For vertices pairs + for (auto v1: medial_axes.at(path1)->vertex_handles()) { + if (v1->is_skeleton()) { + for (auto v2: medial_axes.at(path2)->vertex_handles()) { + if (v2->is_skeleton()) { + distance_v1v2[std::make_pair(v1, v2)] = CGAL::squared_distance(v1->point(), v2->point()); + } + } + } + } + + // For vertex on path1 and edge on path2 + for (auto v1: medial_axes.at(path1)->vertex_handles()) { + if (v1->is_skeleton()) { + for (auto edge2: medial_axes.at(path2)->halfedge_handles()) { + if (edge2->vertex()->id() < edge2->opposite()->vertex()->id() && edge2->is_inner_bisector() && edge2->opposite()->is_inner_bisector()) { + auto segment = K::Segment_2(edge2->opposite()->vertex()->point(), edge2->vertex()->point()); + auto proj = segment.supporting_line().projection(v1->point()); + if (segment.collinear_has_on(proj)) { + distance_v1h2[std::make_pair(v1, edge2)] = std::make_pair(CGAL::squared_distance(v1->point(), proj), proj); + } + } + } + } + } + + // For vertex on path2 and edge on path1 + for (auto v2: medial_axes.at(path2)->vertex_handles()) { + if (v2->is_skeleton()) { + for (auto edge1: medial_axes.at(path1)->halfedge_handles()) { + if (edge1->vertex()->id() < edge1->opposite()->vertex()->id() && edge1->is_inner_bisector() && edge1->opposite()->is_inner_bisector()) { + auto segment = K::Segment_2(edge1->opposite()->vertex()->point(), edge1->vertex()->point()); + auto proj = segment.supporting_line().projection(v2->point()); + if (segment.collinear_has_on(proj)) { + distance_v2h1[std::make_pair(v2, edge1)] = std::make_pair(CGAL::squared_distance(v2->point(), proj), proj); + } + } + } + } + } + + // For vertices pairs + for (auto it = distance_v1v2.begin(); it != distance_v1v2.end(); ++it) { + auto v1 = it->first.first; + auto v2 = it->first.second; + auto d = it->second; + + auto he = v1->halfedge_around_vertex_begin(); + do { + auto v = (*he)->opposite()->vertex(); + if (v->is_skeleton()) { + if (distance_v1v2[std::make_pair(v, v2)] < d) { + goto exit1; + } + if ((*he)->vertex()->id() < (*he)->opposite()->vertex()->id()) { + if (distance_v2h1.count(std::make_pair(v2, *he)) > 0 && distance_v2h1[std::make_pair(v2, *he)].first < d) { + goto exit1; + } + } else { + if (distance_v2h1.count(std::make_pair(v2, (*he)->opposite())) > 0 && distance_v2h1[std::make_pair(v2, (*he)->opposite())].first < d) { + goto exit1; + } + } + } + } while (++he != v1->halfedge_around_vertex_begin()); + + he = v2->halfedge_around_vertex_begin(); + do { + auto v = (*he)->opposite()->vertex(); + if (v->is_skeleton()) { + if (distance_v1v2[std::make_pair(v1, v)] < d) { + goto exit1; + } + if ((*he)->vertex()->id() < (*he)->opposite()->vertex()->id()) { + if (distance_v1h2.count(std::make_pair(v1, *he)) > 0 && distance_v1h2[std::make_pair(v1, *he)].first < d) { + goto exit1; + } + } else { + if (distance_v1h2.count(std::make_pair(v1, (*he)->opposite())) > 0 && distance_v1h2[std::make_pair(v1, (*he)->opposite())].first < d) { + goto exit1; + } + } + } + } while (++he != v2->halfedge_around_vertex_begin()); + + result.insert(std::make_pair(skeletonPoint(path1, v1), skeletonPoint(path2, v2))); + exit1: + continue; + } + + // For vertex on path1 and edge on path2 + for (auto it = distance_v1h2.begin(); it != distance_v1h2.end(); ++it) { + auto v1 = it->first.first; + auto e2 = it->first.second; + auto d = it->second.first; + auto p2 = it->second.second; + + auto he = v1->halfedge_around_vertex_begin(); + do { + auto v = (*he)->opposite()->vertex(); + if (v->is_skeleton()) { + if (CGAL::squared_distance(v->point(), p2) < d) { + goto exit2; + } + auto segment = K::Segment_2((*he)->opposite()->vertex()->point(), (*he)->vertex()->point()); + auto proj = segment.supporting_line().projection(p2); + if (segment.collinear_has_on(proj)) { + goto exit2; + } + } + } while (++he != v1->halfedge_around_vertex_begin()); + + result.insert(std::make_pair(skeletonPoint(path1, v1), skeletonPoint(path2, e2, p2))); + exit2: + continue; + } + + // For vertex on path2 and edge on path1 + for (auto it = distance_v2h1.begin(); it != distance_v2h1.end(); ++it) { + auto v2 = it->first.first; + auto e1 = it->first.second; + auto d = it->second.first; + auto p1 = it->second.second; + + auto he = v2->halfedge_around_vertex_begin(); + do { + auto v = (*he)->opposite()->vertex(); + if (v->is_skeleton()) { + if (CGAL::squared_distance(v->point(), p1) < d) { + goto exit3; + } + auto segment = K::Segment_2((*he)->opposite()->vertex()->point(), (*he)->vertex()->point()); + auto proj = segment.supporting_line().projection(p1); + if (segment.collinear_has_on(proj)) { + goto exit3; + } + } + } while (++he != v2->halfedge_around_vertex_begin()); + + result.insert(std::make_pair(skeletonPoint(path2, v2), skeletonPoint(path1, e1, p1))); + exit3: + continue; + } + + } else if (path1 == path2) { + + std::map::Vertex_handle, CGAL::Straight_skeleton_2::Vertex_handle>, K::FT> distance_vv; + std::map::Vertex_handle, CGAL::Straight_skeleton_2::Halfedge_handle>, std::pair> distance_vh; + + for (auto v1: medial_axes.at(path1)->vertex_handles()) { + if (v1->is_skeleton()) { + // For vertices pairs + for (auto v2: medial_axes.at(path1)->vertex_handles()) { + if (v2->is_skeleton()) { + distance_vv[std::make_pair(v1, v2)] = CGAL::squared_distance(v1->point(), v2->point()); + } + } + // For vertex and edge + for (auto edge2: medial_axes.at(path1)->halfedge_handles()) { + if (edge2->vertex()->id() < edge2->opposite()->vertex()->id() && edge2->is_inner_bisector() && edge2->opposite()->is_inner_bisector()) { + auto segment = K::Segment_2(edge2->opposite()->vertex()->point(), edge2->vertex()->point()); + auto proj = segment.supporting_line().projection(v1->point()); + if (segment.collinear_has_on(proj)) { + distance_vh[std::make_pair(v1, edge2)] = std::make_pair(CGAL::squared_distance(v1->point(), proj), proj); + } + } + } + } + } + + // For vertices pairs + for (auto it = distance_vv.begin(); it != distance_vv.end(); ++it) { + auto v1 = it->first.first; + auto v2 = it->first.second; + auto d = it->second; + + if (v1->id() == v2->id()) continue; + + // Exit link + Exact_predicates_kernel::Segment_2 segment(Exact_predicates_kernel::Point_2(v1->point().x(), v1->point().y()), Exact_predicates_kernel::Point_2(v2->point().x(), v2->point().y())); + bool intersect = false; + for (auto edge = path_polygon.at(path1).outer_boundary().edges_begin(); !intersect && edge != path_polygon.at(path1).outer_boundary().edges_end(); edge++) { + if (CGAL::do_intersect(*edge, segment)) { + intersect = true; + } + } + for (auto hole: path_polygon.at(path1).holes()) { + for (auto edge = hole.edges_begin(); !intersect && edge != hole.edges_end(); edge++) { + if (CGAL::do_intersect(*edge, segment)) { + intersect = true; + } + } + } + if (!intersect) continue; + + auto he = v1->halfedge_around_vertex_begin(); + do { + auto v = (*he)->opposite()->vertex(); + if (v->is_skeleton()) { + if (distance_vv[std::make_pair(v, v2)] < d) { + goto exit4; + } + if ((*he)->vertex()->id() < (*he)->opposite()->vertex()->id()) { + if (distance_vh.count(std::make_pair(v2, *he)) > 0 && distance_vh[std::make_pair(v2, *he)].first < d) { + goto exit4; + } + } else { + if (distance_vh.count(std::make_pair(v2, (*he)->opposite())) > 0 && distance_vh[std::make_pair(v2, (*he)->opposite())].first < d) { + goto exit4; + } + } + } + } while (++he != v1->halfedge_around_vertex_begin()); + + he = v2->halfedge_around_vertex_begin(); + do { + auto v = (*he)->opposite()->vertex(); + if (v->is_skeleton()) { + if (distance_vv[std::make_pair(v1, v)] < d) { + goto exit4; + } + if ((*he)->vertex()->id() < (*he)->opposite()->vertex()->id()) { + if (distance_vh.count(std::make_pair(v1, *he)) > 0 && distance_vh[std::make_pair(v1, *he)].first < d) { + goto exit4; + } + } else { + if (distance_vh.count(std::make_pair(v1, (*he)->opposite())) > 0 && distance_vh[std::make_pair(v1, (*he)->opposite())].first < d) { + goto exit4; + } + } + } + } while (++he != v2->halfedge_around_vertex_begin()); + + result.insert(std::make_pair(skeletonPoint(path1, v1), skeletonPoint(path1, v2))); + exit4: + continue; + } + + // For vertex on path1 and edge on path2 + for (auto it = distance_vh.begin(); it != distance_vh.end(); ++it) { + auto v1 = it->first.first; + auto e2 = it->first.second; + auto d = it->second.first; + auto p2 = it->second.second; + + if (v1->id() == e2->vertex()->id() || v1->id() == e2->opposite()->vertex()->id()) continue; + + // Exit link + Exact_predicates_kernel::Segment_2 segment(Exact_predicates_kernel::Point_2(v1->point().x(), v1->point().y()), Exact_predicates_kernel::Point_2(p2.x(), p2.y())); + bool intersect = false; + for (auto edge = path_polygon.at(path1).outer_boundary().edges_begin(); !intersect && edge != path_polygon.at(path1).outer_boundary().edges_end(); edge++) { + if (CGAL::do_intersect(*edge, segment)) { + intersect = true; + } + } + for (auto hole: path_polygon.at(path1).holes()) { + for (auto edge = hole.edges_begin(); !intersect && edge != hole.edges_end(); edge++) { + if (CGAL::do_intersect(*edge, segment)) { + intersect = true; + } + } + } + if (!intersect) continue; + + auto he = v1->halfedge_around_vertex_begin(); + do { + auto v = (*he)->opposite()->vertex(); + if (v->is_skeleton()) { + if (CGAL::squared_distance(v->point(), p2) < d) { + goto exit5; + } + auto segment = K::Segment_2((*he)->opposite()->vertex()->point(), (*he)->vertex()->point()); + auto proj = segment.supporting_line().projection(p2); + if (segment.collinear_has_on(proj)) { + goto exit5; + } + } + } while (++he != v1->halfedge_around_vertex_begin()); + + result.insert(std::make_pair(skeletonPoint(path1, v1), skeletonPoint(path1, e2, p2))); + exit5: + continue; + } + } + } + } + } + + Surface_mesh links; + + for(auto link: result) { + auto z1 = raster.dsm[int(link.first.point.y())][int(link.first.point.x())]; + auto z2 = raster.dsm[int(link.second.point.y())][int(link.second.point.x())]; + auto v1 = links.add_vertex(Point_3((float) link.first.point.x(), (float) link.first.point.y(), z1)); + auto v2 = links.add_vertex(Point_3((float) link.second.point.x(), (float) link.second.point.y(), z2)); + links.add_edge(v1,v2); + } + + bool created; + Surface_mesh::Property_map edge_prop; + boost::tie(edge_prop, created) = links.add_property_map("prop",0); + assert(created); + + double min_x, min_y; + raster.grid_to_coord(0, 0, min_x, min_y); + + for(auto vertex : links.vertices()) { + auto point = links.point(vertex); + double x, y; + raster.grid_to_coord((float) point.x(), (float) point.y(), x, y); + links.point(vertex) = Point_3(x-min_x, y-min_y, point.z()); + } + + std::ofstream mesh_ofile ("links.ply"); + CGAL::IO::write_PLY (mesh_ofile, links); + mesh_ofile.close(); + + return result; + +} diff --git a/header.hpp b/header.hpp new file mode 100644 index 0000000..ebeb364 --- /dev/null +++ b/header.hpp @@ -0,0 +1,95 @@ +#ifndef HEADER_H_ +#define HEADER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ogrsf_frmts.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "label.hpp" + +typedef CGAL::Simple_cartesian K; +typedef K::Point_2 Point_2; +typedef K::Point_3 Point_3; +typedef CGAL::Surface_mesh Surface_mesh; +typedef CGAL::Polygon_2 Polygon; + +namespace SMS = CGAL::Surface_mesh_simplification; + +typedef CGAL::Exact_predicates_inexact_constructions_kernel Exact_predicates_kernel; +typedef CGAL::Arr_segment_traits_2 Traits_2; +typedef CGAL::Arrangement_2 Arrangement_2; + +namespace PMP = CGAL::Polygon_mesh_processing; + +/// A point on a skeleton +struct skeletonPoint { + /// The skeleton id + int path; + /// The point coordinate + Point_2 point; + /// A handle to the vertex if the point is a vertex + CGAL::Straight_skeleton_2::Vertex_handle vertex; + /// Or a handle to the edge otherwise + CGAL::Straight_skeleton_2::Halfedge_handle halfedge; + + skeletonPoint () {} + + skeletonPoint (int _path, CGAL::Straight_skeleton_2::Vertex_handle _vertex) { + path = _path; + vertex = _vertex; + point = _vertex->point(); + halfedge = nullptr; + } + + skeletonPoint (int _path, CGAL::Straight_skeleton_2::Halfedge_handle _halfedge, K::Point_2 _point) { + path = _path; + halfedge = _halfedge; + point = _point; + vertex = nullptr; + } + + friend bool operator<(const skeletonPoint& l, const skeletonPoint& r) { + if (l.path == r.path) return l.point < r.point; + return l.path < r.path; + } + + friend bool operator==(const skeletonPoint& l, const skeletonPoint& r) { + return (l.path == r.path && l.point == r.point); + } +}; + +#endif /* !HEADER_H_ */ \ No newline at end of file diff --git a/label.cpp b/label.hpp similarity index 89% rename from label.cpp rename to label.hpp index f13c512..88a9e05 100644 --- a/label.cpp +++ b/label.hpp @@ -1,3 +1,6 @@ +#ifndef LABEL_H_ +#define LABEL_H_ + #include #include @@ -12,7 +15,7 @@ struct Label { Label(int value, std::string label, unsigned char red, unsigned char green, unsigned char blue): value(value), label(label), red(red), green(green), blue(blue) {} }; -std::array LABELS { +static std::array LABELS { Label(0, "other", 255, 255, 255), Label(1, "bare ground", 100, 50, 0), Label(2, "low vegetation", 0, 250, 50), @@ -25,3 +28,5 @@ std::array LABELS { Label(9, "railways", 200, 100, 200), Label(10, "swimming pool", 50, 150, 250) }; + +#endif /* !LABEL_H_ */ \ No newline at end of file diff --git a/main.cpp b/main.cpp index d3fff1d..3f322bb 100644 --- a/main.cpp +++ b/main.cpp @@ -1,1629 +1,23 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ogrsf_frmts.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "label.cpp" - -typedef CGAL::Simple_cartesian K; -typedef K::Point_2 Point_2; -typedef K::Point_3 Point_3; -typedef CGAL::Surface_mesh Surface_mesh; -typedef CGAL::Polygon_2 Polygon; - -namespace SMS = CGAL::Surface_mesh_simplification; - -typedef CGAL::Exact_predicates_inexact_constructions_kernel Exact_predicates_kernel; -typedef CGAL::Arr_segment_traits_2 Traits_2; -typedef CGAL::Arrangement_2 Arrangement_2; - -namespace PMP = CGAL::Polygon_mesh_processing; - -class Raster { - public: - std::vector> dsm; - std::vector> dtm; - std::vector> land_cover; - int xSize; - int ySize; - - private: - OGRSpatialReference crs; - double grid_to_crs[6] = {0,1,0,0,0,1}; - - static std::pair grid_conversion(int P, int L, double grid_to_crs[6], OGRCoordinateTransformation *crs_to_other_crs, double other_grid_to_other_crs[6]) { - double x = grid_to_crs[0] + (0.5 + P)*grid_to_crs[1] + (0.5 + L)*grid_to_crs[2]; - double y = grid_to_crs[3] + (0.5 + P)*grid_to_crs[4] + (0.5 + L)*grid_to_crs[5]; - crs_to_other_crs->Transform(1,&x,&y); - double fact = other_grid_to_other_crs[1]*other_grid_to_other_crs[5] - other_grid_to_other_crs[2]*other_grid_to_other_crs[4]; - x -= other_grid_to_other_crs[0]; - y -= other_grid_to_other_crs[3]; - int newP = ((int) ((other_grid_to_other_crs[5]*x - other_grid_to_other_crs[2]*y) / fact)); - int newL = ((int) ((-other_grid_to_other_crs[4]*x + other_grid_to_other_crs[1]*y) / fact)); - return std::pair(newP, newL); - } - - void align_land_cover_and_dsm() { - std::vector> new_land_cover = std::vector>(ySize, std::vector(xSize, -1)); - for (int L = 0; L < ySize; L++) { - for (int P = 0; P < xSize; P++) { - float neighboors_count[LABELS.size()] = {0}; - int n = 0; - float moy = 0; - float square_moy = 0; - for (int i = -5; i <= 5; i++) { - for (int j = -5; j <= 5; j++) { - if (L+i >= 0 && P + j >= 0 && L+i < ySize && P+j < xSize) { - neighboors_count[land_cover[L+i][P+j]] += 1/(0.1+abs(dsm[L][P] - dsm[L+i][P+j])); - if (i > -3 && i < 3 && j > -3 && j < 3) { - n++; - moy += dsm[L+i][P+j]; - square_moy += pow(dsm[L+i][P+j], 2); - } - } - } - } - if (pow(square_moy/n - pow(moy/n, 2), 0.5) > 0.2) { - new_land_cover[L][P] = std::max_element(neighboors_count, neighboors_count + LABELS.size()) - neighboors_count; - } else { - new_land_cover[L][P] = land_cover[L][P]; - } - - } - } - - land_cover = new_land_cover; - } - - public: - void coord_to_grid(double x, double y, float& P, float& L) const { - double fact = grid_to_crs[1]*grid_to_crs[5] - grid_to_crs[2]*grid_to_crs[4]; - x -= grid_to_crs[0]; - y -= grid_to_crs[3]; - P = (grid_to_crs[5]*x - grid_to_crs[2]*y) / fact; - L = (-grid_to_crs[4]*x + grid_to_crs[1]*y) / fact; - } - - void grid_to_coord(int P, int L, double& x, double& y) const { - x = grid_to_crs[0] + (0.5 + P)*grid_to_crs[1] + (0.5 + L)*grid_to_crs[2]; - y = grid_to_crs[3] + (0.5 + P)*grid_to_crs[4] + (0.5 + L)*grid_to_crs[5]; - } - - void grid_to_coord(float P, float L, double& x, double& y) const { - x = grid_to_crs[0] + P*grid_to_crs[1] + L*grid_to_crs[2]; - y = grid_to_crs[3] + P*grid_to_crs[4] + L*grid_to_crs[5]; - } - - float coord_distance_to_grid_distance(double d) const { - float P, L; - double x, y; - grid_to_coord(0, 0, x, y); - coord_to_grid(x+d/sqrt(2),y+d/sqrt(2),P,L); - return sqrt(pow(P,2)+pow(L,2)); - } - - double grid_distance_to_coord_distance(float d) const { - double x1, y1, x2, y2; - grid_to_coord(0, 0, x1, y1); - grid_to_coord((float) (d/sqrt(2)), (float) (d/sqrt(2)), x2, y2); - return sqrt(pow(x2-x1,2)+pow(y2-y1,2)); - } - - OGRSpatialReference get_crs() const { - return OGRSpatialReference(crs); - } - - template - std::list> triangle_to_pixel(CGAL::Point_3 a, CGAL::Point_3 b, CGAL::Point_3 c) const { - std::list> ret; - int min_x = std::max((int) std::min({a.x(), b.x(), c.x()}), 0); - int max_x = std::min((int) std::max({a.x(), b.x(), c.x()}), xSize-1); - int min_y = std::max((int) std::min({a.y(), b.y(), c.y()}), 0); - int max_y = std::min((int) std::max({a.y(), b.y(), c.y()}), ySize-1); - K::Triangle_2 triangle (Point_2(a.x(), a.y()), Point_2(b.x(), b.y()), Point_2(c.x(), c.y())); - for (int L = min_y; L <= max_y; L++) { - for (int P = min_x; P <= max_x; P++) { - if (triangle.bounded_side(Point_2(0.5 + P, 0.5 + L)) != CGAL::ON_UNBOUNDED_SIDE) { - ret.push_back(std::pair(P,L)); - } - } - } - return ret; - } - - Raster(char *dsm_path, char *dtm_path, char *land_cover_path) { - GDALAllRegister(); - - // Get DSM informations and CRS - GDALDataset *dsm_dataset = (GDALDataset *) GDALOpen(dsm_path, GA_ReadOnly ); - if( dsm_dataset == NULL ) { - throw std::invalid_argument(std::string("Unable to open ") + dsm_path + "."); - } - xSize = dsm_dataset->GetRasterBand(1)->GetXSize(); - ySize = dsm_dataset->GetRasterBand(1)->GetYSize(); - crs = *dsm_dataset->GetSpatialRef(); - if (dsm_dataset->GetGeoTransform(grid_to_crs) >= CE_Failure) { - throw std::invalid_argument(std::string(dsm_path) + " do not contain an affine transform."); - } - dsm = std::vector>(ySize, std::vector(xSize, 0)); - for (int L = 0; L < ySize; L++) { - if (dsm_dataset->GetRasterBand(1)->RasterIO(GF_Read, 0, L, xSize, 1, &dsm[L][0], xSize, 1, GDT_Float32, 0, 0) >= CE_Failure) { - throw std::invalid_argument(std::string(dsm_path) + " can't be read."); - } - } - std::cout << "DSM load" << std::endl; - - // Get DTM informations and CRS transform - dtm = std::vector>(ySize, std::vector(xSize, 0)); - GDALDataset *dtm_dataset = (GDALDataset *) GDALOpen( dtm_path, GA_ReadOnly ); - if( dtm_dataset == NULL ) { - throw std::invalid_argument(std::string("Unable to open ") + dtm_path + "."); - } - double dtm_grid_to_dtm_crs[6]; - if (dtm_dataset->GetGeoTransform(dtm_grid_to_dtm_crs) >= CE_Failure) { - throw std::invalid_argument("Can't transform DTM grid to DTM CRS."); - } - OGRCoordinateTransformation *CRS_to_dtm_crs = OGRCreateCoordinateTransformation( - &crs, - dtm_dataset->GetSpatialRef()); - if (CRS_to_dtm_crs == NULL) { - throw std::runtime_error("Can't transform DSM CRS to DTM CRS."); - } - for (int L = 0; L < ySize; L++) { - for (int P = 0; P < xSize; P++) { - std::pair new_coord = grid_conversion(P, L, grid_to_crs, CRS_to_dtm_crs, dtm_grid_to_dtm_crs); - if (new_coord.first >= 0 && new_coord.first < dtm_dataset->GetRasterBand(1)->GetXSize() && new_coord.second >= 0 && new_coord.second < dtm_dataset->GetRasterBand(1)->GetYSize()) { - if (dtm_dataset->GetRasterBand(1)->RasterIO(GF_Read, new_coord.first, new_coord.second, 1, 1, &dtm[L][P], 1, 1, GDT_Float32, 0, 0) >= CE_Failure) { - throw std::invalid_argument(std::string(dtm_path) + " can't be read."); - } - } - } - } - std::cout << "DTM load" << std::endl; - - // Get land cover informations and CRS transform - land_cover = std::vector>(ySize, std::vector(xSize, -1)); - GDALDataset *land_cover_dataset = (GDALDataset *) GDALOpen( land_cover_path, GA_ReadOnly ); - if( dsm_dataset == NULL ) { - throw std::invalid_argument(std::string("Unable to open ") + land_cover_path + "."); - } - - double land_cover_grid_to_land_cover_crs[6]; - if (land_cover_dataset->GetGeoTransform(land_cover_grid_to_land_cover_crs) >= CE_Failure) { - throw std::invalid_argument("Can't transform land cover grid to land cover CRS."); - } - OGRCoordinateTransformation *CRS_to_land_cover_crs = OGRCreateCoordinateTransformation( - &crs, - land_cover_dataset->GetSpatialRef()); - if (CRS_to_land_cover_crs == NULL) { - throw std::runtime_error("Can't transform DSM CRS to land cover CRS."); - } - for (int L = 0; L < ySize; L++) { - for (int P = 0; P < xSize; P++) { - std::pair new_coord = grid_conversion(P, L, grid_to_crs, CRS_to_land_cover_crs, land_cover_grid_to_land_cover_crs); - if (new_coord.first >= 0 && new_coord.first < land_cover_dataset->GetRasterBand(1)->GetXSize() && new_coord.second >= 0 && new_coord.second < land_cover_dataset->GetRasterBand(1)->GetYSize()) { - unsigned char value; - if (land_cover_dataset->GetRasterBand(1)->RasterIO(GF_Read, new_coord.first, new_coord.second, 1, 1, &value, 1, 1, GDT_Byte, 0, 0) >= CE_Failure) { - throw std::invalid_argument(std::string(land_cover_path) + " can't be read."); - } - land_cover[L][P] = (char) value; - } - } - } - std::cout << "Land cover load" << std::endl; - - align_land_cover_and_dsm(); - } -}; - -std::list get_LOD0_from_shapefile(char *path) { - GDALAllRegister(); - - GDALDataset *dataset; - - std::list polygons; - - dataset = (GDALDataset*) GDALOpenEx(path, GDAL_OF_VECTOR, NULL, NULL, NULL ); - if( dataset == NULL ) { - std::cerr << "Unable to open " << path << "." << std::endl; - return polygons; - } - - for( OGRLayer *layer: dataset->GetLayers() ) { - for( const auto& feature: *layer ) { - if ((*feature)["num_class"].GetInteger() == 4) { - // Get only the buildings - OGRGeometry *geometry = feature->GetGeometryRef(); - - if (wkbFlatten(geometry->getGeometryType()) == wkbPolygon) { - - OGRPolygon *shp_polygon = geometry->toPolygon(); - Polygon cgal_polygon; - std::istringstream wkt(shp_polygon->exportToWkt()); - CGAL::IO::read_polygon_WKT (wkt, cgal_polygon); - polygons.push_back(cgal_polygon); - - } else if (wkbFlatten(geometry->getGeometryType()) == wkbMultiPolygon) { - - OGRMultiPolygon *shp_multi_polygon = geometry->toMultiPolygon(); - for (OGRPolygon *shp_polygon: *shp_multi_polygon) { - Polygon cgal_polygon; - std::istringstream wkt(shp_polygon->exportToWkt()); - CGAL::IO::read_polygon_WKT (wkt, cgal_polygon); - polygons.push_back(cgal_polygon); - } - - } - } - } - } - - return polygons; -} - -float single_face_cost(const Raster &raster, const Point_3 &p0, const Point_3 &p1, const Point_3 &p2) { - float nz = ((-p0.x() + p1.x()) * (-p0.y() + p2.y()) - (-p0.x() + p2.x()) * (-p0.y() + p1.y())); - if (nz == 0) { - // flat triangle - return 0; - } - - std::list> pixels = raster.triangle_to_pixel(p0, p1, p2); - - // Entropy - int face_label[LABELS.size()] = {0}; - int sum_face_label = 0; - for (auto pixel : pixels) { - if (raster.land_cover[pixel.second][pixel.first] > -1) { - sum_face_label++; - face_label[raster.land_cover[pixel.second][pixel.first]]++; - } - } - float entropy = 0; - if (sum_face_label > 0) { - for (int i = 0; i < LABELS.size(); i++) { - if (face_label[i] > 0) { - entropy += ((float) face_label[i])*log((float) face_label[i]); - } - } - entropy = log((float) sum_face_label) - entropy/((float) sum_face_label); - } - - // Least squares - float least_squares = 0; - float least = 0; - if (pixels.size() != 0) { - K::Plane_3 plane(p0, p1, p2); - for (auto pixel : pixels) { - float px = 0.5 + pixel.first; - float py = 0.5 + pixel.second; - float pz = - (plane.a() * px + plane.b() * py + plane.d()) / plane.c(); - least_squares += pow(raster.dsm[pixel.second][pixel.first] - pz,2); - least += abs(raster.dsm[pixel.second][pixel.first] - pz); - } - least_squares /= pixels.size(); - least /= pixels.size(); - } - - // Verticality - float verticality = 0; - if (pixels.size() == 0) { - float surface = pow(K::Triangle_3(p0, p1, p2).squared_area (), 0.5); - verticality = abs(nz)/(2*surface); - } - - // Eccentricity - Point_3 S = CGAL::centroid(p0,p1,p2); - float M = (pow(p2.x()-S.x(),2) + pow(p2.y()-S.y(),2) + pow(p2.z()-S.z(),2) + (pow(p1.x()-p0.x(),2) + pow(p1.y()-p0.y(),2) + pow(p1.z()-p0.z(),2))/3)/4; - float N = sqrtf(pow((p2.y()-S.y())*(p1.z()-p0.z())-(p2.z()-S.z())*(p1.y()-p0.y()),2) + pow((p2.z()-S.z())*(p1.x()-p0.x())-(p2.x()-S.x())*(p1.z()-p0.z()),2) + pow((p2.x()-S.x())*(p1.y()-p0.y())-(p2.y()-S.y())*(p1.x()-p0.x()),2))/(4 * sqrtf(3)); - float eccentricity = M*M - 4*N*N; - eccentricity = sqrtf(1-(M-eccentricity)/(M+eccentricity)); - - return 0 * least_squares + 1 * least + 1 * entropy + 0 * verticality + 0 * eccentricity; -} - -float face_cost(const Raster &raster, const Point_3 &p0, const Point_3 &p1, const Point_3 &p2) { - float surface = pow(K::Triangle_3(p0, p1, p2).squared_area (), 0.5); - return surface * (1 + single_face_cost(raster, p0, p1, p2)); -} - -Point_3 best_point(const Raster &raster, K::FT x, K::FT y, const SMS::Edge_profile& profile) { - - float z; - float d = 0; - float D = 0; - std::list> values; - float t = 0; - int count = 0; - - //Foreach face - for (auto he : CGAL::halfedges_around_source(profile.v0(), profile.surface_mesh())) { - if (he != profile.v0_v1() && he != profile.v0_vR()) { - Point_3 A = get(profile.vertex_point_map(),CGAL::target(he, profile.surface_mesh())); - Point_3 B = get(profile.vertex_point_map(),CGAL::target(CGAL::next(he, profile.surface_mesh()), profile.surface_mesh())); - float i2 = ((-A.x() + B.x()) * (-A.y() + y) - (-A.x() + x) * (-A.y() + B.y())); - if (i2 != 0) { - for (auto pixel : raster.triangle_to_pixel(A, B, Point_3(x, y, 0))) { //for each pixel - float px = 0.5 + pixel.first; - float py = 0.5 + pixel.second; - float pz = raster.dsm[pixel.second][pixel.first]; - - float i1 = ((A.x() - B.x()) * (-A.y() + py) + (-A.x() + px) * (-A.y() + B.y())); - - d += i1 * (((A.x() - px) * (A.y() * B.z() - A.z() * B.y() + A.z() * y - B.z() * y) + (A.y() - py) * (-A.x() * B.z() + A.z() * B.x() - A.z() * x + B.z() * x) + (A.z() - pz) * i2) / (i2 * i2)); - D += (i1 * i1) / (i2 * i2); - values.push_back(std::pair(((A.x() - px) * (A.y() * B.z() - A.z() * B.y() + A.z() * y - B.z() * y) + (A.y() - py) * (-A.x() * B.z() + A.z() * B.x() - A.z() * x + B.z() * x) + (A.z() - pz) * i2) / i1, abs(i1/i2))); - t += abs(i1/i2); - count ++; - } - } - } - } - for (auto he : CGAL::halfedges_around_source(profile.v1(), profile.surface_mesh())) { - if (he != profile.v1_v0() && he != profile.v1_vL()) { - Point_3 A = get(profile.vertex_point_map(),CGAL::target(he, profile.surface_mesh())); - Point_3 B = get(profile.vertex_point_map(),CGAL::target(CGAL::next(he, profile.surface_mesh()), profile.surface_mesh())); - float i2 = ((-A.x() + B.x()) * (-A.y() + y) - (-A.x() + x) * (-A.y() + B.y())); - if (i2 != 0) { - for (auto pixel : raster.triangle_to_pixel(A, B, Point_3(x, y, 0))) { //for each pixel - float px = 0.5 + pixel.first; - float py = 0.5 + pixel.second; - float pz = raster.dsm[pixel.second][pixel.first]; - - float i1 = ((A.x() - B.x()) * (-A.y() + py) + (-A.x() + px) * (-A.y() + B.y())); - - d += i1 * (((A.x() - px) * (A.y() * B.z() - A.z() * B.y() + A.z() * y - B.z() * y) + (A.y() - py) * (-A.x() * B.z() + A.z() * B.x() - A.z() * x + B.z() * x) + (A.z() - pz) * i2) / (i2 * i2)); - D += (i1 * i1) / (i2 * i2); - values.push_back(std::pair(((A.x() - px) * (A.y() * B.z() - A.z() * B.y() + A.z() * y - B.z() * y) + (A.y() - py) * (-A.x() * B.z() + A.z() * B.x() - A.z() * x + B.z() * x) + (A.z() - pz) * i2) / i1, abs(i1/i2))); - t += abs(i1/i2); - count ++; - } - } - } - } - - if (count > 0) { - z = d / D; - - values.sort([](std::pair a, std::pair b) { - return a.first > b.first; - }); - - auto p = values.begin(); - float s = 0; - while (s+p->second < t/2) { - s += p->second; - p++; - } - z = p->first; - - } else { - if (profile.p1().x() != profile.p0().x()) { - z = (x - profile.p0().x())/(profile.p1().x() - profile.p0().x())*(profile.p1().z() - profile.p0().z()); - } else { - z = (y - profile.p0().y())/(profile.p1().y() - profile.p0().y())*(profile.p1().z() - profile.p0().z()); - } - } - - return Point_3(x,y,z); -} - -class Custom_placement { - const Raster &raster; - - public: - Custom_placement (const Raster &raster) : raster(raster) {} - - boost::optional::Point> operator()(const SMS::Edge_profile& profile) const { - typedef boost::optional::Point> result_type; - - K::Vector_3 vector(profile.p0(), profile.p1()); - Point_3 p[5] = {profile.p0(), profile.p0() + 0.25 * vector, profile.p0() + 0.5 * vector, profile.p0() + 0.75 * vector, profile.p1()}; - float cost[5] = {0}; - - /*for (int j = 0; j < 5; j++) { - p[j] = best_point(raster, p[j].x(), p[j].y(), profile); - }*/ - - for (auto he : CGAL::halfedges_around_source(profile.v0(), profile.surface_mesh())) { - if (he != profile.v0_v1() && he != profile.v0_vR()) { - Point_3 A = get(profile.vertex_point_map(),CGAL::target(he, profile.surface_mesh())); - Point_3 B = get(profile.vertex_point_map(),CGAL::target(CGAL::next(he, profile.surface_mesh()), profile.surface_mesh())); - for (int j = 0; j < 5; j++) { - cost[j] += face_cost(raster, A, B, p[j]); - } - } - } - for (auto he : CGAL::halfedges_around_source(profile.v1(), profile.surface_mesh())) { - if (he != profile.v1_v0() && he != profile.v1_vL()) { - Point_3 A = get(profile.vertex_point_map(),CGAL::target(he, profile.surface_mesh())); - Point_3 B = get(profile.vertex_point_map(),CGAL::target(CGAL::next(he, profile.surface_mesh()), profile.surface_mesh())); - for (int j = 0; j < 5; j++) { - cost[j] += face_cost(raster, A, B, p[j]); - } - } - } - - for (int i = 0; i < 2; i++) { - int min_cost = std::min_element(cost, cost + 5) - cost; - - if (min_cost == 0 || min_cost == 1) { - p[4] = p[2]; - cost[4] = cost[2]; - p[2] = p[1]; - cost[2] = cost[1]; - } else if (min_cost == 2) { - p[0] = p[1]; - cost[0] = cost[1]; - p[4] = p[3]; - cost[4] = cost[3]; - } else { - p[0] = p[2]; - cost[0] = cost[2]; - p[2] = p[3]; - cost[2] = cost[3]; - } - - vector = K::Vector_3(p[0], p[4]); - p[1] = p[0] + 0.25 * vector; - p[3] = p[0] + 0.75 * vector; - //p[1] = best_point(raster, p[1].x(), p[1].y(), profile); - //p[3] = best_point(raster, p[1].x(), p[1].y(), profile); - - cost[1] = 0; - cost[3] = 0; - - for (auto he : CGAL::halfedges_around_source(profile.v0(), profile.surface_mesh())) { - if (he != profile.v0_v1() && he != profile.v0_vR()) { - Point_3 A = get(profile.vertex_point_map(),CGAL::target(he, profile.surface_mesh())); - Point_3 B = get(profile.vertex_point_map(),CGAL::target(CGAL::next(he, profile.surface_mesh()), profile.surface_mesh())); - cost[1] += face_cost(raster, A, B, p[1]); - cost[3] += face_cost(raster, A, B, p[3]); - } - } - for (auto he : CGAL::halfedges_around_source(profile.v1(), profile.surface_mesh())) { - if (he != profile.v1_v0() && he != profile.v1_vL()) { - Point_3 A = get(profile.vertex_point_map(),CGAL::target(he, profile.surface_mesh())); - Point_3 B = get(profile.vertex_point_map(),CGAL::target(CGAL::next(he, profile.surface_mesh()), profile.surface_mesh())); - cost[1] += face_cost(raster, A, B, p[1]); - cost[3] += face_cost(raster, A, B, p[3]); - } - } - } - - int min_cost = std::min_element(cost, cost + 5) - cost; - - //std::cout << "Placement: (" << profile.p0() << ") - (" << profile.p1() << ") -> (" << p[min_cost] << ")\n"; - Point_3 placement = p[min_cost]; - - for (auto he : CGAL::halfedges_around_source(profile.v0(), profile.surface_mesh())) { - if (he != profile.v0_v1() && he != profile.v0_vR()) { - Point_3 A = get(profile.vertex_point_map(),CGAL::target(he, profile.surface_mesh())); - Point_3 B = get(profile.vertex_point_map(),CGAL::target(CGAL::next(he, profile.surface_mesh()), profile.surface_mesh())); - if (CGAL::orientation(Point_2(A.x(),A.y()), Point_2(B.x(),B.y()), Point_2(profile.p0().x(),profile.p0().y())) != CGAL::orientation(Point_2(A.x(),A.y()), Point_2(B.x(),B.y()), Point_2(placement.x(),placement.y()))) { - return result_type(); - } - } - } - for (auto he : CGAL::halfedges_around_source(profile.v1(), profile.surface_mesh())) { - if (he != profile.v1_v0() && he != profile.v1_vL()) { - Point_3 A = get(profile.vertex_point_map(),CGAL::target(he, profile.surface_mesh())); - Point_3 B = get(profile.vertex_point_map(),CGAL::target(CGAL::next(he, profile.surface_mesh()), profile.surface_mesh())); - if (CGAL::orientation(Point_2(A.x(),A.y()), Point_2(B.x(),B.y()), Point_2(profile.p1().x(),profile.p1().y())) != CGAL::orientation(Point_2(A.x(),A.y()), Point_2(B.x(),B.y()), Point_2(placement.x(),placement.y()))) { - return result_type(); - } - } - } - - return result_type(placement); - } -}; - -class Custom_cost { - const Raster &raster; - - public: - Custom_cost (const Raster &raster) : raster(raster) {} - - boost::optional::FT> operator()(const SMS::Edge_profile& profile, const boost::optional::Point>& placement) const { - typedef boost::optional::FT> result_type; - - if (placement) { - - float old_cost = 0; - float new_cost = 0; - - SMS::Edge_profile::Triangle_vector triangles = profile.triangles(); - for (auto triange: triangles) { - Point_3 A = get(profile.vertex_point_map(),triange.v0); - Point_3 B = get(profile.vertex_point_map(),triange.v1); - Point_3 C = get(profile.vertex_point_map(),triange.v2); - old_cost += face_cost(raster, A, B, C); - } - - Point_3 C = *placement; - for (auto he : CGAL::halfedges_around_source(profile.v0(), profile.surface_mesh())) { - if (he != profile.v0_v1() && he != profile.v0_vR()) { - Point_3 A = get(profile.vertex_point_map(),CGAL::target(he, profile.surface_mesh())); - Point_3 B = get(profile.vertex_point_map(),CGAL::target(CGAL::next(he, profile.surface_mesh()), profile.surface_mesh())); - new_cost += face_cost(raster, A, B, C); - } - } - for (auto he : CGAL::halfedges_around_source(profile.v1(), profile.surface_mesh())) { - if (he != profile.v1_v0() && he != profile.v1_vL()) { - Point_3 A = get(profile.vertex_point_map(),CGAL::target(he, profile.surface_mesh())); - Point_3 B = get(profile.vertex_point_map(),CGAL::target(CGAL::next(he, profile.surface_mesh()), profile.surface_mesh())); - new_cost += face_cost(raster, A, B, C); - } - } - - //std::cout << "Cost: " << (new_cost - old_cost) << "\n"; - - return result_type(new_cost - old_cost); - } - - return result_type(); - } -}; - -class Cost_stop_predicate { - public: - - Cost_stop_predicate(const float cost) : cost(cost) {} - - bool operator()(const SMS::Edge_profile::FT & current_cost, const SMS::Edge_profile &, const SMS::Edge_profile::edges_size_type, const SMS::Edge_profile::edges_size_type) const { - return current_cost > cost; - } - - private: - const float cost; -}; - -void save_mesh(const Surface_mesh &mesh, const Raster &raster, const char *filename) { - typedef CGAL::Surface_mesh::Point_3> OutputMesh; - OutputMesh output_mesh; - - std::unordered_map::face_descriptor, boost::graph_traits::face_descriptor> f2f; - CGAL::copy_face_graph (mesh, output_mesh, CGAL::parameters::face_to_face_output_iterator(std::inserter(f2f, f2f.end()))); - - // Label - OutputMesh::Property_map output_label; - bool created; - boost::tie(output_label, created) = output_mesh.add_property_map("label",0); - assert(created); - - Surface_mesh::Property_map label; - bool has_label; - boost::tie(label, has_label) = mesh.property_map("label"); - if (has_label) { - for (auto face : mesh.faces()) { - output_label[f2f[face]] = label[face]; - } - } - - // Color - OutputMesh::Property_map red; - OutputMesh::Property_map green; - OutputMesh::Property_map blue; - boost::tie(red, created) = output_mesh.add_property_map("red",0); - assert(created); - boost::tie(green, created) = output_mesh.add_property_map("green",0); - assert(created); - boost::tie(blue, created) = output_mesh.add_property_map("blue",0); - assert(created); - - // Entropy - OutputMesh::Property_map quality; - boost::tie(quality, created) = output_mesh.add_property_map("quality",0); - assert(created); - - Surface_mesh::Property_map path; - bool has_path; - boost::tie(path, has_path) = mesh.property_map("path"); - if (has_path) { - for (auto face : mesh.faces()) { - quality[f2f[face]] = path[face]; - } - } - - for (auto face : output_mesh.faces()) { - if (!has_label) { - int face_label[LABELS.size()] = {0}; - - CGAL::Vertex_around_face_iterator vbegin, vend; - boost::tie(vbegin, vend) = vertices_around_face(output_mesh.halfedge(face), output_mesh); - for (auto pixel : raster.triangle_to_pixel(output_mesh.point(*(vbegin++)), output_mesh.point(*(vbegin++)), output_mesh.point(*(vbegin++)))) { - if (raster.land_cover[pixel.second][pixel.first] > -1) { - face_label[raster.land_cover[pixel.second][pixel.first]]++; - } - } - - auto argmax = std::max_element(face_label, face_label+LABELS.size()); - output_label[face] = argmax - face_label; - } - - red[face] = LABELS[output_label[face]].red; - green[face] = LABELS[output_label[face]].green; - blue[face] = LABELS[output_label[face]].blue; - - if (!has_path) { - CGAL::Vertex_around_face_iterator vbegin, vend; - boost::tie(vbegin, vend) = vertices_around_face(output_mesh.halfedge(face), output_mesh); - auto pa = output_mesh.point(*(vbegin++)); - auto pb = output_mesh.point(*(vbegin++)); - auto pc = output_mesh.point(*(vbegin++)); - quality[face] = single_face_cost(raster, Point_3(pa.x(), pa.y(), pa.z()), Point_3(pb.x(), pb.y(), pb.z()), Point_3(pc.x(), pc.y(), pc.z())); - } - } - - double min_x, min_y; - raster.grid_to_coord(0, 0, min_x, min_y); - - char *temp; - const char *options_json[] = { "MULTILINE=NO", NULL }; - raster.get_crs().exportToPROJJSON(&temp, options_json); - std::string crs_as_json(temp); - CPLFree(temp); - - crs_as_json = regex_replace(crs_as_json, std::regex(",\"id\":\\{\"authority\":\"EPSG\",\"code\":[^\\}]+\\}"), ""); - crs_as_json = regex_replace(crs_as_json, std::regex("Lambert-93"), "Custom"); - - std::smatch matches; - regex_search(crs_as_json, matches, std::regex("\"name\":\"Northing at false origin\",\"value\":([^,]+),")); - crs_as_json = regex_replace(crs_as_json, std::regex("\"name\":\"Northing at false origin\",\"value\":([^,]+),"), "\"name\":\"Northing at false origin\",\"value\":" + std::to_string(std::stoi(matches[1]) - min_y) + ","); - - regex_search(crs_as_json, matches, std::regex("\"name\":\"Easting at false origin\",\"value\":([^,]+),")); - crs_as_json = regex_replace(crs_as_json, std::regex("\"name\":\"Easting at false origin\",\"value\":([^,]+),"), "\"name\":\"Easting at false origin\",\"value\":" + std::to_string(std::stoi(matches[1]) - min_x) + ","); - - OGRSpatialReference output_crs; - output_crs.SetFromUserInput(crs_as_json.c_str()); - output_crs.AutoIdentifyEPSG(); - - /*const char *options_wkt[] = { "MULTILINE=NO", "FORMAT=WKT2", NULL }; - output_crs.exportToWkt(&temp, options_wkt);*/ output_crs.exportToProj4(&temp); // WKT format is too long for MeshLab - std::string crs_as_wkt(temp); - CPLFree(temp); - - for(auto vertex : output_mesh.vertices()) { - auto point = output_mesh.point(vertex); - double x, y; - raster.grid_to_coord((float) point.x(), (float) point.y(), x, y); - output_mesh.point(vertex) = CGAL::Simple_cartesian::Point_3(x-min_x, y-min_y, (double) point.z()); - } - - std::ofstream mesh_ofile (filename, std::ios_base::binary); - CGAL::IO::set_binary_mode (mesh_ofile); - CGAL::IO::write_PLY (mesh_ofile, output_mesh, "crs " + crs_as_wkt); - mesh_ofile.close(); -} - - -struct My_visitor : SMS::Edge_collapse_visitor_base { - private: - int i_collecte = 0; - const Surface_mesh *mesh; - const Raster *raster; - std::chrono::time_point start_collecte; - std::chrono::time_point start_collapse; - bool output[5] = {false}; - - public: - My_visitor(const Surface_mesh *mesh, const Raster *raster) : mesh(mesh), raster(raster) {} - - void OnStarted (Surface_mesh &mesh) { - start_collecte = std::chrono::system_clock::now(); - } - - void OnCollected(const SMS::Edge_profile& profile, const boost::optional< SMS::Edge_profile::FT >& cost) { - start_collapse = std::chrono::system_clock::now(); - i_collecte++; - if (i_collecte%1000 == 0) { - std::chrono::duration diff = start_collapse - start_collecte; - std::cout << "\rCollecte: " << i_collecte << "/" << mesh->number_of_edges() << " (" << ((int) (((float) i_collecte)/mesh->number_of_edges()*100)) << "%)" << " still " << (((float) mesh->number_of_edges() - i_collecte) * diff.count() / i_collecte) << "s" << " (" << (((float) i_collecte) / diff.count()) << " op/s)" << std::flush; - } - } - - void OnSelected (const SMS::Edge_profile &profile, boost::optional< SMS::Edge_profile::FT > cost, const SMS::Edge_profile::edges_size_type initial_edge_count, const SMS::Edge_profile::edges_size_type current_edge_count) { - if (current_edge_count%100 == 0) { - auto end = std::chrono::system_clock::now(); - std::chrono::duration diff = end - start_collapse; - std::cout << "\rCollapse: " << (initial_edge_count-current_edge_count) << "/" << initial_edge_count << " (" << ((int) (((float) (initial_edge_count-current_edge_count))/initial_edge_count*100)) << "%)" << " still " << (((float) current_edge_count) * diff.count() / (initial_edge_count-current_edge_count)) << "s" << " (" << (((float) (initial_edge_count-current_edge_count)) / diff.count()) << " op/s)"; - if (cost) { - std::cout << " - cost: " << *cost << " " << std::flush; - } - } - - if (cost) { - if(*cost > 1e-4 && !output[0]) { - output[0] = true; - save_mesh(*mesh,*raster,"mesh-1e-4.ply"); - } else if(*cost > 0 && !output[1]) { - output[1] = true; - save_mesh(*mesh,*raster,"mesh-0.ply"); - } - if(current_edge_count <= 100000 && !output[2]) { - output[2] = true; - save_mesh(*mesh,*raster,"mesh-100000.ply"); - } else if(current_edge_count <= 10000 && !output[3]) { - output[3] = true; - save_mesh(*mesh,*raster,"mesh-10000.ply"); - } else if(current_edge_count <= 5000 && !output[4]) { - output[4] = true; - save_mesh(*mesh,*raster,"mesh-5000.ply"); - } else if(current_edge_count <= 1000000 && !output[5]) { - output[5] = true; - save_mesh(*mesh,*raster,"mesh-1000000.ply"); - } - } - - } - -}; - -std::tuple compute_meshes(const Raster &raster) { - - std::cout << "Terrain mesh" << std::endl; - Surface_mesh terrain_mesh; - // Add points - std::vector> terrain_vertex_index(raster.ySize, std::vector(raster.xSize, Surface_mesh::Vertex_index())); - for (int L = 0; L < raster.ySize; L++) { - for (int P = 0; P < raster.xSize; P++) { - terrain_vertex_index[L][P] = terrain_mesh.add_vertex(Point_3(0.5 + P, 0.5 + L, raster.dtm[L][P])); - } - } - std::cout << "Point added" << std::endl; - // Add faces - for (int L = 0; L < raster.ySize-1; L++) { - for (int P = 0; P < raster.xSize-1; P++) { - if (pow(raster.dtm[L][P]-raster.dtm[L+1][P+1], 2) < pow(raster.dtm[L+1][P]-raster.dtm[L][P+1], 2)) { - terrain_mesh.add_face(terrain_vertex_index[L][P], terrain_vertex_index[L+1][P+1], terrain_vertex_index[L+1][P]); - terrain_mesh.add_face(terrain_vertex_index[L][P], terrain_vertex_index[L][P+1], terrain_vertex_index[L+1][P+1]); - } else { - terrain_mesh.add_face(terrain_vertex_index[L][P], terrain_vertex_index[L][P+1], terrain_vertex_index[L+1][P]); - terrain_mesh.add_face(terrain_vertex_index[L][P+1], terrain_vertex_index[L+1][P+1], terrain_vertex_index[L+1][P]); - } - } - } - std::cout << "Faces added" << std::endl; +#include "header.hpp" +#include "raster.hpp" - save_mesh(terrain_mesh, raster, "initial-terrain-mesh.ply"); +void save_mesh(const Surface_mesh &mesh, const Raster &raster, const char *filename); - SMS::edge_collapse(terrain_mesh, Cost_stop_predicate(10)); - std::cout << "Terrain mesh simplified" << std::endl; +std::tuple compute_meshes(const Raster &raster); - save_mesh(terrain_mesh, raster, "terrain-mesh.ply"); +void add_label(const Raster &raster, Surface_mesh &mesh); - std::cout << "Surface mesh" << std::endl; - Surface_mesh mesh; - // Add points - std::vector> vertex_index(raster.ySize, std::vector(raster.xSize, Surface_mesh::Vertex_index())); - for (int L = 0; L < raster.ySize; L++) { - for (int P = 0; P < raster.xSize; P++) { - vertex_index[L][P] = mesh.add_vertex(Point_3(0.5 + P, 0.5 + L, raster.dsm[L][P])); - } - } - std::cout << "Point added" << std::endl; - // Add faces - for (int L = 0; L < raster.ySize-1; L++) { - for (int P = 0; P < raster.xSize-1; P++) { - if (pow(raster.dsm[L][P]-raster.dsm[L+1][P+1], 2) < pow(raster.dsm[L+1][P]-raster.dsm[L][P+1], 2)) { - mesh.add_face(vertex_index[L][P], vertex_index[L+1][P+1], vertex_index[L+1][P]); - mesh.add_face(vertex_index[L][P], vertex_index[L][P+1], vertex_index[L+1][P+1]); - } else { - mesh.add_face(vertex_index[L][P], vertex_index[L][P+1], vertex_index[L+1][P]); - mesh.add_face(vertex_index[L][P+1], vertex_index[L+1][P+1], vertex_index[L+1][P]); - } - } - } - std::cout << "Faces added" << std::endl; +void change_vertical_faces(Surface_mesh &mesh, const Raster &raster); - save_mesh(mesh, raster, "initial-mesh.ply"); +std::vector> compute_path(Surface_mesh &mesh); - Cost_stop_predicate stop(10); - //SMS::Count_stop_predicate stop(1000); - Custom_cost cf(raster); - Custom_placement pf(raster); - int r = SMS::edge_collapse(mesh, stop, CGAL::parameters::get_cost(cf).get_placement(pf).visitor(My_visitor(&mesh, &raster))); - std::cout << "\rMesh simplified " << std::endl; +std::map> compute_path_polygon(const Surface_mesh &mesh, const std::vector> &paths, const Raster &raster); - save_mesh(mesh, raster, "final-mesh.ply"); - return std::make_tuple(terrain_mesh, mesh); -} +std::map>> compute_medial_axes(const Surface_mesh &mesh, const std::vector> &paths, const std::map> &path_polygon, const Raster &raster); -void add_label(const Raster &raster, Surface_mesh &mesh) { - Surface_mesh::Property_map label; - bool created; - boost::tie(label, created) = mesh.add_property_map("label",0); - assert(created); - - for (auto face : mesh.faces()) { - int face_label[LABELS.size()] = {0}; - int sum_face_label = 0; - - CGAL::Vertex_around_face_iterator vbegin, vend; - boost::tie(vbegin, vend) = vertices_around_face(mesh.halfedge(face), mesh); - for (auto pixel : raster.triangle_to_pixel(mesh.point(*(vbegin++)), mesh.point(*(vbegin++)), mesh.point(*(vbegin++)))) { - if (raster.land_cover[pixel.second][pixel.first] > -1) { - sum_face_label++; - face_label[raster.land_cover[pixel.second][pixel.first]]++; - } - } - auto argmax = std::max_element(face_label, face_label+LABELS.size()); - label[face] = argmax - face_label; - } -} - -void change_vertical_faces(Surface_mesh &mesh, const Raster &raster) { - Surface_mesh::Property_map label; - bool has_label; - boost::tie(label, has_label) = mesh.property_map("label"); - assert(has_label); - - std::unordered_map new_label; - std::list remove_face; - - for (auto face : mesh.faces()) { - new_label[face] = label[face]; - CGAL::Vertex_around_face_iterator vbegin, vend; - boost::tie(vbegin, vend) = vertices_around_face(mesh.halfedge(face), mesh); - auto p0 = mesh.point(*(vbegin++)); - auto p1 = mesh.point(*(vbegin++)); - auto p2 = mesh.point(*(vbegin++)); - //TODO : use coords and not grid - float nz = ((-p0.x() + p1.x()) * (-p0.y() + p2.y()) - (-p0.x() + p2.x()) * (-p0.y() + p1.y())); - float surface = pow(K::Triangle_3(p0, p1, p2).squared_area(), 0.5); - if (abs(nz)/(2*surface) < 0.5) { - /*CGAL::Face_around_face_iterator fbegin, fend; - for(boost::tie(fbegin, fend) = faces_around_face(mesh.halfedge(face), mesh); fbegin != fend && new_label[face] != 4; ++fbegin) { - if (*fbegin != boost::graph_traits::null_face()) { - if (label[*fbegin] == 4) { - //Building - new_label[face] = 4; - } else if (label[*fbegin] == 5 && new_label[face] != 4) { - //High vegetation - new_label[face] = 5; - } - CGAL::Face_around_face_iterator fbegin2, fend2; - for(boost::tie(fbegin2, fend2) = faces_around_face(mesh.halfedge(*fbegin), mesh); fbegin2 != fend2 && new_label[face] != 4; ++fbegin2) { - if (*fbegin2 != boost::graph_traits::null_face()) { - if (label[*fbegin2] == 4) { - //Building - new_label[face] = 4; - } else if (label[*fbegin2] == 5 && new_label[face] != 4) { - //High vegetation - new_label[face] = 5; - } - } - } - } - }*/ - - int face_label[LABELS.size()] = {0}; - int sum_face_label = 0; - - CGAL::Vertex_around_face_iterator vbegin, vend; - boost::tie(vbegin, vend) = vertices_around_face(mesh.halfedge(face), mesh); - for (auto pixel : raster.triangle_to_pixel(mesh.point(*(vbegin++)), mesh.point(*(vbegin++)), mesh.point(*(vbegin++)))) { - if (raster.land_cover[pixel.second][pixel.first] > -1) { - sum_face_label++; - face_label[raster.land_cover[pixel.second][pixel.first]]++; - } - } - - if (face_label[4] > 0) { - //Building - new_label[face] = 4; - } else if (face_label[5] > 0) { - //High vegetation - new_label[face] = 5; - } - - if (new_label[face] != 4 && new_label[face] != 5) { - new_label[face] = 0; - } - } - } - - for (auto &face_label: new_label) { - label[face_label.first] = face_label.second; - } - -} - -void set_path(Surface_mesh &mesh, - Surface_mesh::Property_map &path, - Surface_mesh::Property_map &label, - std::vector> &paths, - Surface_mesh::Face_index face, - int path_id) { - path[face] = path_id; - paths[path_id].push_back(face); - CGAL::Face_around_face_iterator fbegin, fend; - for(boost::tie(fbegin, fend) = faces_around_face(mesh.halfedge(face), mesh); fbegin != fend; ++fbegin) { - if (*fbegin != boost::graph_traits::null_face()) { - if (label[face] == label[*fbegin] && path[*fbegin] == -1) { - set_path(mesh, path, label, paths, *fbegin, path_id); - } - } - } -} - -std::vector> compute_path(Surface_mesh &mesh) { - Surface_mesh::Property_map label; - bool has_label; - boost::tie(label, has_label) = mesh.property_map("label"); - assert(has_label); - - Surface_mesh::Property_map path; - bool created; - boost::tie(path, created) = mesh.add_property_map("path", -1); - assert(created); - - std::vector> paths; - - for (auto face: mesh.faces()) { - if (path[face] == -1) { - paths.push_back(std::list{}); - set_path(mesh, path, label, paths, face, paths.size()-1); - } - } - - return paths; -} - -template -CGAL::Polygon_with_holes_2 polygon(Face_handle face) { - - typedef typename Face_handle::value_type::Vertex::Point::R Kernel; - - CGAL::Polygon_2 border; - auto curr = face->outer_ccb(); - do { - auto p = curr->target()->point(); - border.push_back(p); - } while (++curr != face->outer_ccb()); - - std::list> holes; - for(auto hole = face->holes_begin(); hole != face->holes_end(); hole++) { - auto curr = *hole; - CGAL::Polygon_2 hole_polygon; - do { - auto p = curr->target()->point(); - hole_polygon.push_back(p); - } while (++curr != *hole); - holes.push_back(hole_polygon); - } - - return CGAL::Polygon_with_holes_2(border, holes.begin(), holes.end()); -} - - -std::map> compute_path_polygon(const Surface_mesh &mesh, const std::vector> &paths, const Raster &raster) { - Surface_mesh::Property_map path; - bool has_path; - boost::tie(path, has_path) = mesh.property_map("path"); - assert(has_path); - - Surface_mesh::Property_map label; - bool has_label; - boost::tie(label, has_label) = mesh.property_map("label"); - assert(has_label); - - std::map> path_polygon; - - typedef CGAL::Face_filtered_graph Filtered_graph; - for (int i = 0; i < paths.size(); i++) { - - Filtered_graph filtered_sm(mesh, i, path); - - if (filtered_sm.number_of_faces() > 3) { - - int lab = label[*(CGAL::faces(filtered_sm).first)]; - if (lab == 3 || lab == 8 || lab == 9) { - - Arrangement_2 arr; - std::map point_map; - for (auto edge: CGAL::edges(filtered_sm)) { - if (CGAL::is_border (edge, filtered_sm)) { - auto p0 = mesh.point(CGAL::source(edge, filtered_sm)); - auto p1 = mesh.point(CGAL::target(edge, filtered_sm)); - insert_non_intersecting_curve(arr, Traits_2::X_monotone_curve_2(Exact_predicates_kernel::Point_2(p0.x(), p0.y()), Exact_predicates_kernel::Point_2(p1.x(), p1.y()))); - } - } - - path_polygon[i] = polygon((*(arr.unbounded_face()->holes_begin()))->twin()->face()); - - { // Arrangement - Surface_mesh skeleton; - - std::map v_map; - for (auto v = arr.vertices_begin(); v != arr.vertices_end(); v++) { - auto p = v->point(); - auto z = raster.dsm[int(p.y())][int(p.x())]; - v_map[v] = skeleton.add_vertex(Point_3((float) p.x(), (float) p.y(), z)); - } - - for (auto he = arr.edges_begin(); he != arr.edges_end(); ++he ) { - skeleton.add_edge(v_map[he->source()], v_map[he->target()]); - } - - Surface_mesh::Property_map edge_prop; - bool created; - boost::tie(edge_prop, created) = skeleton.add_property_map("prop",0); - assert(created); - - double min_x, min_y; - raster.grid_to_coord(0, 0, min_x, min_y); - - for(auto vertex : skeleton.vertices()) { - auto point = skeleton.point(vertex); - double x, y; - raster.grid_to_coord((float) point.x(), (float) point.y(), x, y); - skeleton.point(vertex) = Point_3(x-min_x, y-min_y, point.z()); - } - - std::stringstream skeleton_name; - skeleton_name << "arr_" << lab << "_" << i << ".ply"; - std::ofstream mesh_ofile (skeleton_name.str().c_str()); - CGAL::IO::write_PLY (mesh_ofile, skeleton); - mesh_ofile.close(); - } - - Surface_mesh part_mesh; - CGAL::copy_face_graph(filtered_sm, part_mesh); - std::stringstream name; - name << "part_mesh_" << lab << "_" << i << ".ply"; - save_mesh(part_mesh, raster, name.str().c_str()); - - } - } - } - - return path_polygon; - -} - - -std::map>> compute_medial_axes(const Surface_mesh &mesh, const std::vector> &paths, const std::map> &path_polygon, const Raster &raster) { - Surface_mesh::Property_map label; - bool has_label; - boost::tie(label, has_label) = mesh.property_map("label"); - assert(has_label); - - std::map>> medial_axes; - - for (int i = 0; i < paths.size(); i++) { - - if (path_polygon.count(i) > 0) { - - int lab = label[paths.at(i).front()]; - - auto poly = path_polygon.at(i); - - poly = CGAL::Polyline_simplification_2::simplify( - poly, - CGAL::Polyline_simplification_2::Squared_distance_cost(), - CGAL::Polyline_simplification_2::Stop_above_cost_threshold(pow(raster.coord_distance_to_grid_distance(1.5),2)) - ); - - auto iss = CGAL::create_interior_straight_skeleton_2(poly, Exact_predicates_kernel()); - medial_axes[i] = CGAL::convert_straight_skeleton_2>(*iss); - - { // Skeleton - Surface_mesh skeleton; - - std::map v_map; - for (auto v = iss->vertices_begin(); v != iss->vertices_end(); v++) { - auto p = v->point(); - auto z = raster.dsm[int(p.y())][int(p.x())]; - v_map[v->id()] = skeleton.add_vertex(Point_3((float) p.x(), (float) p.y(), z)); - } - - for (auto he = iss->halfedges_begin(); he != iss->halfedges_end(); ++he ) { - skeleton.add_edge(v_map[he->vertex()->id()], v_map[he->opposite()->vertex()->id()]); - } - - Surface_mesh::Property_map edge_prop; - bool created; - boost::tie(edge_prop, created) = skeleton.add_property_map("prop",0); - assert(created); - - double min_x, min_y; - raster.grid_to_coord(0, 0, min_x, min_y); - - for(auto vertex : skeleton.vertices()) { - auto point = skeleton.point(vertex); - double x, y; - raster.grid_to_coord((float) point.x(), (float) point.y(), x, y); - skeleton.point(vertex) = Point_3(x-min_x, y-min_y, point.z()); - } - - std::stringstream skeleton_name; - skeleton_name << "skeleton_" << lab << "_" << i << ".ply"; - std::ofstream mesh_ofile (skeleton_name.str().c_str()); - CGAL::IO::write_PLY (mesh_ofile, skeleton); - mesh_ofile.close(); - } - - { // Path - Surface_mesh skeleton; - - std::map v_map; - for (auto v = iss->vertices_begin(); v != iss->vertices_end(); v++) { - if (v->is_skeleton()) { - auto p = v->point(); - auto z = raster.dsm[int(p.y())][int(p.x())]; - v_map[v->id()] = skeleton.add_vertex(Point_3((float) p.x(), (float) p.y(),z)); - } - } - - for (auto he = iss->halfedges_begin(); he != iss->halfedges_end(); ++he ) { - if (he->is_inner_bisector()) { - auto v0 = v_map.find(he->vertex()->id()); - auto v1 = v_map.find(he->opposite()->vertex()->id()); - if (v0 != v_map.end() && v1 != v_map.end() && v0->second != v1->second) { - skeleton.add_edge(v_map[he->vertex()->id()], v_map[he->opposite()->vertex()->id()]); - } - } - } - - bool created; - Surface_mesh::Property_map edge_prop; - boost::tie(edge_prop, created) = skeleton.add_property_map("prop",0); - assert(created); - Surface_mesh::Property_map red; - boost::tie(red, created) = skeleton.add_property_map("red", LABELS[lab].red); - assert(created); - Surface_mesh::Property_map green; - boost::tie(green, created) = skeleton.add_property_map("green", LABELS[lab].green); - assert(created); - Surface_mesh::Property_map blue; - boost::tie(blue, created) = skeleton.add_property_map("blue", LABELS[lab].blue); - assert(created); - - double min_x, min_y; - raster.grid_to_coord(0, 0, min_x, min_y); - - for(auto vertex : skeleton.vertices()) { - auto point = skeleton.point(vertex); - double x, y; - raster.grid_to_coord((float) point.x(), (float) point.y(), x, y); - skeleton.point(vertex) = Point_3(x-min_x, y-min_y, point.z()); - } - - std::stringstream skeleton_name; - skeleton_name << "path_" << lab << "_" << i << ".ply"; - std::ofstream mesh_ofile (skeleton_name.str().c_str()); - CGAL::IO::write_PLY (mesh_ofile, skeleton); - mesh_ofile.close(); - } - - } - } - - return medial_axes; - -} - -/// A point on a skeleton -struct skeletonPoint { - /// The skeleton id - int path; - /// The point coordinate - Point_2 point; - /// A handle to the vertex if the point is a vertex - CGAL::Straight_skeleton_2::Vertex_handle vertex; - /// Or a handle to the edge otherwise - CGAL::Straight_skeleton_2::Halfedge_handle halfedge; - - skeletonPoint () {} - - skeletonPoint (int _path, CGAL::Straight_skeleton_2::Vertex_handle _vertex) { - path = _path; - vertex = _vertex; - point = _vertex->point(); - halfedge = nullptr; - } - - skeletonPoint (int _path, CGAL::Straight_skeleton_2::Halfedge_handle _halfedge, K::Point_2 _point) { - path = _path; - halfedge = _halfedge; - point = _point; - vertex = nullptr; - } - - friend bool operator<(const skeletonPoint& l, const skeletonPoint& r) { - if (l.path == r.path) return l.point < r.point; - return l.path < r.path; - } - - friend bool operator==(const skeletonPoint& l, const skeletonPoint& r) { - return (l.path == r.path && l.point == r.point); - } -}; - -std::set> link_paths(const Surface_mesh &mesh, const std::vector> &paths, const std::map> &path_polygon, const std::map>> &medial_axes, const Raster &raster) { - - // Get label property - Surface_mesh::Property_map label; - bool has_label; - boost::tie(label, has_label) = mesh.property_map("label"); - assert(has_label); - - std::set> result; - - for (int selected_label: {3, 8, 9}) { - // List path with selected label - std::list same_label_paths; - for (int i = 0; i < paths.size(); i++) { - if (label[paths[i].front()] == selected_label && medial_axes.count(i) == 1) { - same_label_paths.push_back(i); - } - } - - for (int path1: same_label_paths) { - for (int path2: same_label_paths) { - if (path1 < path2) { - - std::map::Vertex_handle, CGAL::Straight_skeleton_2::Vertex_handle>, K::FT> distance_v1v2; - std::map::Vertex_handle, CGAL::Straight_skeleton_2::Halfedge_handle>, std::pair> distance_v1h2; - std::map::Vertex_handle, CGAL::Straight_skeleton_2::Halfedge_handle>, std::pair> distance_v2h1; - - // For vertices pairs - for (auto v1: medial_axes.at(path1)->vertex_handles()) { - if (v1->is_skeleton()) { - for (auto v2: medial_axes.at(path2)->vertex_handles()) { - if (v2->is_skeleton()) { - distance_v1v2[std::make_pair(v1, v2)] = CGAL::squared_distance(v1->point(), v2->point()); - } - } - } - } - - // For vertex on path1 and edge on path2 - for (auto v1: medial_axes.at(path1)->vertex_handles()) { - if (v1->is_skeleton()) { - for (auto edge2: medial_axes.at(path2)->halfedge_handles()) { - if (edge2->vertex()->id() < edge2->opposite()->vertex()->id() && edge2->is_inner_bisector() && edge2->opposite()->is_inner_bisector()) { - auto segment = K::Segment_2(edge2->opposite()->vertex()->point(), edge2->vertex()->point()); - auto proj = segment.supporting_line().projection(v1->point()); - if (segment.collinear_has_on(proj)) { - distance_v1h2[std::make_pair(v1, edge2)] = std::make_pair(CGAL::squared_distance(v1->point(), proj), proj); - } - } - } - } - } - - // For vertex on path2 and edge on path1 - for (auto v2: medial_axes.at(path2)->vertex_handles()) { - if (v2->is_skeleton()) { - for (auto edge1: medial_axes.at(path1)->halfedge_handles()) { - if (edge1->vertex()->id() < edge1->opposite()->vertex()->id() && edge1->is_inner_bisector() && edge1->opposite()->is_inner_bisector()) { - auto segment = K::Segment_2(edge1->opposite()->vertex()->point(), edge1->vertex()->point()); - auto proj = segment.supporting_line().projection(v2->point()); - if (segment.collinear_has_on(proj)) { - distance_v2h1[std::make_pair(v2, edge1)] = std::make_pair(CGAL::squared_distance(v2->point(), proj), proj); - } - } - } - } - } - - // For vertices pairs - for (auto it = distance_v1v2.begin(); it != distance_v1v2.end(); ++it) { - auto v1 = it->first.first; - auto v2 = it->first.second; - auto d = it->second; - - auto he = v1->halfedge_around_vertex_begin(); - do { - auto v = (*he)->opposite()->vertex(); - if (v->is_skeleton()) { - if (distance_v1v2[std::make_pair(v, v2)] < d) { - goto exit1; - } - if ((*he)->vertex()->id() < (*he)->opposite()->vertex()->id()) { - if (distance_v2h1.count(std::make_pair(v2, *he)) > 0 && distance_v2h1[std::make_pair(v2, *he)].first < d) { - goto exit1; - } - } else { - if (distance_v2h1.count(std::make_pair(v2, (*he)->opposite())) > 0 && distance_v2h1[std::make_pair(v2, (*he)->opposite())].first < d) { - goto exit1; - } - } - } - } while (++he != v1->halfedge_around_vertex_begin()); - - he = v2->halfedge_around_vertex_begin(); - do { - auto v = (*he)->opposite()->vertex(); - if (v->is_skeleton()) { - if (distance_v1v2[std::make_pair(v1, v)] < d) { - goto exit1; - } - if ((*he)->vertex()->id() < (*he)->opposite()->vertex()->id()) { - if (distance_v1h2.count(std::make_pair(v1, *he)) > 0 && distance_v1h2[std::make_pair(v1, *he)].first < d) { - goto exit1; - } - } else { - if (distance_v1h2.count(std::make_pair(v1, (*he)->opposite())) > 0 && distance_v1h2[std::make_pair(v1, (*he)->opposite())].first < d) { - goto exit1; - } - } - } - } while (++he != v2->halfedge_around_vertex_begin()); - - result.insert(std::make_pair(skeletonPoint(path1, v1), skeletonPoint(path2, v2))); - exit1: - continue; - } - - // For vertex on path1 and edge on path2 - for (auto it = distance_v1h2.begin(); it != distance_v1h2.end(); ++it) { - auto v1 = it->first.first; - auto e2 = it->first.second; - auto d = it->second.first; - auto p2 = it->second.second; - - auto he = v1->halfedge_around_vertex_begin(); - do { - auto v = (*he)->opposite()->vertex(); - if (v->is_skeleton()) { - if (CGAL::squared_distance(v->point(), p2) < d) { - goto exit2; - } - auto segment = K::Segment_2((*he)->opposite()->vertex()->point(), (*he)->vertex()->point()); - auto proj = segment.supporting_line().projection(p2); - if (segment.collinear_has_on(proj)) { - goto exit2; - } - } - } while (++he != v1->halfedge_around_vertex_begin()); - - result.insert(std::make_pair(skeletonPoint(path1, v1), skeletonPoint(path2, e2, p2))); - exit2: - continue; - } - - // For vertex on path2 and edge on path1 - for (auto it = distance_v2h1.begin(); it != distance_v2h1.end(); ++it) { - auto v2 = it->first.first; - auto e1 = it->first.second; - auto d = it->second.first; - auto p1 = it->second.second; - - auto he = v2->halfedge_around_vertex_begin(); - do { - auto v = (*he)->opposite()->vertex(); - if (v->is_skeleton()) { - if (CGAL::squared_distance(v->point(), p1) < d) { - goto exit3; - } - auto segment = K::Segment_2((*he)->opposite()->vertex()->point(), (*he)->vertex()->point()); - auto proj = segment.supporting_line().projection(p1); - if (segment.collinear_has_on(proj)) { - goto exit3; - } - } - } while (++he != v2->halfedge_around_vertex_begin()); - - result.insert(std::make_pair(skeletonPoint(path2, v2), skeletonPoint(path1, e1, p1))); - exit3: - continue; - } - - } else if (path1 == path2) { - - std::map::Vertex_handle, CGAL::Straight_skeleton_2::Vertex_handle>, K::FT> distance_vv; - std::map::Vertex_handle, CGAL::Straight_skeleton_2::Halfedge_handle>, std::pair> distance_vh; - - for (auto v1: medial_axes.at(path1)->vertex_handles()) { - if (v1->is_skeleton()) { - // For vertices pairs - for (auto v2: medial_axes.at(path1)->vertex_handles()) { - if (v2->is_skeleton()) { - distance_vv[std::make_pair(v1, v2)] = CGAL::squared_distance(v1->point(), v2->point()); - } - } - // For vertex and edge - for (auto edge2: medial_axes.at(path1)->halfedge_handles()) { - if (edge2->vertex()->id() < edge2->opposite()->vertex()->id() && edge2->is_inner_bisector() && edge2->opposite()->is_inner_bisector()) { - auto segment = K::Segment_2(edge2->opposite()->vertex()->point(), edge2->vertex()->point()); - auto proj = segment.supporting_line().projection(v1->point()); - if (segment.collinear_has_on(proj)) { - distance_vh[std::make_pair(v1, edge2)] = std::make_pair(CGAL::squared_distance(v1->point(), proj), proj); - } - } - } - } - } - - // For vertices pairs - for (auto it = distance_vv.begin(); it != distance_vv.end(); ++it) { - auto v1 = it->first.first; - auto v2 = it->first.second; - auto d = it->second; - - if (v1->id() == v2->id()) continue; - - // Exit link - Exact_predicates_kernel::Segment_2 segment(Exact_predicates_kernel::Point_2(v1->point().x(), v1->point().y()), Exact_predicates_kernel::Point_2(v2->point().x(), v2->point().y())); - bool intersect = false; - for (auto edge = path_polygon.at(path1).outer_boundary().edges_begin(); !intersect && edge != path_polygon.at(path1).outer_boundary().edges_end(); edge++) { - if (CGAL::do_intersect(*edge, segment)) { - intersect = true; - } - } - for (auto hole: path_polygon.at(path1).holes()) { - for (auto edge = hole.edges_begin(); !intersect && edge != hole.edges_end(); edge++) { - if (CGAL::do_intersect(*edge, segment)) { - intersect = true; - } - } - } - if (!intersect) continue; - - auto he = v1->halfedge_around_vertex_begin(); - do { - auto v = (*he)->opposite()->vertex(); - if (v->is_skeleton()) { - if (distance_vv[std::make_pair(v, v2)] < d) { - goto exit4; - } - if ((*he)->vertex()->id() < (*he)->opposite()->vertex()->id()) { - if (distance_vh.count(std::make_pair(v2, *he)) > 0 && distance_vh[std::make_pair(v2, *he)].first < d) { - goto exit4; - } - } else { - if (distance_vh.count(std::make_pair(v2, (*he)->opposite())) > 0 && distance_vh[std::make_pair(v2, (*he)->opposite())].first < d) { - goto exit4; - } - } - } - } while (++he != v1->halfedge_around_vertex_begin()); - - he = v2->halfedge_around_vertex_begin(); - do { - auto v = (*he)->opposite()->vertex(); - if (v->is_skeleton()) { - if (distance_vv[std::make_pair(v1, v)] < d) { - goto exit4; - } - if ((*he)->vertex()->id() < (*he)->opposite()->vertex()->id()) { - if (distance_vh.count(std::make_pair(v1, *he)) > 0 && distance_vh[std::make_pair(v1, *he)].first < d) { - goto exit4; - } - } else { - if (distance_vh.count(std::make_pair(v1, (*he)->opposite())) > 0 && distance_vh[std::make_pair(v1, (*he)->opposite())].first < d) { - goto exit4; - } - } - } - } while (++he != v2->halfedge_around_vertex_begin()); - - result.insert(std::make_pair(skeletonPoint(path1, v1), skeletonPoint(path1, v2))); - exit4: - continue; - } - - // For vertex on path1 and edge on path2 - for (auto it = distance_vh.begin(); it != distance_vh.end(); ++it) { - auto v1 = it->first.first; - auto e2 = it->first.second; - auto d = it->second.first; - auto p2 = it->second.second; - - if (v1->id() == e2->vertex()->id() || v1->id() == e2->opposite()->vertex()->id()) continue; - - // Exit link - Exact_predicates_kernel::Segment_2 segment(Exact_predicates_kernel::Point_2(v1->point().x(), v1->point().y()), Exact_predicates_kernel::Point_2(p2.x(), p2.y())); - bool intersect = false; - for (auto edge = path_polygon.at(path1).outer_boundary().edges_begin(); !intersect && edge != path_polygon.at(path1).outer_boundary().edges_end(); edge++) { - if (CGAL::do_intersect(*edge, segment)) { - intersect = true; - } - } - for (auto hole: path_polygon.at(path1).holes()) { - for (auto edge = hole.edges_begin(); !intersect && edge != hole.edges_end(); edge++) { - if (CGAL::do_intersect(*edge, segment)) { - intersect = true; - } - } - } - if (!intersect) continue; - - auto he = v1->halfedge_around_vertex_begin(); - do { - auto v = (*he)->opposite()->vertex(); - if (v->is_skeleton()) { - if (CGAL::squared_distance(v->point(), p2) < d) { - goto exit5; - } - auto segment = K::Segment_2((*he)->opposite()->vertex()->point(), (*he)->vertex()->point()); - auto proj = segment.supporting_line().projection(p2); - if (segment.collinear_has_on(proj)) { - goto exit5; - } - } - } while (++he != v1->halfedge_around_vertex_begin()); - - result.insert(std::make_pair(skeletonPoint(path1, v1), skeletonPoint(path1, e2, p2))); - exit5: - continue; - } - } - } - } - } - - Surface_mesh links; - - for(auto link: result) { - auto z1 = raster.dsm[int(link.first.point.y())][int(link.first.point.x())]; - auto z2 = raster.dsm[int(link.second.point.y())][int(link.second.point.x())]; - auto v1 = links.add_vertex(Point_3((float) link.first.point.x(), (float) link.first.point.y(), z1)); - auto v2 = links.add_vertex(Point_3((float) link.second.point.x(), (float) link.second.point.y(), z2)); - links.add_edge(v1,v2); - } - - bool created; - Surface_mesh::Property_map edge_prop; - boost::tie(edge_prop, created) = links.add_property_map("prop",0); - assert(created); - - double min_x, min_y; - raster.grid_to_coord(0, 0, min_x, min_y); - - for(auto vertex : links.vertices()) { - auto point = links.point(vertex); - double x, y; - raster.grid_to_coord((float) point.x(), (float) point.y(), x, y); - links.point(vertex) = Point_3(x-min_x, y-min_y, point.z()); - } - - std::ofstream mesh_ofile ("links.ply"); - CGAL::IO::write_PLY (mesh_ofile, links); - mesh_ofile.close(); - - return result; - -} +std::set> link_paths(const Surface_mesh &mesh, const std::vector> &paths, const std::map> &path_polygon, const std::map>> &medial_axes, const Raster &raster); void add_road( std::list> &roads, diff --git a/raster.cpp b/raster.cpp new file mode 100644 index 0000000..356f7bb --- /dev/null +++ b/raster.cpp @@ -0,0 +1,166 @@ +#include "raster.hpp" + +std::pair Raster::grid_conversion(int P, int L, double grid_to_crs[6], OGRCoordinateTransformation *crs_to_other_crs, double other_grid_to_other_crs[6]) { + double x = grid_to_crs[0] + (0.5 + P)*grid_to_crs[1] + (0.5 + L)*grid_to_crs[2]; + double y = grid_to_crs[3] + (0.5 + P)*grid_to_crs[4] + (0.5 + L)*grid_to_crs[5]; + crs_to_other_crs->Transform(1,&x,&y); + double fact = other_grid_to_other_crs[1]*other_grid_to_other_crs[5] - other_grid_to_other_crs[2]*other_grid_to_other_crs[4]; + x -= other_grid_to_other_crs[0]; + y -= other_grid_to_other_crs[3]; + int newP = ((int) ((other_grid_to_other_crs[5]*x - other_grid_to_other_crs[2]*y) / fact)); + int newL = ((int) ((-other_grid_to_other_crs[4]*x + other_grid_to_other_crs[1]*y) / fact)); + return std::pair(newP, newL); +} + +void Raster::align_land_cover_and_dsm() { + std::vector> new_land_cover = std::vector>(ySize, std::vector(xSize, -1)); + for (int L = 0; L < ySize; L++) { + for (int P = 0; P < xSize; P++) { + float neighboors_count[LABELS.size()] = {0}; + int n = 0; + float moy = 0; + float square_moy = 0; + for (int i = -5; i <= 5; i++) { + for (int j = -5; j <= 5; j++) { + if (L+i >= 0 && P + j >= 0 && L+i < ySize && P+j < xSize) { + neighboors_count[land_cover[L+i][P+j]] += 1/(0.1+abs(dsm[L][P] - dsm[L+i][P+j])); + if (i > -3 && i < 3 && j > -3 && j < 3) { + n++; + moy += dsm[L+i][P+j]; + square_moy += pow(dsm[L+i][P+j], 2); + } + } + } + } + if (pow(square_moy/n - pow(moy/n, 2), 0.5) > 0.2) { + new_land_cover[L][P] = std::max_element(neighboors_count, neighboors_count + LABELS.size()) - neighboors_count; + } else { + new_land_cover[L][P] = land_cover[L][P]; + } + + } + } + + land_cover = new_land_cover; +} + +void Raster::coord_to_grid(double x, double y, float& P, float& L) const { + double fact = grid_to_crs[1]*grid_to_crs[5] - grid_to_crs[2]*grid_to_crs[4]; + x -= grid_to_crs[0]; + y -= grid_to_crs[3]; + P = (grid_to_crs[5]*x - grid_to_crs[2]*y) / fact; + L = (-grid_to_crs[4]*x + grid_to_crs[1]*y) / fact; +} + +void Raster::grid_to_coord(int P, int L, double& x, double& y) const { + x = grid_to_crs[0] + (0.5 + P)*grid_to_crs[1] + (0.5 + L)*grid_to_crs[2]; + y = grid_to_crs[3] + (0.5 + P)*grid_to_crs[4] + (0.5 + L)*grid_to_crs[5]; +} + +void Raster::grid_to_coord(float P, float L, double& x, double& y) const { + x = grid_to_crs[0] + P*grid_to_crs[1] + L*grid_to_crs[2]; + y = grid_to_crs[3] + P*grid_to_crs[4] + L*grid_to_crs[5]; +} + +float Raster::coord_distance_to_grid_distance(double d) const { + float P, L; + double x, y; + grid_to_coord(0, 0, x, y); + coord_to_grid(x+d/sqrt(2),y+d/sqrt(2),P,L); + return sqrt(pow(P,2)+pow(L,2)); +} + +double Raster::grid_distance_to_coord_distance(float d) const { + double x1, y1, x2, y2; + grid_to_coord(0, 0, x1, y1); + grid_to_coord((float) (d/sqrt(2)), (float) (d/sqrt(2)), x2, y2); + return sqrt(pow(x2-x1,2)+pow(y2-y1,2)); +} + +OGRSpatialReference Raster::get_crs() const { + return OGRSpatialReference(crs); +} + +Raster::Raster(char *dsm_path, char *dtm_path, char *land_cover_path) { + GDALAllRegister(); + + // Get DSM informations and CRS + GDALDataset *dsm_dataset = (GDALDataset *) GDALOpen(dsm_path, GA_ReadOnly ); + if( dsm_dataset == NULL ) { + throw std::invalid_argument(std::string("Unable to open ") + dsm_path + "."); + } + xSize = dsm_dataset->GetRasterBand(1)->GetXSize(); + ySize = dsm_dataset->GetRasterBand(1)->GetYSize(); + crs = *dsm_dataset->GetSpatialRef(); + if (dsm_dataset->GetGeoTransform(grid_to_crs) >= CE_Failure) { + throw std::invalid_argument(std::string(dsm_path) + " do not contain an affine transform."); + } + dsm = std::vector>(ySize, std::vector(xSize, 0)); + for (int L = 0; L < ySize; L++) { + if (dsm_dataset->GetRasterBand(1)->RasterIO(GF_Read, 0, L, xSize, 1, &dsm[L][0], xSize, 1, GDT_Float32, 0, 0) >= CE_Failure) { + throw std::invalid_argument(std::string(dsm_path) + " can't be read."); + } + } + std::cout << "DSM load" << std::endl; + + // Get DTM informations and CRS transform + dtm = std::vector>(ySize, std::vector(xSize, 0)); + GDALDataset *dtm_dataset = (GDALDataset *) GDALOpen( dtm_path, GA_ReadOnly ); + if( dtm_dataset == NULL ) { + throw std::invalid_argument(std::string("Unable to open ") + dtm_path + "."); + } + double dtm_grid_to_dtm_crs[6]; + if (dtm_dataset->GetGeoTransform(dtm_grid_to_dtm_crs) >= CE_Failure) { + throw std::invalid_argument("Can't transform DTM grid to DTM CRS."); + } + OGRCoordinateTransformation *CRS_to_dtm_crs = OGRCreateCoordinateTransformation( + &crs, + dtm_dataset->GetSpatialRef()); + if (CRS_to_dtm_crs == NULL) { + throw std::runtime_error("Can't transform DSM CRS to DTM CRS."); + } + for (int L = 0; L < ySize; L++) { + for (int P = 0; P < xSize; P++) { + std::pair new_coord = grid_conversion(P, L, grid_to_crs, CRS_to_dtm_crs, dtm_grid_to_dtm_crs); + if (new_coord.first >= 0 && new_coord.first < dtm_dataset->GetRasterBand(1)->GetXSize() && new_coord.second >= 0 && new_coord.second < dtm_dataset->GetRasterBand(1)->GetYSize()) { + if (dtm_dataset->GetRasterBand(1)->RasterIO(GF_Read, new_coord.first, new_coord.second, 1, 1, &dtm[L][P], 1, 1, GDT_Float32, 0, 0) >= CE_Failure) { + throw std::invalid_argument(std::string(dtm_path) + " can't be read."); + } + } + } + } + std::cout << "DTM load" << std::endl; + + // Get land cover informations and CRS transform + land_cover = std::vector>(ySize, std::vector(xSize, -1)); + GDALDataset *land_cover_dataset = (GDALDataset *) GDALOpen( land_cover_path, GA_ReadOnly ); + if( dsm_dataset == NULL ) { + throw std::invalid_argument(std::string("Unable to open ") + land_cover_path + "."); + } + + double land_cover_grid_to_land_cover_crs[6]; + if (land_cover_dataset->GetGeoTransform(land_cover_grid_to_land_cover_crs) >= CE_Failure) { + throw std::invalid_argument("Can't transform land cover grid to land cover CRS."); + } + OGRCoordinateTransformation *CRS_to_land_cover_crs = OGRCreateCoordinateTransformation( + &crs, + land_cover_dataset->GetSpatialRef()); + if (CRS_to_land_cover_crs == NULL) { + throw std::runtime_error("Can't transform DSM CRS to land cover CRS."); + } + for (int L = 0; L < ySize; L++) { + for (int P = 0; P < xSize; P++) { + std::pair new_coord = grid_conversion(P, L, grid_to_crs, CRS_to_land_cover_crs, land_cover_grid_to_land_cover_crs); + if (new_coord.first >= 0 && new_coord.first < land_cover_dataset->GetRasterBand(1)->GetXSize() && new_coord.second >= 0 && new_coord.second < land_cover_dataset->GetRasterBand(1)->GetYSize()) { + unsigned char value; + if (land_cover_dataset->GetRasterBand(1)->RasterIO(GF_Read, new_coord.first, new_coord.second, 1, 1, &value, 1, 1, GDT_Byte, 0, 0) >= CE_Failure) { + throw std::invalid_argument(std::string(land_cover_path) + " can't be read."); + } + land_cover[L][P] = (char) value; + } + } + } + std::cout << "Land cover load" << std::endl; + + align_land_cover_and_dsm(); +} diff --git a/raster.hpp b/raster.hpp new file mode 100644 index 0000000..7c9c8cc --- /dev/null +++ b/raster.hpp @@ -0,0 +1,56 @@ +#ifndef RASTER_H_ +#define RASTER_H_ + +#include "header.hpp" + +class Raster { + public: + std::vector> dsm; + std::vector> dtm; + std::vector> land_cover; + int xSize; + int ySize; + + private: + OGRSpatialReference crs; + double grid_to_crs[6] = {0,1,0,0,0,1}; + + static std::pair grid_conversion(int P, int L, double grid_to_crs[6], OGRCoordinateTransformation *crs_to_other_crs, double other_grid_to_other_crs[6]); + + void align_land_cover_and_dsm(); + + public: + void coord_to_grid(double x, double y, float& P, float& L) const; + + void grid_to_coord(int P, int L, double& x, double& y) const; + + void grid_to_coord(float P, float L, double& x, double& y) const; + + float coord_distance_to_grid_distance(double d) const; + + double grid_distance_to_coord_distance(float d) const; + + OGRSpatialReference get_crs() const; + + template + std::list> triangle_to_pixel(CGAL::Point_3 a, CGAL::Point_3 b, CGAL::Point_3 c) const { + std::list> ret; + int min_x = std::max((int) std::min({a.x(), b.x(), c.x()}), 0); + int max_x = std::min((int) std::max({a.x(), b.x(), c.x()}), xSize-1); + int min_y = std::max((int) std::min({a.y(), b.y(), c.y()}), 0); + int max_y = std::min((int) std::max({a.y(), b.y(), c.y()}), ySize-1); + K::Triangle_2 triangle (Point_2(a.x(), a.y()), Point_2(b.x(), b.y()), Point_2(c.x(), c.y())); + for (int L = min_y; L <= max_y; L++) { + for (int P = min_x; P <= max_x; P++) { + if (triangle.bounded_side(Point_2(0.5 + P, 0.5 + L)) != CGAL::ON_UNBOUNDED_SIDE) { + ret.push_back(std::pair(P,L)); + } + } + } + return ret; + } + + Raster(char *dsm_path, char *dtm_path, char *land_cover_path); +}; + +#endif /* !RASTER_H_ */