diff --git a/navmap_ros/CMakeLists.txt b/navmap_ros/CMakeLists.txt index 3d9248a..88ef261 100644 --- a/navmap_ros/CMakeLists.txt +++ b/navmap_ros/CMakeLists.txt @@ -79,6 +79,7 @@ ament_export_dependencies( navmap_ros_interfaces geometry_msgs sensor_msgs + std_srvs PCL pcl_conversions ) diff --git a/navmap_ros/include/navmap_ros/conversions.hpp b/navmap_ros/include/navmap_ros/conversions.hpp index 9187863..acd6288 100644 --- a/navmap_ros/include/navmap_ros/conversions.hpp +++ b/navmap_ros/include/navmap_ros/conversions.hpp @@ -84,29 +84,44 @@ navmap_ros_interfaces::msg::NavMap to_msg( * @details * - Intended to be the inverse of ::navmap_ros::to_msg for a round-trip without loss. * - Assumes that the message is internally consistent (sizes and indices match). + * + * @throw std::runtime_error If the message describes inconsistent geometry or layer sizes. */ navmap::NavMap from_msg(const navmap_ros_interfaces::msg::NavMap & msg); /** - * \brief Convert a single layer from a NavMap into a ROS message. + * @brief Convert a single layer from a NavMap into a ROS message. + * + * @param[in] nm Input NavMap. + * @param[in] layer Name of the layer to export. + * @return A NavMapLayer message containing the layer values and metadata. * - * \param nm Input NavMap. - * \param layer Name of the layer to export. - * \return A NavMapLayer message containing the layer values and metadata. - * \throw std::runtime_error if the layer does not exist. + * @details + * - The returned message contains the layer name, type tag, and exactly one populated data + * array whose length equals the number of NavCels in @p nm. + * - The function performs a type-safe extraction (U8/F32/F64). + * + * @throw std::runtime_error If the layer does not exist or has an unsupported type. */ navmap_ros_interfaces::msg::NavMapLayer to_msg( const navmap::NavMap & nm, const std::string & layer); /** - * \brief Import a single NavMapLayer message into a NavMap. + * @brief Import a single NavMapLayer message into a NavMap. * - * If the layer already exists in \p nm, it is overwritten. Otherwise, it is created. + * If the layer already exists in @p nm, it is overwritten. Otherwise, it is created. * Performs type dispatch based on the message field `type`. * - * \param msg Input NavMapLayer message. - * \param nm Destination NavMap (must already have navcels sized correctly). + * @param[in] msg Input NavMapLayer message. + * @param[in,out] nm Destination NavMap (must already have navcels sized correctly). + * + * @details + * - The function verifies that the length of the populated data array matches + * the number of triangles (NavCels) in @p nm. + * - Exactly one of the arrays `data_u8`, `data_f32`, or `data_f64` must be set. + * + * @throw std::runtime_error If sizes are inconsistent or the message is ill-formed. */ void from_msg( const navmap_ros_interfaces::msg::NavMapLayer & msg, @@ -155,22 +170,91 @@ navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid); * @note The fallback path assumes the presence of an `"occupancy"` layer. The precise sampling * strategy (bounds, resolution, and handling of cells without a containing navcel) is * implementation-defined. + * + * @warning If the map does not carry grid metadata or the `"occupancy"` layer is missing, + * the result may be incomplete or implementation-defined. */ nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm); +/** + * @brief Parameters controlling NavMap construction from unorganized points. + * + * These parameters guide neighborhood search, local meshing, and basic geometric filtering + * used by the point-cloud based builders. + */ struct BuildParams { + /** @brief Seed position (world frame) used by region growing or initial search heuristics. */ Eigen::Vector3f seed = {0.0, 0.0, 0.0}; + + /** @brief Target in-plane sampling resolution (meters) used by voxelization or gridding. */ float resolution = 1.0; + + /** @brief Maximum allowed edge length (meters) when forming triangles. */ float max_edge_len = 2.0; + + /** @brief Maximum slope with respect to the vertical axis (degrees). */ float max_slope_deg = 30.0f; // maximum slope w.r.t. vertical + + /** @brief Neighborhood radius (meters) for candidate connectivity. */ float neighbor_radius = 2.0f; // search radius + + /** @brief Alternative to radius: number of nearest neighbors (k-NN). */ int k_neighbors = 20; // k-NN alternative to radius + + /** @brief Minimum triangle area (square meters) to reject degenerate faces. */ float min_area = 1e-6f; // minimum triangle area to avoid degenerates + + /** @brief If true, use radius-based neighborhoods; otherwise use k-NN. */ bool use_radius = true; + + /** @brief Minimum interior angle (degrees) to avoid sliver triangles. */ float min_angle_deg = 20.0f; // minimum interior angle (deg) to avoid sliver triangles }; +/** + * @brief Build a NavMap surface from a PCL point cloud. + * + * @param[in] input_points Point set in world coordinates (`pcl::PointXYZ`). + * @param[out] out_msg Output transport message mirroring the created NavMap. + * @param[in] params Meshing and filtering parameters (see ::BuildParams). + * @return The constructed `navmap::NavMap`. + * + * @details + * Typical steps implemented by this builder include: + * - Optional downsampling according to @p params.resolution. + * - Local neighborhood discovery using either radius (@p params.use_radius) or k-NN. + * - Edge and face filtering based on @p params.max_edge_len, @p params.min_area and + * @p params.min_angle_deg. + * - Optional slope gating using @p params.max_slope_deg with respect to the vertical axis. + * - Creation of a single surface with shared vertices and triangle indices. + * The function also fills @p out_msg with the compact ROS representation of the resulting map. + * + * @note Input is treated as an unorganized cloud. If normals or intensities are present, + * they are ignored by this overload. + * @throw std::runtime_error If meshing fails due to inconsistent parameters or empty input. + */ +navmap::NavMap from_points( + const pcl::PointCloud & input_points, + navmap_ros_interfaces::msg::NavMap & out_msg, + BuildParams params); + +/** + * @brief Build a NavMap surface from a ROS `sensor_msgs::msg::PointCloud2`. + * + * @param[in] pc2 Input PointCloud2 message (expects fields `x`, `y`, `z`). + * @param[out] out_msg Output transport message mirroring the created NavMap. + * @param[in] params Meshing and filtering parameters (see ::BuildParams). + * @return The constructed `navmap::NavMap`. + * + * @details + * - The cloud is decoded to `pcl::PointXYZ` and processed as in ::navmap_ros::from_points. + * - Non-Cartesian fields present in @p pc2 are ignored by this overload. + * - The resulting NavMap is exported to @p out_msg for downstream publication or storage. + * + * @note If the message is empty or lacks the required fields, no geometry is produced. + * @throw std::runtime_error If decoding fails or meshing cannot be completed. + */ navmap::NavMap from_pointcloud2( const sensor_msgs::msg::PointCloud2 & pc2, navmap_ros_interfaces::msg::NavMap & out_msg, diff --git a/navmap_ros/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index 2794c39..3cd327d 100644 --- a/navmap_ros/src/navmap_ros/conversions.cpp +++ b/navmap_ros/src/navmap_ros/conversions.cpp @@ -802,6 +802,62 @@ struct VoxelAccum }; +// Compute voxel index with an offset (used for shifted grids) +static inline Voxel voxel_index_of_offset( + const pcl::PointXYZ & p, float res, float ox, float oy, float oz) +{ + return Voxel{ + static_cast(std::floor((p.x - ox) / res)), + static_cast(std::floor((p.y - oy) / res)), + static_cast(std::floor((p.z - oz) / res)) + }; +} + +// Single voxelization pass with offset; returns centroids per voxel +static std::vector voxel_pass_offset( + const pcl::PointCloud & in, float res, float ox, float oy, float oz) +{ + std::unordered_map map; + map.reserve(in.size() / 2 + 1); + + for (const auto & pt : in.points) { + if (!pcl::isFinite(pt)) {continue;} + Voxel v = voxel_index_of_offset(pt, res, ox, oy, oz); + auto & acc = map[v]; + acc.sum_x += pt.x; + acc.sum_y += pt.y; + acc.sum_z += pt.z; + acc.count += 1; + } + + std::vector out; + out.reserve(map.size()); + for (const auto & kv : map) { + const VoxelAccum & acc = kv.second; + const float inv = acc.count > 0 ? 1.0f / static_cast(acc.count) : 0.0f; + out.emplace_back(acc.sum_x * inv, acc.sum_y * inv, acc.sum_z * inv); + } + return out; +} + +// Compute hash grid cell index for a point +static inline Voxel cell_of_point(const pcl::PointXYZ & p, float cell) +{ + return Voxel{ + static_cast(std::floor(p.x / cell)), + static_cast(std::floor(p.y / cell)), + static_cast(std::floor(p.z / cell)) + }; +} + +static inline pcl::PointXYZ accum_centroid(const VoxelAccum & a) +{ + const float inv = a.count > 0 ? 1.0f / static_cast(a.count) : 0.0f; + return pcl::PointXYZ(a.sum_x * inv, a.sum_y * inv, a.sum_z * inv); +} + +// Downsampling by voxelization with two passes (normal and shifted grid), +// followed by merging centroids that fall within 0.5*resolution pcl::PointCloud downsample_voxelize_avgXYZ( const pcl::PointCloud & input_points, @@ -809,44 +865,150 @@ downsample_voxelize_avgXYZ( { pcl::PointCloud output; - std::unordered_map voxels; - voxels.reserve(input_points.size() / 2); // rough estimate + if (input_points.empty() || !(resolution > 0.0f)) { + output.width = 0; output.height = 1; output.is_dense = true; + return output; + } + + const float r = resolution; + const float half = 0.5f * r; + + // Pass A: regular grid + auto centroids_a = voxel_pass_offset(input_points, r, 0.0f, 0.0f, 0.0f); + + // Pass B: grid shifted by half resolution + auto centroids_b = voxel_pass_offset(input_points, r, half, half, half); + + // Merge centroids using a hash grid to join those split by voxel borders + const float merge_radius = half; + const float merge_radius2 = merge_radius * merge_radius; + const float grid_cell_size = r; + + std::vector clusters; + clusters.reserve(centroids_a.size()); + + std::unordered_map, VoxelHash> grid; + grid.reserve(centroids_a.size() + centroids_b.size()); + + auto try_insert = [&](const pcl::PointXYZ & p) + { + Voxel c = cell_of_point(p, grid_cell_size); + int best_idx = -1; + float best_d2 = std::numeric_limits::max(); + + // Search 27 neighboring cells for a close cluster + for (int dz = -1; dz <= 1; ++dz) { + for (int dy = -1; dy <= 1; ++dy) { + for (int dx = -1; dx <= 1; ++dx) { + Voxel nb{c.x + dx, c.y + dy, c.z + dz}; + auto it = grid.find(nb); + if (it == grid.end()) {continue;} + + for (int idx : it->second) { + const pcl::PointXYZ q = accum_centroid(clusters[idx]); + const float ex = p.x - q.x; + const float ey = p.y - q.y; + const float ez = p.z - q.z; + const float d2 = ex * ex + ey * ey + ez * ez; + if (d2 < best_d2) {best_d2 = d2; best_idx = idx;} + } + } + } + } + + if (best_idx >= 0 && best_d2 <= merge_radius2) { + // Merge into existing cluster + clusters[best_idx].sum_x += p.x; + clusters[best_idx].sum_y += p.y; + clusters[best_idx].sum_z += p.z; + clusters[best_idx].count += 1; + } else { + // Create new cluster + VoxelAccum acc; + acc.sum_x = p.x; acc.sum_y = p.y; acc.sum_z = p.z; acc.count = 1; + int new_idx = static_cast(clusters.size()); + clusters.push_back(acc); + grid[c].push_back(new_idx); + } + }; - // 1) Accumulate all points per voxel - for (std::size_t i = 0; i < input_points.size(); ++i) { - const auto & pt = input_points[i]; + // Insert centroids from both passes + for (const auto & p : centroids_a) { + try_insert(p); + } + for (const auto & p : centroids_b) { + try_insert(p); + } + + // Emit one point per cluster + output.points.reserve(clusters.size()); + for (const auto & cl : clusters) { + output.points.push_back(accum_centroid(cl)); + } + + output.width = static_cast(output.points.size()); + output.height = 1; + output.is_dense = true; + return output; +} + +pcl::PointCloud +downsample_voxelize_avgZ( + const pcl::PointCloud & input_points, + float resolution) +{ + pcl::PointCloud output; + + if (input_points.empty() || !(resolution > 0.0f)) { + output.width = 0; output.height = 1; output.is_dense = true; + return output; + } + + struct Accum + { + double sum_z{0.0}; + int count{0}; + }; + + std::unordered_map voxels; + voxels.reserve(input_points.size() / 2); + + // Accumulate Z per voxel + for (const auto & pt : input_points) { if (!pcl::isFinite(pt)) {continue;} int vx = static_cast(std::floor(pt.x / resolution)); int vy = static_cast(std::floor(pt.y / resolution)); - int vz = static_cast(std::floor(pt.z / resolution)); + int vz = static_cast(std::floor(pt.z / resolution)); // only for voxel id Voxel v{vx, vy, vz}; auto & acc = voxels[v]; - acc.sum_x += pt.x; - acc.sum_y += pt.y; acc.sum_z += pt.z; acc.count += 1; } - // 2) Emit one averaged point per voxel + // Emit one point per voxel output.points.reserve(voxels.size()); for (const auto & kv : voxels) { - const VoxelAccum & acc = kv.second; - float cx = acc.sum_x / acc.count; - float cy = acc.sum_y / acc.count; - float cz = acc.sum_z / acc.count; + const Voxel & v = kv.first; + const Accum & acc = kv.second; + + // Center of voxel in XY + float cx = (v.x + 0.5f) * resolution; + float cy = (v.y + 0.5f) * resolution; + // Average Z of all points + float cz = static_cast(acc.sum_z / acc.count); + output.emplace_back(cx, cy, cz); } - output.width = static_cast(output.size()); + output.width = static_cast(output.points.size()); output.height = 1; output.is_dense = true; return output; } - std::vector grow_surface_from_seed( const pcl::PointCloud & cloud, int seed_idx, @@ -925,14 +1087,12 @@ std::vector grow_surface_from_seed( return tris; } -navmap::NavMap from_pointcloud2( - const sensor_msgs::msg::PointCloud2 & pc2, + +navmap::NavMap from_points( + const pcl::PointCloud & input_points, navmap_ros_interfaces::msg::NavMap & out_msg, BuildParams params) { - pcl::PointCloud input_points; - pcl::fromROSMsg(pc2, input_points); - auto downsampled_points = downsample_voxelize_avgXYZ(input_points, params.resolution); pcl::KdTreeFLANN kdtree; @@ -966,5 +1126,15 @@ navmap::NavMap from_pointcloud2( return navmap; } +navmap::NavMap from_pointcloud2( + const sensor_msgs::msg::PointCloud2 & pc2, + navmap_ros_interfaces::msg::NavMap & out_msg, + BuildParams params) +{ + pcl::PointCloud input_points; + pcl::fromROSMsg(pc2, input_points); + + return from_points(input_points, out_msg, params); +} } // namespace navmap_ros diff --git a/navmap_ros/src/slam_server_app.cpp b/navmap_ros/src/slam_server_app.cpp index c3665cf..74fc532 100644 --- a/navmap_ros/src/slam_server_app.cpp +++ b/navmap_ros/src/slam_server_app.cpp @@ -53,6 +53,11 @@ 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.resolution = 1.0f; navmap_ = navmap_ros::from_pointcloud2(*msg, navmap_msg_, params); navmap_msg_.header.frame_id = "map";