Merge pull request #6 from EasyNavigation/imp_surf

Imp surf

Author: fmrico

navmap_ros/include/navmap_ros/conversions.hpp

@@ -193,6 +193,9 @@ struct BuildParams
   /** @brief Maximum allowed edge length (meters) when forming triangles. */
   float max_edge_len = 2.0;
 
+  /** @brief Maximum allowed vertical jump (meters) between vertices of a triangle. */
+  float max_dz = 0.25f;
+
   /** @brief Maximum slope with respect to the vertical axis (degrees). */
   float max_slope_deg = 30.0f;   // maximum slope w.r.t. vertical
 

navmap_ros/src/navmap_ros/conversions.cpp

@@ -1089,43 +1089,250 @@ std::vector<Triangle> grow_surface_from_seed(
 
 
 navmap::NavMap from_points(
-  const  pcl::PointCloud<pcl::PointXYZ> & input_points,
+  const pcl::PointCloud<pcl::PointXYZ> & input_points,
   navmap_ros_interfaces::msg::NavMap & out_msg,
   BuildParams params)
 {
-  auto downsampled_points = downsample_voxelize_avgXYZ(input_points, params.resolution);
+  // --- Overview ------------------------------------------------------------
+  // 1) Optional voxel downsampling in XY (averaging XYZ).
+  // 2) 2D Delaunay triangulation in the XY plane (no crossing edges by construction).
+  // 3) Lift triangles back to 3D and filter by:
+  //    - area bounds,
+  //    - maximum edge length,
+  //    - maximum vertical discontinuity per edge (max_dz),
+  //    - maximum slope w.r.t. +Z (max_slope_deg).
+  // 4) Emit a single-surface NavMap and mirror it into out_msg.
+  // -------------------------------------------------------------------------
+
+  using std::vector;
+
+  if (input_points.empty() || input_points.size() < 3) {
+    out_msg = navmap_ros_interfaces::msg::NavMap();
+    return navmap::NavMap();
+  }
 
-  pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
-  kdtree.setInputCloud(downsampled_points.makeShared());
+  pcl::PointCloud<pcl::PointXYZ> cloud;
+  if (params.resolution > 0.0f) {
+    cloud = downsample_voxelize_avgXYZ(input_points, params.resolution);
+  } else {
+    cloud = input_points;
+  }
+  if (cloud.size() < 3) {
+    out_msg = navmap_ros_interfaces::msg::NavMap();
+    return navmap::NavMap();
+  }
 
-  pcl::PointXYZ origin(params.seed.x(), params.seed.y(), params.seed.z());
+  struct Pt2 { double x, y; int idx3d; };
+  struct Tri2 { int a, b, c; };
 
-  std::vector<int> nbr_indices;
-  std::vector<float> nbr_dists;
+  auto orient2d = [](const Pt2 & a, const Pt2 & b, const Pt2 & c) -> double {
+      return (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x);
+    };
 
-  nbr_indices.clear(); nbr_dists.clear();
-  bool found = kdtree.radiusSearch(origin, params.resolution, nbr_indices, nbr_dists);
+  auto in_circumcircle = [&](const Pt2 & p,
+    const Pt2 & A, const Pt2 & B, const Pt2 & C) -> bool
+    {
+      const double ax = A.x - p.x, ay = A.y - p.y;
+      const double bx = B.x - p.x, by = B.y - p.y;
+      const double cx = C.x - p.x, cy = C.y - p.y;
+      const double det = (ax * ax + ay * ay) * (bx * cy - by * cx) -
+        (bx * bx + by * by) * (ax * cy - ay * cx) +
+        (cx * cx + cy * cy) * (ax * by - ay * bx);
+      const double o = orient2d(A, B, C);
+      return (o > 0.0) ? (det > 0.0) : (det < 0.0);
+    };
 
-  if (!found) {
-    std::cerr << "Unable to find surface near seed" << std::endl;
-    return {};
+  vector<Pt2> pts2; pts2.reserve(cloud.size());
+  for (int i = 0; i < static_cast<int>(cloud.size()); ++i) {
+    pts2.push_back({static_cast<double>(cloud[i].x),
+        static_cast<double>(cloud[i].y), i});
   }
 
-  int seed = nbr_indices[0];
+  double minx = 1e300, miny = 1e300, maxx = -1e300, maxy = -1e300;
+  for (const auto & p : pts2) {
+    if (p.x < minx) {minx = p.x;}
+    if (p.y < miny) {miny = p.y;}
+    if (p.x > maxx) {maxx = p.x;}
+    if (p.y > maxy) {maxy = p.y;}
+  }
+  const double dx = maxx - minx, dy = maxy - miny, d = std::max(dx, dy);
+  Pt2 S1{minx - 10 * d, miny - d, -1};
+  Pt2 S2{minx + 0.5 * d, maxy + 10 * d, -2};
+  Pt2 S3{maxx + 10 * d, miny - d, -3};
+
+  vector<Pt2> vs = pts2;
+  const int iS1 = static_cast<int>(vs.size()); vs.push_back(S1);
+  const int iS2 = static_cast<int>(vs.size()); vs.push_back(S2);
+  const int iS3 = static_cast<int>(vs.size()); vs.push_back(S3);
+
+  vector<Tri2> tris;
+  if (orient2d(vs[iS1], vs[iS2], vs[iS3]) <= 0.0) {
+    std::swap(vs[iS2], vs[iS3]);
+  }
+  tris.push_back({iS1, iS2, iS3});
+
+  // Incremental Bowyer–Watson
+  for (int pi = 0; pi < static_cast<int>(pts2.size()); ++pi) {
+    const Pt2 p = vs[pi];
+
+    // 2.1) Collect "bad" triangles (circumcircle contains p)
+    vector<int> bad; bad.reserve(tris.size());
+    for (int ti = 0; ti < static_cast<int>(tris.size()); ++ti) {
+      const auto & t = tris[ti];
+      if (in_circumcircle(p, vs[t.a], vs[t.b], vs[t.c])) {
+        bad.push_back(ti);
+      }
+    }
 
-  auto triangles = grow_surface_from_seed(downsampled_points, seed, params);
+    // 2.2) Boundary of the polygonal cavity: edges touched exactly once
+    struct EdgeKey
+    {
+      int u, v;
+      bool operator==(const EdgeKey & o) const noexcept
+      {
+        return u == o.u && v == o.v;
+      }
+    };
+    struct EdgeKeyHash
+    {
+      std::size_t operator()(const EdgeKey & e) const noexcept
+      {
+        return (static_cast<std::size_t>(e.u) << 32) ^ static_cast<std::size_t>(e.v);
+      }
+    };
 
-  navmap::NavMap navmap;
-  if (!navmap_ros::build_navmap_from_mesh(
-    downsampled_points, triangles, "map", out_msg, &navmap))
-  {
-    std::cerr << "Error building ::navmap::NavMap from mesh" << std::endl;
-    return {};
+    std::unordered_map<EdgeKey, int, EdgeKeyHash> edge_count;
+    edge_count.reserve(bad.size() * 3);
+
+    auto add_edge = [&](int u, int v) {
+      // canonicalize (min, max) so opposite directions collide
+        if (u > v) {std::swap(u, v);}
+        EdgeKey e{u, v};
+        ++edge_count[e];
+      };
+
+    for (int ti : bad) {
+      const auto & t = tris[ti];
+      add_edge(t.a, t.b);
+      add_edge(t.b, t.c);
+      add_edge(t.c, t.a);
+    }
+
+    // 2.3) Remove bad triangles
+    vector<Tri2> kept; kept.reserve(tris.size());
+    vector<char> removed(tris.size(), 0);
+    for (int idx : bad) {
+      removed[idx] = 1;
+    }
+    for (int ti = 0; ti < static_cast<int>(tris.size()); ++ti) {
+      if (!removed[ti]) {kept.push_back(tris[ti]);}
+    }
+    tris.swap(kept);
+
+    // 2.4) Retriangulate the cavity with p
+    for (const auto & kv : edge_count) {
+      if (kv.second != 1) {continue;} // interior edges appear twice
+      int u = kv.first.u, v = kv.first.v;
+      // enforce CCW for (u, v, p)
+      if (orient2d(vs[u], vs[v], p) <= 0.0) {std::swap(u, v);}
+      tris.push_back({u, v, pi});
+    }
+  }
+
+  // 2.5) Discard triangles touching the super-triangle
+  vector<Tri2> final_tris; final_tris.reserve(tris.size());
+  for (const auto & t : tris) {
+    if (t.a >= static_cast<int>(pts2.size()) ||
+      t.b >= static_cast<int>(pts2.size()) ||
+      t.c >= static_cast<int>(pts2.size()))
+    {
+      continue;
+    }
+    final_tris.push_back(t);
   }
 
-  return navmap;
+  // ---- 3) Lift to 3D and filter ------------------------------------------
+  auto tri_area3D = [](const Eigen::Vector3f & A,
+    const Eigen::Vector3f & B,
+    const Eigen::Vector3f & C) -> float {
+      return 0.5f * ((B - A).cross(C - A)).norm();
+    };
+  auto tri_normal = [](const Eigen::Vector3f & A,
+    const Eigen::Vector3f & B,
+    const Eigen::Vector3f & C) -> Eigen::Vector3f {
+      Eigen::Vector3f n = (B - A).cross(C - A);
+      const float L = n.norm();
+      return (L > 1e-12f) ? (n / L) : Eigen::Vector3f(0.f, 0.f, 1.f);
+    };
+  auto edge_len3D = [&](int i, int j) -> float {
+      const auto & a = cloud[i]; const auto & b = cloud[j];
+      const float dx = a.x - b.x, dy = a.y - b.y, dz = a.z - b.z;
+      return std::sqrt(dx * dx + dy * dy + dz * dz);
+    };
+
+  const float cos_max_slope =
+    std::cos(params.max_slope_deg * static_cast<float>(M_PI) / 180.0f);
+  const float min_area = std::max(params.min_area, 1e-9f);
+  const float max_edge =
+    (params.max_edge_len > 0.0f) ?
+    params.max_edge_len :
+    std::numeric_limits<float>::infinity();
+  const float max_dz =
+    (params.max_dz > 0.0f) ?
+    params.max_dz :
+    std::numeric_limits<float>::infinity();
+
+  vector<Eigen::Vector3i> triangles;
+  triangles.reserve(final_tris.size());
+
+  // Debug counters
+  size_t dropped_area = 0, dropped_edge = 0, dropped_dz = 0, dropped_slope = 0;
+
+  for (const auto & t : final_tris) {
+    const int ia = vs[t.a].idx3d;
+    const int ib = vs[t.b].idx3d;
+    const int ic = vs[t.c].idx3d;
+
+    const Eigen::Vector3f A(cloud[ia].x, cloud[ia].y, cloud[ia].z);
+    const Eigen::Vector3f B(cloud[ib].x, cloud[ib].y, cloud[ib].z);
+    const Eigen::Vector3f C(cloud[ic].x, cloud[ic].y, cloud[ic].z);
+
+    const float area = tri_area3D(A, B, C);
+    if (area < min_area) {++dropped_area; continue;}
+
+    const float lAB = edge_len3D(ia, ib);
+    const float lBC = edge_len3D(ib, ic);
+    const float lCA = edge_len3D(ic, ia);
+    if (lAB > max_edge || lBC > max_edge || lCA > max_edge) {++dropped_edge; continue;}
+
+    const float dzAB = std::fabs(A.z() - B.z());
+    const float dzBC = std::fabs(B.z() - C.z());
+    const float dzCA = std::fabs(C.z() - A.z());
+    if (std::max({dzAB, dzBC, dzCA}) > max_dz) {++dropped_dz; continue;}
+
+    const Eigen::Vector3f n = tri_normal(A, B, C);
+    if (n.dot(Eigen::Vector3f::UnitZ()) < cos_max_slope) {++dropped_slope; continue;}
+
+    triangles.emplace_back(ia, ib, ic);
+  }
+
+  // ---- 4) Emit message + core --------------------------------------------
+  navmap::NavMap core;
+  build_navmap_from_mesh(cloud, triangles, /*frame_id*/ "map", out_msg, &core);
+
+  // ---- Debug report ------------------------------------------------------
+  // std::cerr << "[from_points] Candidate tris: " << final_tris.size()
+  //           << " | Accepted: " << triangles.size()
+  //           << " | Dropped (area=" << dropped_area
+  //           << ", edge=" << dropped_edge
+  //           << ", dz=" << dropped_dz
+  //           << ", slope=" << dropped_slope
+  //           << ")\n";
+
+  return core;
 }
 
+
 navmap::NavMap from_pointcloud2(
   const sensor_msgs::msg::PointCloud2 & pc2,
   navmap_ros_interfaces::msg::NavMap & out_msg,

navmap_ros/src/slam_server_app.cpp

@@ -53,10 +53,9 @@ class SLAMServerNode : public rclcpp::Node
       [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) {
         RCLCPP_INFO(get_logger(), "Creating and publishing NavMap from PointCloud2");
         navmap_ros::BuildParams params;
-        params.max_edge_len = 1.5f;
-        params.neighbor_radius = 1.5f;
-        params.min_angle_deg = 15.0f;
-        params.max_slope_deg = 20.0f;
+        params.max_edge_len = 5.0f;
+        params.min_angle_deg = 25.0f;
+        params.max_slope_deg = 30.0f;
         params.resolution = 1.0f;
         navmap_ = navmap_ros::from_pointcloud2(*msg, navmap_msg_, params);