From f121acbef36a3cd4dd4720d85405fb12e7540a44 Mon Sep 17 00:00:00 2001 From: akilpath <56412097+akilpath@users.noreply.github.com> Date: Fri, 27 Sep 2024 18:59:58 -0400 Subject: [PATCH] Patchwork Paper implementation using ROS 2. (#142) * Copy transformer samples to occupancy_seg * Copied transformer node layout * Renamed files * Fix small error * Clean up * Add pcl package * Added eigen to dockerifle * Update docker-compose.world_modeling.yaml * Update docker-compose.world_modeling.yaml * added tbb to dockerfile * Started working on it * Finished czm initialization * Plane * Mostly done * Add apt update * Fixing small bugs * Build / make error with core.cpp * Added main node code, still fails build * Commented some things to get it to compile * Builds * Foxglove setup plus error debugging exit exit() * Templated Core file * lowkey works * Added headers to publishers * Copy transformer samples to occupancy_seg * Copied transformer node layout * Renamed files * Fix small error * Clean up * Add pcl package * Added eigen to dockerifle * Update docker-compose.world_modeling.yaml * Update docker-compose.world_modeling.yaml * added tbb to dockerfile * Started working on it * Finished czm initialization * Plane * Mostly done * Add apt update * Fixing small bugs * Build / make error with core.cpp * Added main node code, still fails build * Commented some things to get it to compile * Builds * Foxglove setup plus error debugging exit exit() * Templated Core file * lowkey works * Added headers to publishers * Remove watod-config changes * Cleanup + Timing for patchwork * Update watod-config.sh * Update docker_context.sh * Fix code style issues with clang_format * added ros params * Fix code style issues with clang_format * Removed adaptive seed selection for 1st zone * Made adaptive seed selection into toggle param * Put num_zones in params * Fix code style issues with clang_format --------- Co-authored-by: VishGit1234 Co-authored-by: VishGit1234 <47926842+VishGit1234@users.noreply.github.com> Co-authored-by: WATonomousAdmin --- .../occupancy_segmentation.Dockerfile | 7 +- .../occupancy_segmentation/CMakeLists.txt | 71 ++++- .../occupancy_segmentation/config/params.yaml | 23 ++ .../include/occupancy_segmentation_core.hpp | 129 ++++++++ .../include/occupancy_segmentation_node.hpp | 53 ++++ .../launch/occupancy_segmentation.launch.py | 31 ++ .../occupancy_segmentation/package.xml | 14 +- .../src/occupancy_segmentation_core.cpp | 295 ++++++++++++++++++ .../src/occupancy_segmentation_node.cpp | 107 +++++++ 9 files changed, 718 insertions(+), 12 deletions(-) create mode 100644 src/world_modeling/occupancy_segmentation/config/params.yaml create mode 100644 src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp create mode 100644 src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp create mode 100755 src/world_modeling/occupancy_segmentation/launch/occupancy_segmentation.launch.py create mode 100644 src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp create mode 100644 src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp diff --git a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile index 1514079f..088351e9 100644 --- a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile +++ b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile @@ -7,7 +7,6 @@ WORKDIR ${AMENT_WS}/src # Copy in source code COPY src/world_modeling/occupancy_segmentation occupancy_segmentation -COPY src/wato_msgs/sample_msgs sample_msgs # Scan for rosdeps RUN apt-get -qq update && rosdep update && \ @@ -19,9 +18,13 @@ RUN apt-get -qq update && rosdep update && \ ################################# Dependencies ################################ FROM ${BASE_IMAGE} as dependencies +# Install some patchwork dependencies +RUN sudo apt-get install libeigen3-dev +RUN sudo apt-get -y install libtbb-dev + # Install Rosdep requirements COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list -RUN apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list) +RUN apt-get -qq update && apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list) # Copy in source code from source stage WORKDIR ${AMENT_WS} diff --git a/src/world_modeling/occupancy_segmentation/CMakeLists.txt b/src/world_modeling/occupancy_segmentation/CMakeLists.txt index dfa8b8bb..ed31cbcb 100644 --- a/src/world_modeling/occupancy_segmentation/CMakeLists.txt +++ b/src/world_modeling/occupancy_segmentation/CMakeLists.txt @@ -1,14 +1,73 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.10) project(occupancy_segmentation) +# Set compiler to use C++ 17 standard +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) +# Search for dependencies required for building this package +find_package(ament_cmake REQUIRED) # ROS2 build tool +find_package(sensor_msgs REQUIRED) +find_package(rclcpp REQUIRED) # ROS2 C++ package +find_package(pcl_ros REQUIRED) + +# Compiles source files into a library +# A library is not executed, instead other executables can link +# against it to access defined methods and classes. +# We build a library so that the methods defined can be used by +# both the unit test and ROS2 node executables. +add_library(occupancy_segmentation_lib + src/occupancy_segmentation_core.cpp) +# Indicate to compiler where to search for header files +target_include_directories(occupancy_segmentation_lib + PUBLIC + include) +# Add ROS2 dependencies required by package +ament_target_dependencies(occupancy_segmentation_lib + sensor_msgs + rclcpp + pcl_ros) + +# # By default tests are built. This can be overridden by modifying the +# # colcon build command to include the -DBUILD_TESTING=OFF flag. +# option(BUILD_TESTING "Build tests" ON) +# if(BUILD_TESTING) +# # Search for dependencies required for building tests + linting +# find_package(ament_cmake_gtest REQUIRED) + +# # Create test executable from source files +# ament_add_gtest(occupancy_segmentation_test test/occupancy_segmentation_test.cpp) +# # Link to the previously built library to access occupancy_segmentation classes and methods +# target_link_libraries(occupancy_segmentation_test +# occupancy_segmentation_lib +# gtest_main) + +# # Copy executable to installation location +# install(TARGETS +# occupancy_segmentation_test +# DESTINATION lib/${PROJECT_NAME}) +# endif() + +# Create ROS2 node executable from source files +add_executable(occupancy_segmentation_node src/occupancy_segmentation_node.cpp) +# Link to the previously built library to access occupancy_segmentation classes and methods +target_link_libraries(occupancy_segmentation_node occupancy_segmentation_lib) + +# Copy executable to installation location +install(TARGETS + occupancy_segmentation_node + DESTINATION lib/${PROJECT_NAME}) + +# Copy launch and config files to installation location +install(DIRECTORY + config + launch + DESTINATION share/${PROJECT_NAME}) ament_package() + diff --git a/src/world_modeling/occupancy_segmentation/config/params.yaml b/src/world_modeling/occupancy_segmentation/config/params.yaml new file mode 100644 index 00000000..ba955edc --- /dev/null +++ b/src/world_modeling/occupancy_segmentation/config/params.yaml @@ -0,0 +1,23 @@ +occupancy_segmentation: + ros__parameters: + version: 1 + lidar_input_topic: "/velodyne_points" + ground_output_topic: "/ground_points" + nonground_output_topic: "/nonground_points" + num_zones: 4 + l_min: 2.7 + l_max: 80.0 + md: 0.3 + mh: -1.1 + min_num_points: 10 + num_seed_points: 20 + th_seeds: 0.5 + uprightness_thresh: 0.5 + num_rings_of_interest: 4 + sensor_height: 1.7 + global_el_thresh: 0.0 + zone_rings: [2, 4, 4, 4] + zone_sectors: [16, 32, 54, 32] + flatness_thr: [0.0005, 0.000725, 0.001, 0.001] + elevation_thr: [0.523, 0.746, 0.879, 1.125] + adaptive_selection_en: false diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp new file mode 100644 index 00000000..749b4ad9 --- /dev/null +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -0,0 +1,129 @@ +#ifndef OCCUPANCY_SEGMENTATION_CORE_HPP_ +#define OCCUPANCY_SEGMENTATION_CORE_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include + +struct PointXYZIRT { + PCL_ADD_POINT4D; // preferred way of adding a XYZ+padding + float intensity; + u_int16_t ring; + float time; + PCL_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned +} EIGEN_ALIGN16; + +struct PCAFeature { + Eigen::Vector3f principal_; + Eigen::Vector3f normal_; + Eigen::Vector3f singular_values_; + Eigen::Vector3f mean_; + float d_; + float th_dist_d_; + float linearity_; + float planarity_; +}; + +struct Patch_Index { + int idx; + int zone_idx; + int ring_idx; + int sector_idx; + int concentric_idx; +}; + +enum Status { + TOO_TILTED, + FLAT_ENOUGH, + TOO_HIGH_ELEV, + UPRIGHT_ENOUGH, + GLOBAL_TOO_HIGH_ELEV, + FEW_POINTS +}; + +template +class OccupancySegmentationCore { + public: + typedef std::vector> Ring; + typedef std::vector Zone; + + int NUM_ZONES; + + float L_MIN; + float L_MAX; + + float MD; + float MH; + + int MIN_NUM_POINTS; + int NUM_SEED_POINTS; + + float TH_SEEDS; + float UPRIGHTNESS_THRESH; + + int NUM_RINGS_OF_INTEREST; + float SENSOR_HEIGHT; + float GLOBAL_EL_THRESH; + + std::vector ZONE_RINGS; + std::vector ZONE_SECTORS; + std::vector FLATNESS_THR; + std::vector ELEVATION_THR; + std::vector lmins = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, + (L_MIN + L_MAX) / 2}; + std::vector lmaxs = {lmins[1], lmins[2], lmins[3], L_MAX}; + + int num_patches = -1; + + bool ADAPTIVE_SELECTION_EN; + + std::vector _czm; + std::vector> _regionwise_ground; + std::vector> _regionwise_nonground; + + std::vector _patch_indices; + + pcl::PointCloud _ground; + pcl::PointCloud _non_ground; + + std::vector _statuses; + + OccupancySegmentationCore(); + OccupancySegmentationCore(int num_zones, float l_min, float l_max, float md, float mh, + int min_num_points, int num_seed_points, float th_seeds, + float uprightness_thresh, int num_rings_of_interest, + float sensor_height, float global_el_thresh, + std::vector> &zone_rings, + std::vector> &zone_sectors, + std::vector &flatness_thr, std::vector &elevation_thr, + bool adaptive_selection_en); + + void segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, + pcl::PointCloud &nonground); + + private: + void init_czm(); + + void fill_czm(pcl::PointCloud &cloud_in); + + void clear_czm_and_regionwise(); + + void estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat); + + void rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat); + + void extract_initial_seeds(pcl::PointCloud &cloud, pcl::PointCloud &seed_cloud, + int zone_idx); + + Status ground_likelihood_est(PCAFeature &feat, int concentric_idx); + + static bool point_z_cmp(PointT a, PointT b) { return a.z < b.z; }; +}; + +#endif diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp new file mode 100644 index 00000000..ed71ef67 --- /dev/null +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp @@ -0,0 +1,53 @@ +#ifndef OCCUPANCY_SEGMENTATION_NODE_HPP_ +#define OCCUPANCY_SEGMENTATION_NODE_HPP_ +#define PCL_NO_PRECOMPILE + +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include "occupancy_segmentation_core.hpp" + +typedef std::chrono::high_resolution_clock Clock; + +POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRT, // here we assume a XYZ + "test" (as fields) + (float, x, x)(float, y, y)(float, z, + z)(float, intensity, + intensity)(u_int16_t, ring, + ring)(float, time, time)) + +/** + * Implementation of a ROS2 node that converts unfiltered messages to filtered_array + * messages. + * + * Listens to the "unfiltered" topic and filters out data with invalid fields + * and odd timestamps. Once the node collects messages it packs + * the processed messages into an array and publishes it to the "filtered" topic. + */ +class OccupancySegmentationNode : public rclcpp::Node { + public: + // Configure pubsub nodes to keep last 20 messages. + // https://docs.ros.org/en/foxy/Concepts/About-Quality-of-Service-Settings.html + static constexpr int ADVERTISING_FREQ = 20; + + /** + * Transformer node constructor. + */ + OccupancySegmentationNode(); + + private: + // Object that handles data processing and validation. + OccupancySegmentationCore _patchwork; + rclcpp::Subscription::SharedPtr _subscriber; + rclcpp::Publisher::SharedPtr _nonground_publisher; + rclcpp::Publisher::SharedPtr _ground_publisher; + + void subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud); +}; + +#endif // OCCUPANCY_SEGMENTATION_NODE_HPP_ diff --git a/src/world_modeling/occupancy_segmentation/launch/occupancy_segmentation.launch.py b/src/world_modeling/occupancy_segmentation/launch/occupancy_segmentation.launch.py new file mode 100755 index 00000000..975e8a31 --- /dev/null +++ b/src/world_modeling/occupancy_segmentation/launch/occupancy_segmentation.launch.py @@ -0,0 +1,31 @@ +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +import os + + +def generate_launch_description(): + """Launch occupancy_seg node.""" + occupancy_seg_pkg_prefix = get_package_share_directory('occupancy_segmentation') + occupancy_seg_param_file = os.path.join( + occupancy_seg_pkg_prefix, 'config', 'params.yaml') + + occupancy_seg_param = DeclareLaunchArgument( + 'occupancy_segmentation_param_file', + default_value=occupancy_seg_param_file, + description='Path to config file for occupancy segmentation node' + ) + + occupancy_seg_node = Node( + package='occupancy_segmentation', + executable='occupancy_segmentation_node', + parameters=[LaunchConfiguration('occupancy_segmentation_param_file')], + ) + + return LaunchDescription([ + occupancy_seg_param, + occupancy_seg_node + ]) diff --git a/src/world_modeling/occupancy_segmentation/package.xml b/src/world_modeling/occupancy_segmentation/package.xml index 57c0712d..14ec454f 100644 --- a/src/world_modeling/occupancy_segmentation/package.xml +++ b/src/world_modeling/occupancy_segmentation/package.xml @@ -1,15 +1,21 @@ - occupancy_segmentation 0.0.0 - TODO: Package description - bolty - TODO: License declaration + A ROS2 package to handle LiDAR ground segmentation using Patchworks + Akil Pathiranage + TODO + + ament_cmake + sensor_msgs + rclcpp + pcl_ros + ament_cmake_gtest + ament_cmake diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp new file mode 100644 index 00000000..b590a17c --- /dev/null +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -0,0 +1,295 @@ +#include "occupancy_segmentation_core.hpp" + +// Explicitly instantiate template constructor for the type we will use +template class OccupancySegmentationCore; + +template +OccupancySegmentationCore::OccupancySegmentationCore( + int num_zones, float l_min, float l_max, float md, float mh, int min_num_points, + int num_seed_points, float th_seeds, float uprightness_thresh, int num_rings_of_interest, + float sensor_height, float global_el_thresh, + std::vector > &zone_rings, + std::vector > &zone_sectors, + std::vector &flatness_thr, std::vector &elevation_thr, + bool adaptive_selection_en) + : NUM_ZONES{num_zones}, + L_MIN{l_min}, + L_MAX{l_max}, + MD{md}, + MH{mh}, + MIN_NUM_POINTS{min_num_points}, + NUM_SEED_POINTS{num_seed_points}, + TH_SEEDS{th_seeds}, + UPRIGHTNESS_THRESH{uprightness_thresh}, + NUM_RINGS_OF_INTEREST{num_rings_of_interest}, + SENSOR_HEIGHT{sensor_height}, + GLOBAL_EL_THRESH{global_el_thresh}, + ZONE_RINGS{zone_rings.begin(), zone_rings.end()}, + ZONE_SECTORS{zone_rings.begin(), zone_sectors.end()}, + FLATNESS_THR{flatness_thr.begin(), flatness_thr.end()}, + ELEVATION_THR{elevation_thr.begin(), elevation_thr.end()}, + lmins{L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}, + lmaxs{lmins[1], lmins[2], lmins[3], L_MAX}, + ADAPTIVE_SELECTION_EN{adaptive_selection_en} { + init_czm(); +} + +template +OccupancySegmentationCore::OccupancySegmentationCore() {} + +template +void OccupancySegmentationCore::init_czm() { + int concentric_count = 0; + int patch_count = 0; + + for (int z = 0; z < NUM_ZONES; z++) { + Zone zone; + Patch_Index index; + index.zone_idx = z; + + for (int r = 0; r < ZONE_RINGS[z]; r++) { + Ring ring; + index.ring_idx = r; + index.concentric_idx = concentric_count; + + for (int s = 0; s < ZONE_SECTORS[z]; s++) { + index.sector_idx = s; + index.idx = patch_count; + _patch_indices.push_back(index); + pcl::PointCloud patch; + ring.emplace_back(patch); + + pcl::PointCloud regionground; + pcl::PointCloud regionnonground; + Status status; + _regionwise_ground.push_back(regionground); + _regionwise_nonground.push_back(regionnonground); + + _statuses.push_back(status); + + patch_count++; + } + zone.emplace_back(ring); + } + _czm.emplace_back(zone); + } + num_patches = patch_count; +} + +template +void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_in) { + for (PointT &p : cloud_in.points) { + double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); + + // Out of range for czm + if (r < L_MIN || r > L_MAX) { + continue; + } + + double theta = atan2(p.y, p.x); + if (theta < 0) { + theta += 2 * M_PI; + } + + double deltal = L_MAX - L_MIN; + + for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++) { + int ring_idx, sector_idx; + if (r < lmaxs[zone_idx]) { + double ring_size = deltal / ZONE_RINGS[zone_idx]; + double sector_size = 2 * M_PI / ZONE_SECTORS[zone_idx]; + + ring_idx = std::min((int)((r - lmins[zone_idx]) / ring_size), ZONE_RINGS[zone_idx] - 1); + sector_idx = std::min((int)(theta / sector_size), ZONE_SECTORS[zone_idx] - 1); + _czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); + break; + } + } + } +} + +template +void OccupancySegmentationCore::clear_czm_and_regionwise() { + for (Zone &zone : _czm) { + for (Ring &ring : zone) { + for (pcl::PointCloud &patch : ring) { + patch.clear(); + } + } + } +} + +template +void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, + PCAFeature &feat) { + // Code taken directly from repo + Eigen::Matrix3f cov; + Eigen::Vector4f mean; + pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); + + Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); + feat.singular_values_ = svd.singularValues(); + + feat.linearity_ = + (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); + feat.planarity_ = + (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); + + // use the least singular vector as normal + feat.normal_ = (svd.matrixU().col(2)); + if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive + feat.normal_ = -feat.normal_; + } + // mean ground seeds value + feat.mean_ = mean.head<3>(); + // according to normal.T*[x,y,z] = -d + feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); + feat.th_dist_d_ = MD - feat.d_; +} + +template +void OccupancySegmentationCore::segment_ground(pcl::PointCloud &unfiltered_cloud, + pcl::PointCloud &ground, + pcl::PointCloud &nonground) { + clear_czm_and_regionwise(); + + // TODO error point removal + + fill_czm(unfiltered_cloud); + tbb::parallel_for(tbb::blocked_range(0, num_patches), [&](tbb::blocked_range r) { + for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { + Patch_Index &p_idx = _patch_indices[patch_num]; + pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; + pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + PCAFeature features; + + region_ground.clear(); + region_nonground.clear(); + if (patch.points.size() > MIN_NUM_POINTS) { + std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); + rgpf(patch, p_idx, features); + + Status status = ground_likelihood_est(features, p_idx.concentric_idx); + _statuses[p_idx.idx] = status; + } else { + region_ground = patch; + _statuses[p_idx.idx] = FEW_POINTS; + } + } + }); + + for (Patch_Index p_idx : _patch_indices) { + Status status = _statuses[p_idx.idx]; + + if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH) { + ground += _regionwise_ground[p_idx.idx]; + nonground += _regionwise_nonground[p_idx.idx]; + } else { + nonground += _regionwise_ground[p_idx.idx]; + nonground += _regionwise_nonground[p_idx.idx]; + } + } +} + +template +void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, + PCAFeature &feat) { + pcl::PointCloud ground_temp; + pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + + size_t N = patch.size(); + + extract_initial_seeds(patch, ground_temp, p_idx.zone_idx); + int num_iters = 3; + for (int i = 0; i < num_iters; i++) { + estimate_plane(ground_temp, feat); + ground_temp.clear(); + Eigen::MatrixXf points(N, 3); + int j = 0; + for (PointT &p : patch.points) { + points.row(j++) = p.getVector3fMap(); + } + + Eigen::VectorXf result = points * feat.normal_; + for (size_t r = 0; r < N; r++) { + if (i < num_iters - 1) { + if (result[r] < feat.th_dist_d_) { + ground_temp.points.emplace_back(patch.points[r]); + } + } else { + if (result[r] < feat.th_dist_d_) { + region_ground.points.emplace_back(patch.points[r]); + } else { + region_nonground.points.emplace_back(patch.points[r]); + } + } + } + } +} + +template +Status OccupancySegmentationCore::ground_likelihood_est(PCAFeature &feat, + int concentric_idx) { + // uprightness filter + if (std::abs(feat.normal_(2)) < UPRIGHTNESS_THRESH) { + return TOO_TILTED; + } + + const double z_elevation = feat.mean_(2); + const double surface_variable = + feat.singular_values_.minCoeff() / + (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)); + + // elevation filter + if (concentric_idx < NUM_RINGS_OF_INTEREST) { + if (z_elevation > -SENSOR_HEIGHT + ELEVATION_THR[concentric_idx]) { + if (FLATNESS_THR[concentric_idx] <= surface_variable) { + return FLAT_ENOUGH; + } else { + return TOO_HIGH_ELEV; + } + } else { + return UPRIGHT_ENOUGH; + } + } else { + if (z_elevation > GLOBAL_EL_THRESH) { + return GLOBAL_TOO_HIGH_ELEV; + } else { + return UPRIGHT_ENOUGH; + } + } +} + +template +void OccupancySegmentationCore::extract_initial_seeds(pcl::PointCloud &cloud, + pcl::PointCloud &seed_cloud, + int zone_idx) { + // adaptive seed selection for 1st zone + size_t init_idx = 0; + if (ADAPTIVE_SELECTION_EN && zone_idx == 0) { + double adaptive_seed_selection_margin = MH * SENSOR_HEIGHT; + for (size_t i = 0; i < cloud.points.size(); i++) { + if (cloud.points[i].z < adaptive_seed_selection_margin) { + init_idx++; + } else { + break; + } + } + } + + double sum = 0; + int cnt = 0; + for (size_t i = 0; i < cloud.points.size() && cnt < NUM_SEED_POINTS; i++) { + sum += cloud.points[i].z; + cnt++; + } + + double mean_z = (cnt != 0) ? sum / cnt : 0; + for (size_t i = init_idx; i < cloud.points.size(); i++) { + if (cloud.points[i].z < mean_z + TH_SEEDS) { + seed_cloud.points.push_back(cloud.points[i]); + } + } +} diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp new file mode 100644 index 00000000..ebe01238 --- /dev/null +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -0,0 +1,107 @@ +#include "occupancy_segmentation_node.hpp" +#include +#include + +OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segmentation") { + // Declare parameters + this->declare_parameter("num_zones", 4); + this->declare_parameter("l_min", 2.7); + this->declare_parameter("l_max", 80.0); + this->declare_parameter("md", 0.3); + this->declare_parameter("mh", -1.1); + this->declare_parameter("min_num_points", 10); + this->declare_parameter("num_seed_points", 20); + this->declare_parameter("th_seeds", 20); + this->declare_parameter("uprightness_thresh", 0.5); + this->declare_parameter("num_rings_of_interest", 4); + this->declare_parameter("sensor_height", 1.7); + this->declare_parameter("global_el_thresh", 0.0); + this->declare_parameter>("zone_rings", std::vector{2, 4, 4, 4}); + this->declare_parameter>("zone_sectors", std::vector{16, 32, 54, 32}); + this->declare_parameter>("flatness_thr", + std::vector{0.0005, 0.000725, 0.001, 0.001}); + this->declare_parameter>("elevation_thr", + std::vector{0.523, 0.746, 0.879, 1.125}); + this->declare_parameter("adaptive_selection_en", bool(false)); + this->declare_parameter("lidar_input_topic", std::string("/velodyne_points")); + this->declare_parameter("ground_output_topic", std::string("/ground_points")); + this->declare_parameter("nonground_output_topic", std::string("/nonground_points")); + + // Retrieve parameters + int num_zones = this->get_parameter("num_zones").as_int(); + double l_min = this->get_parameter("l_min").as_double(); + double l_max = this->get_parameter("l_max").as_double(); + double md = this->get_parameter("md").as_double(); + double mh = this->get_parameter("mh").as_double(); + int min_num_points = this->get_parameter("min_num_points").as_int(); + int num_seed_points = this->get_parameter("num_seed_points").as_int(); + double th_seeds = this->get_parameter("th_seeds").as_double(); + double uprightness_thresh = this->get_parameter("uprightness_thresh").as_double(); + int num_rings_of_interest = this->get_parameter("num_rings_of_interest").as_int(); + double sensor_height = this->get_parameter("sensor_height").as_double(); + double global_el_thresh = this->get_parameter("global_el_thresh").as_double(); + auto zone_rings = this->get_parameter("zone_rings").as_integer_array(); + auto zone_sectors = this->get_parameter("zone_sectors").as_integer_array(); + auto flatness_thr = this->get_parameter("flatness_thr").as_double_array(); + auto elevation_thr = this->get_parameter("elevation_thr").as_double_array(); + bool adaptive_selection_en = this->get_parameter("adaptive_selection_en").as_bool(); + std::string lidar_input_topic = this->get_parameter("lidar_input_topic").as_string(); + std::string ground_output_topic = this->get_parameter("ground_output_topic").as_string(); + std::string nonground_output_topic = this->get_parameter("nonground_output_topic").as_string(); + + _patchwork = OccupancySegmentationCore( + num_zones, l_min, l_max, md, mh, min_num_points, num_seed_points, th_seeds, + uprightness_thresh, num_rings_of_interest, sensor_height, global_el_thresh, zone_rings, + zone_sectors, flatness_thr, elevation_thr, adaptive_selection_en); + + _subscriber = this->create_subscription( + lidar_input_topic, 10, + std::bind(&OccupancySegmentationNode::subscription_callback, this, std::placeholders::_1)); + + _ground_publisher = + this->create_publisher(ground_output_topic, 10); + _nonground_publisher = + this->create_publisher(nonground_output_topic, 10); +} + +void OccupancySegmentationNode::subscription_callback( + const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud) { + pcl::PointCloud temp_cloud; + RCLCPP_DEBUG(this->get_logger(), "Header incoming: %s", lidar_cloud->header.frame_id.c_str()); + pcl::fromROSMsg(*lidar_cloud, temp_cloud); + + pcl::PointCloud ground; + pcl::PointCloud nonground; + + ground.clear(); + nonground.clear(); + ground.header = temp_cloud.header; + nonground.header = temp_cloud.header; + + auto begin = Clock::now(); + _patchwork.segment_ground(temp_cloud, ground, nonground); + auto end = Clock::now(); + + int duration = std::chrono::duration_cast(end - begin).count(); + RCLCPP_DEBUG(this->get_logger(), "Runtime for Patchwork: %i ms", duration); + RCLCPP_DEBUG(this->get_logger(), "Temp_cloud points %i", static_cast(temp_cloud.size())); + RCLCPP_DEBUG(this->get_logger(), "Ground points %i", static_cast(ground.size())); + RCLCPP_DEBUG(this->get_logger(), "Non ground points %i", static_cast(nonground.size())); + + sensor_msgs::msg::PointCloud2 ground_msg; + sensor_msgs::msg::PointCloud2 nonground_msg; + + pcl::toROSMsg(ground, ground_msg); + pcl::toROSMsg(nonground, nonground_msg); + RCLCPP_DEBUG(this->get_logger(), "Header outgoing: %s", ground_msg.header.frame_id.c_str()); + + _ground_publisher->publish(ground_msg); + _nonground_publisher->publish(nonground_msg); +} + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +}