From b0d438d71d03b417ecc7bb79b00f0c9c5342332b Mon Sep 17 00:00:00 2001 From: Sam Richter Date: Mon, 14 Oct 2024 23:07:11 -0500 Subject: [PATCH] upgrade everything to work with simulator --- .github/workflows/ci.yml | 4 +- CMakeLists.txt | 29 +- README.md | 1 + config/params.yaml | 7 +- launch/ldrp.launch.py | 44 - launch/sick_perception.launch.py | 32 + package.xml | 4 +- src/ldrp/filtering.hpp | 1271 ++++++++++++++-------------- src/ldrp/grid.hpp | 1340 +++++++++++++++--------------- src/ldrp/perception.cpp | 720 ++++++++-------- src/ldrp/perception.hpp | 97 ++- src/ldrp/timestamp_sampler.hpp | 136 +-- src/main.cpp | 13 +- 13 files changed, 1825 insertions(+), 1873 deletions(-) delete mode 100644 launch/ldrp.launch.py create mode 100644 launch/sick_perception.launch.py diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 3010818..d9b7dbe 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -24,12 +24,12 @@ jobs: - uses: ros-tooling/setup-ros@v0.7 with: - required-ros-distributions: humble + required-ros-distributions: jazzy - uses: ros-tooling/action-ros-ci@v0.3 with: package-name: sick_perception - target-ros2-distro: humble + target-ros2-distro: jazzy colcon-extra-args: --event-handler console_direct+ # - name: "Get PCL" diff --git a/CMakeLists.txt b/CMakeLists.txt index 18e1ff7..f5f1263 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,8 @@ if(${sickperception_VSCODE_CONFIGURE}) # this is just a dummy configuration so v find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) + find_package(tf2_ros REQUIRED) + find_package(tf2_sensor_msgs REQUIRED) find_package(pcl_conversions REQUIRED) find_package(PCL REQUIRED COMPONENTS common filters, octree) @@ -80,16 +82,16 @@ if(${sickperception_VSCODE_CONFIGURE}) # this is just a dummy configuration so v list(APPEND ldrp_sources "${ldrp_src}/main.cpp" "${ldrp_src}/ldrp/perception.cpp") list(APPEND ldrp_includedirs) - add_executable(ldrp_node ${ldrp_sources}) + add_executable(${PROJECT_NAME} ${ldrp_sources}) - target_include_directories(ldrp_node + target_include_directories(${PROJECT_NAME} PRIVATE ${PCL_INCLUDE_DIRS} PRIVATE ${ldrp_includedirs}) - target_link_directories(ldrp_node + target_link_directories(${PROJECT_NAME} PRIVATE ${PCL_LIBRARY_DIRS}) - target_link_libraries(ldrp_node + target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES}) # PRIVATE here causes conflicts with ament_target_dependencies - target_compile_definitions(ldrp_node + target_compile_definitions(${PROJECT_NAME} PRIVATE ${ldrp_defines} PRIVATE ${PCL_DEFINITIONS}) @@ -102,6 +104,8 @@ else() find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) + find_package(tf2_ros REQUIRED) + find_package(tf2_sensor_msgs REQUIRED) find_package(pcl_conversions REQUIRED) find_package(pcl_ros REQUIRED) # find_package(tf2_msgs REQUIRED) @@ -189,30 +193,31 @@ endif() list(APPEND ldrp_sources "${ldrp_src}/main.cpp" "${ldrp_src}/ldrp/perception.cpp") list(APPEND ldrp_includedirs) - add_executable(ldrp_node ${ldrp_sources}) + add_executable(${PROJECT_NAME} ${ldrp_sources}) - target_include_directories(ldrp_node + target_include_directories(${PROJECT_NAME} PRIVATE ${PCL_INCLUDE_DIRS} PRIVATE ${ldrp_includedirs}) - target_link_directories(ldrp_node + target_link_directories(${PROJECT_NAME} PRIVATE ${PCL_LIBRARY_DIRS}) - target_link_libraries(ldrp_node + target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES}) # PRIVATE here causes conflicts with ament_target_dependencies - target_compile_definitions(ldrp_node + target_compile_definitions(${PROJECT_NAME} PRIVATE ${ldrp_defines} PRIVATE ${PCL_DEFINITIONS}) - ament_target_dependencies(ldrp_node + ament_target_dependencies(${PROJECT_NAME} "rclcpp" "pcl_conversions" "pcl_ros" "tf2_ros" + "tf2_sensor_msgs" "nav_msgs" "sensor_msgs" "geometry_msgs") install( - TARGETS ldrp_node + TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME}) install( DIRECTORY config launch diff --git a/README.md b/README.md index 37b1cc0..7f50747 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,7 @@ [![CI](https://github.com/Cardinal-Space-Mining/Sick-Perception/actions/workflows/ci.yml/badge.svg?branch=main)](https://github.com/Cardinal-Space-Mining/Sick-Perception/actions/workflows/ci.yml) # Sick-Perception +*Don't be fooled, this is actually a mapping package!* ## ⚠️ IMPORTANT ⚠️ Previous versions (branches) of this project were architected to be standalone "pure-C++" modules, however, __this branch has been updated to target ROS\[2\], and offloads sensor interfacing and localization pipeline prerequisites to various external ROS packages__, which are discuessed below. To view the old architecture, check out the [perception-v1](/Cardinal-Space-Mining/Sick-Perception/tree/perception-v1) and/or [perception-v2](/Cardinal-Space-Mining/Sick-Perception/tree/perception-v2) branches. These branches are not likely to be maintained, and as a result may not reflect the same functionality as this branch in the future. diff --git a/config/params.yaml b/config/params.yaml index 22ad3db..20f1c7c 100644 --- a/config/params.yaml +++ b/config/params.yaml @@ -1,7 +1,9 @@ /**: ros__parameters: - scan_matching_history_s: 0.25 + scan_topic: "/cardinal_perception/filtered_scan" + + max_tf_wait_time_s: 0.25 map_resolution_cm: 5.0 voxel_size_cm: 3.0 @@ -16,4 +18,5 @@ pmf_max_distance_cm: 12.0 pmf_slope: 2.0 - output_frame: "world" + map_frame: "map" + lidar_frame: "lidar_link" diff --git a/launch/ldrp.launch.py b/launch/ldrp.launch.py deleted file mode 100644 index 19bc8c7..0000000 --- a/launch/ldrp.launch.py +++ /dev/null @@ -1,44 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - current_pkg = FindPackageShare('sick_perception') - - pointcloud_topic_cfg = LaunchConfiguration('pointcloud_topic', default='/uesim/scan') - declare_pointcloud_topic_arg = DeclareLaunchArgument( - 'pointcloud_topic', - default_value = pointcloud_topic_cfg, - description = 'Input point cloud topic name' - ) - - pose_topic_cfg = LaunchConfiguration('pose_topic', default = '/uesim/pose') - declare_pose_topic_arg = DeclareLaunchArgument( - 'pose_topic', - default_value = pose_topic_cfg, - description = 'Input pose topic name' - ) - - params_yaml_path = PathJoinSubstitution([current_pkg, 'config', 'params.yaml']) - - ldrp_node = Node( - name = 'ldrp', - package = 'sick_perception', - executable = 'ldrp_node', - output = 'screen', - parameters = [params_yaml_path], - remappings = [ - ('scan', pointcloud_topic_cfg), - ('pose', pose_topic_cfg) - ] - ) - - return LaunchDescription([ - declare_pointcloud_topic_arg, - declare_pose_topic_arg, - ldrp_node - ]) \ No newline at end of file diff --git a/launch/sick_perception.launch.py b/launch/sick_perception.launch.py new file mode 100644 index 0000000..22ae54f --- /dev/null +++ b/launch/sick_perception.launch.py @@ -0,0 +1,32 @@ +import os + +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 + + +def generate_launch_description(): + + pkg_path = get_package_share_directory('sick_perception') + param_file = os.path.join(pkg_path, 'config', 'params.yaml') + + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + + map_node = Node( + name = 'sick_perception', + package = 'sick_perception', + executable = 'sick_perception', + output = 'screen', + parameters = [param_file, {'use_sim_time': use_sim_time}], + remappings = [ + ('obstacle_grid', '/sick_perception/obstacle_grid') + ] + ) + + return LaunchDescription([ + DeclareLaunchArgument('use_sim_time', default_value='false'), + map_node + ]) diff --git a/package.xml b/package.xml index 84eed1c..1c5681f 100644 --- a/package.xml +++ b/package.xml @@ -4,7 +4,7 @@ sick_perception 0.1.0 LiDAR Obstacle Detection Mapping - sigma + Sam Richter MIT rclcpp @@ -12,6 +12,8 @@ sensor_msgs geometry_msgs nav_msgs + tf2_ros + tf2_sensor_msgs pcl_conversions pcl_ros diff --git a/src/ldrp/filtering.hpp b/src/ldrp/filtering.hpp index 48d07f4..534074e 100644 --- a/src/ldrp/filtering.hpp +++ b/src/ldrp/filtering.hpp @@ -17,76 +17,75 @@ #include // includes - /** Get the min/max of first N dimensions for a selection of points. Does not properly handle non-dense clouds. */ template< - int Ndims = 3, - typename PointT = pcl::PointXYZ, - typename IntT = pcl::index_t, - typename FloatT = float> + int Ndims = 3, + typename PointT = pcl::PointXYZ, + typename IntT = pcl::index_t, + typename FloatT = float> void minMaxND( - const pcl::PointCloud& cloud, - const std::vector& selection, - Eigen::Vector& min, - Eigen::Vector& max + const pcl::PointCloud& cloud, + const std::vector& selection, + Eigen::Vector& min, + Eigen::Vector& max ) { - static_assert(Ndims > 0 && Ndims <= 4, ""); - using VecT = Eigen::Vector; - - min.setConstant( std::numeric_limits::max() ); - max.setConstant( std::numeric_limits::min() ); - - if(selection.empty()) { - for(const PointT& pt : cloud.points) { - const VecT* pt2 = reinterpret_cast(&pt); - min = min.cwiseMin(*pt2); - max = max.cwiseMax(*pt2); - } - } else { - for(const IntT i : selection) { - const VecT* pt2 = reinterpret_cast(&cloud.points[i]); - min = min.cwiseMin(*pt2); - max = max.cwiseMax(*pt2); - } - } + static_assert(Ndims > 0 && Ndims <= 4, ""); + using VecT = Eigen::Vector; + + min.setConstant( std::numeric_limits::max() ); + max.setConstant( std::numeric_limits::min() ); + + if(selection.empty()) { + for(const PointT& pt : cloud.points) { + const VecT* pt2 = reinterpret_cast(&pt); + min = min.cwiseMin(*pt2); + max = max.cwiseMax(*pt2); + } + } else { + for(const IntT i : selection) { + const VecT* pt2 = reinterpret_cast(&cloud.points[i]); + min = min.cwiseMin(*pt2); + max = max.cwiseMax(*pt2); + } + } } /** minMaxND<>() alias for getting min/max for x and y */ template< - typename PointT = pcl::PointXYZ, - typename IntT = pcl::index_t, - typename FloatT = float> + typename PointT = pcl::PointXYZ, + typename IntT = pcl::index_t, + typename FloatT = float> inline void minMaxXY( - const pcl::PointCloud& cloud, - const std::vector& selection, - Eigen::Vector2& min, - Eigen::Vector2& max + const pcl::PointCloud& cloud, + const std::vector& selection, + Eigen::Vector2& min, + Eigen::Vector2& max ) { - return minMaxND<2, PointT, IntT, FloatT>( - cloud, - selection, - min, - max - ); + return minMaxND<2, PointT, IntT, FloatT>( + cloud, + selection, + min, + max + ); } /** minMaxND<>() alias for getting min/max for x, y, and z */ template< - typename PointT = pcl::PointXYZ, - typename IntT = pcl::index_t, - typename FloatT = float> + typename PointT = pcl::PointXYZ, + typename IntT = pcl::index_t, + typename FloatT = float> inline void minMaxXYZ( - const pcl::PointCloud& cloud, - const std::vector& selection, - Eigen::Vector3& min, - Eigen::Vector3& max + const pcl::PointCloud& cloud, + const std::vector& selection, + Eigen::Vector3& min, + Eigen::Vector3& max ) { - return minMaxND<3, PointT, IntT, FloatT>( - cloud, - selection, - min, - max - ); + return minMaxND<3, PointT, IntT, FloatT>( + cloud, + selection, + min, + max + ); } @@ -96,155 +95,155 @@ inline void minMaxXYZ( /** Voxelization static reimpl -- copied from VoxelGrid<>::applyFilter() and simplified */ template< - typename PointT = pcl::PointXYZ, - typename IntT = pcl::index_t, - typename FloatT = float> + typename PointT = pcl::PointXYZ, + typename IntT = pcl::index_t, + typename FloatT = float> void voxel_filter( - const pcl::PointCloud& cloud, - const std::vector& selection, - pcl::PointCloud& voxelized, - FloatT leaf_x, FloatT leaf_y, FloatT leaf_z, - unsigned int min_points_per_voxel_ = 0, - bool downsample_all_data_ = false + const pcl::PointCloud& cloud, + const std::vector& selection, + pcl::PointCloud& voxelized, + FloatT leaf_x, FloatT leaf_y, FloatT leaf_z, + unsigned int min_points_per_voxel_ = 0, + bool downsample_all_data_ = false ) { - const bool use_selection = !selection.empty(); - - const Eigen::Vector3 - leaf_size_{ leaf_x, leaf_y, leaf_z }; - const Eigen::Array3 - inverse_leaf_size_{ Eigen::Array3::Ones() / leaf_size_.array() }; - - // Copy the header (and thus the frame_id) + allocate enough space for points - voxelized.height = 1; // downsampling breaks the organized structure - voxelized.is_dense = true; // we filter out invalid points - - // Eigen::Vector4f min_p, max_p; - // // Get the minimum and maximum dimensions - // if(use_selection) { - // pcl::getMinMax3D(cloud, selection, min_p, max_p); - // } else { - // pcl::getMinMax3D(cloud, min_p, max_p); - // } - Eigen::Vector3 min_p, max_p; - minMaxXYZ(cloud, selection, min_p, max_p); - - // Check that the leaf size is not too small, given the size of the data - std::int64_t dx = static_cast((max_p[0] - min_p[0]) * inverse_leaf_size_[0]) + 1; - std::int64_t dy = static_cast((max_p[1] - min_p[1]) * inverse_leaf_size_[1]) + 1; - std::int64_t dz = static_cast((max_p[2] - min_p[2]) * inverse_leaf_size_[2]) + 1; - - if( (dx * dy * dz) > static_cast(std::numeric_limits::max()) ) { - voxelized.clear(); - return; - } - - Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_; - - // Compute the minimum and maximum bounding box values - min_b_[0] = static_cast ( std::floor(min_p[0] * inverse_leaf_size_[0]) ); - max_b_[0] = static_cast ( std::floor(max_p[0] * inverse_leaf_size_[0]) ); - min_b_[1] = static_cast ( std::floor(min_p[1] * inverse_leaf_size_[1]) ); - max_b_[1] = static_cast ( std::floor(max_p[1] * inverse_leaf_size_[1]) ); - min_b_[2] = static_cast ( std::floor(min_p[2] * inverse_leaf_size_[2]) ); - max_b_[2] = static_cast ( std::floor(max_p[2] * inverse_leaf_size_[2]) ); - - // Compute the number of divisions needed along all axis - div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones (); - div_b_[3] = 0; - - // Set up the division multiplier - divb_mul_ = Eigen::Vector4i{ 1, div_b_[0], div_b_[0] * div_b_[1], 0 }; - - // Storage for mapping leaf and pointcloud indexes - std::vector index_vector; - - // First pass: go over all points and insert them into the index_vector vector - // with calculated idx. Points with the same idx value will contribute to the - // same point of resulting CloudPoint - if(use_selection) { - index_vector.reserve(selection.size()); - for(const auto& index : selection) { - if(!cloud.is_dense && !pcl::isXYZFinite(cloud[index])) continue; - - int ijk0 = static_cast( std::floor(cloud[index].x * inverse_leaf_size_[0]) - static_cast(min_b_[0]) ); - int ijk1 = static_cast( std::floor(cloud[index].y * inverse_leaf_size_[1]) - static_cast(min_b_[1]) ); - int ijk2 = static_cast( std::floor(cloud[index].z * inverse_leaf_size_[2]) - static_cast(min_b_[2]) ); - - // Compute the centroid leaf index - int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; - index_vector.emplace_back( static_cast(idx), index ); - } - } else { - index_vector.reserve(cloud.size()); - for(IntT index = 0; index < cloud.size(); index++) { - if(!cloud.is_dense && !pcl::isXYZFinite(cloud[index])) continue; - - int ijk0 = static_cast( std::floor(cloud[index].x * inverse_leaf_size_[0]) - static_cast(min_b_[0]) ); - int ijk1 = static_cast( std::floor(cloud[index].y * inverse_leaf_size_[1]) - static_cast(min_b_[1]) ); - int ijk2 = static_cast( std::floor(cloud[index].z * inverse_leaf_size_[2]) - static_cast(min_b_[2]) ); - - // Compute the centroid leaf index - int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; - index_vector.emplace_back( static_cast(idx), index ); - } - } - - // Second pass: sort the index_vector vector using value representing target cell as index - // in effect all points belonging to the same output cell will be next to each other - auto rightshift_func = [](const cloud_point_index_idx &x, const unsigned offset) { return x.idx >> offset; }; - boost::sort::spreadsort::integer_sort(index_vector.begin(), index_vector.end(), rightshift_func); - - // Third pass: count output cells - // we need to skip all the same, adjacent idx values - unsigned int total = 0; - unsigned int index = 0; - // first_and_last_indices_vector[i] represents the index in index_vector of the first point in - // index_vector belonging to the voxel which corresponds to the i-th output point, - // and of the first point not belonging to. - std::vector > first_and_last_indices_vector; - // Worst case size - first_and_last_indices_vector.reserve (index_vector.size()); - while(index < index_vector.size()) { - unsigned int i = index + 1; - for(; i < index_vector.size() && index_vector[i].idx == index_vector[index].idx; ++i); - if (i - index >= min_points_per_voxel_) { - ++total; - first_and_last_indices_vector.emplace_back(index, i); - } - index = i; - } - - // Fourth pass: compute centroids, insert them into their final position - voxelized.resize(total); - - index = 0; - for (const auto &cp : first_and_last_indices_vector) { - // calculate centroid - sum values from all input points, that have the same idx value in index_vector array - unsigned int first_index = cp.first; - unsigned int last_index = cp.second; - - //Limit downsampling to coords - if (!downsample_all_data_) { - Eigen::Vector4f centroid{ Eigen::Vector4f::Zero() }; - - for (unsigned int li = first_index; li < last_index; ++li) { - centroid += cloud[index_vector[li].cloud_point_index].getVector4fMap(); - } - centroid /= static_cast (last_index - first_index); - voxelized[index].getVector4fMap() = centroid; - } - else { - pcl::CentroidPoint centroid; - - // fill in the accumulator with leaf points - for (unsigned int li = first_index; li < last_index; ++li) { - centroid.add( cloud[index_vector[li].cloud_point_index] ); - } - centroid.get(voxelized[index]); - } - ++index; - } - voxelized.width = voxelized.size (); + const bool use_selection = !selection.empty(); + + const Eigen::Vector3 + leaf_size_{ leaf_x, leaf_y, leaf_z }; + const Eigen::Array3 + inverse_leaf_size_{ Eigen::Array3::Ones() / leaf_size_.array() }; + + // Copy the header (and thus the frame_id) + allocate enough space for points + voxelized.height = 1; // downsampling breaks the organized structure + voxelized.is_dense = true; // we filter out invalid points + + // Eigen::Vector4f min_p, max_p; + // // Get the minimum and maximum dimensions + // if(use_selection) { + // pcl::getMinMax3D(cloud, selection, min_p, max_p); + // } else { + // pcl::getMinMax3D(cloud, min_p, max_p); + // } + Eigen::Vector3 min_p, max_p; + minMaxXYZ(cloud, selection, min_p, max_p); + + // Check that the leaf size is not too small, given the size of the data + std::int64_t dx = static_cast((max_p[0] - min_p[0]) * inverse_leaf_size_[0]) + 1; + std::int64_t dy = static_cast((max_p[1] - min_p[1]) * inverse_leaf_size_[1]) + 1; + std::int64_t dz = static_cast((max_p[2] - min_p[2]) * inverse_leaf_size_[2]) + 1; + + if( (dx * dy * dz) > static_cast(std::numeric_limits::max()) ) { + voxelized.clear(); + return; + } + + Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_; + + // Compute the minimum and maximum bounding box values + min_b_[0] = static_cast ( std::floor(min_p[0] * inverse_leaf_size_[0]) ); + max_b_[0] = static_cast ( std::floor(max_p[0] * inverse_leaf_size_[0]) ); + min_b_[1] = static_cast ( std::floor(min_p[1] * inverse_leaf_size_[1]) ); + max_b_[1] = static_cast ( std::floor(max_p[1] * inverse_leaf_size_[1]) ); + min_b_[2] = static_cast ( std::floor(min_p[2] * inverse_leaf_size_[2]) ); + max_b_[2] = static_cast ( std::floor(max_p[2] * inverse_leaf_size_[2]) ); + + // Compute the number of divisions needed along all axis + div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones (); + div_b_[3] = 0; + + // Set up the division multiplier + divb_mul_ = Eigen::Vector4i{ 1, div_b_[0], div_b_[0] * div_b_[1], 0 }; + + // Storage for mapping leaf and pointcloud indexes + std::vector index_vector; + + // First pass: go over all points and insert them into the index_vector vector + // with calculated idx. Points with the same idx value will contribute to the + // same point of resulting CloudPoint + if(use_selection) { + index_vector.reserve(selection.size()); + for(const auto& index : selection) { + if(!cloud.is_dense && !pcl::isXYZFinite(cloud[index])) continue; + + int ijk0 = static_cast( std::floor(cloud[index].x * inverse_leaf_size_[0]) - static_cast(min_b_[0]) ); + int ijk1 = static_cast( std::floor(cloud[index].y * inverse_leaf_size_[1]) - static_cast(min_b_[1]) ); + int ijk2 = static_cast( std::floor(cloud[index].z * inverse_leaf_size_[2]) - static_cast(min_b_[2]) ); + + // Compute the centroid leaf index + int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; + index_vector.emplace_back( static_cast(idx), index ); + } + } else { + index_vector.reserve(cloud.size()); + for(IntT index = 0; index < cloud.size(); index++) { + if(!cloud.is_dense && !pcl::isXYZFinite(cloud[index])) continue; + + int ijk0 = static_cast( std::floor(cloud[index].x * inverse_leaf_size_[0]) - static_cast(min_b_[0]) ); + int ijk1 = static_cast( std::floor(cloud[index].y * inverse_leaf_size_[1]) - static_cast(min_b_[1]) ); + int ijk2 = static_cast( std::floor(cloud[index].z * inverse_leaf_size_[2]) - static_cast(min_b_[2]) ); + + // Compute the centroid leaf index + int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; + index_vector.emplace_back( static_cast(idx), index ); + } + } + + // Second pass: sort the index_vector vector using value representing target cell as index + // in effect all points belonging to the same output cell will be next to each other + auto rightshift_func = [](const cloud_point_index_idx &x, const unsigned offset) { return x.idx >> offset; }; + boost::sort::spreadsort::integer_sort(index_vector.begin(), index_vector.end(), rightshift_func); + + // Third pass: count output cells + // we need to skip all the same, adjacent idx values + unsigned int total = 0; + unsigned int index = 0; + // first_and_last_indices_vector[i] represents the index in index_vector of the first point in + // index_vector belonging to the voxel which corresponds to the i-th output point, + // and of the first point not belonging to. + std::vector > first_and_last_indices_vector; + // Worst case size + first_and_last_indices_vector.reserve (index_vector.size()); + while(index < index_vector.size()) { + unsigned int i = index + 1; + for(; i < index_vector.size() && index_vector[i].idx == index_vector[index].idx; ++i); + if (i - index >= min_points_per_voxel_) { + ++total; + first_and_last_indices_vector.emplace_back(index, i); + } + index = i; + } + + // Fourth pass: compute centroids, insert them into their final position + voxelized.resize(total); + + index = 0; + for (const auto &cp : first_and_last_indices_vector) { + // calculate centroid - sum values from all input points, that have the same idx value in index_vector array + unsigned int first_index = cp.first; + unsigned int last_index = cp.second; + + //Limit downsampling to coords + if (!downsample_all_data_) { + Eigen::Vector4f centroid{ Eigen::Vector4f::Zero() }; + + for (unsigned int li = first_index; li < last_index; ++li) { + centroid += cloud[index_vector[li].cloud_point_index].getVector4fMap(); + } + centroid /= static_cast (last_index - first_index); + voxelized[index].getVector4fMap() = centroid; + } + else { + pcl::CentroidPoint centroid; + + // fill in the accumulator with leaf points + for (unsigned int li = first_index; li < last_index; ++li) { + centroid.add( cloud[index_vector[li].cloud_point_index] ); + } + centroid.get(voxelized[index]); + } + ++index; + } + voxelized.width = voxelized.size (); } @@ -254,53 +253,53 @@ void voxel_filter( /** Cartesian "crop box" filter reimpl -- copied from pcl::CropBox<>::applyFilter() and simplified */ template< - typename PointT = pcl::PointXYZ, - typename IntT = pcl::index_t, - bool negative_ = false> + typename PointT = pcl::PointXYZ, + typename IntT = pcl::index_t, + bool negative_ = false> void cropbox_filter( - const pcl::PointCloud& cloud, - const std::vector& selection, - std::vector& filtered, - const Eigen::Vector3f min_pt_ = Eigen::Vector3f{ -1.f, -1.f, -1.f }, - const Eigen::Vector3f max_pt_ = Eigen::Vector3f{ 1.f, 1.f, 1.f } - // full CropBox<> also allows additional transformation of box and cloud + const pcl::PointCloud& cloud, + const std::vector& selection, + std::vector& filtered, + const Eigen::Vector3f min_pt_ = Eigen::Vector3f{ -1.f, -1.f, -1.f }, + const Eigen::Vector3f max_pt_ = Eigen::Vector3f{ 1.f, 1.f, 1.f } + // full CropBox<> also allows additional transformation of box and cloud ) { - const bool use_selection = !selection.empty(); - - filtered.clear(); - filtered.reserve(use_selection ? selection.size() : cloud.size()); // reserve maximum size - - if(use_selection) { - for (const IntT index : selection) { - const PointT& pt = cloud[index]; - if( !cloud.is_dense && !isFinite(pt) ) continue; - - if( (pt.x < min_pt_[0] || pt.y < min_pt_[1] || pt.z < min_pt_[2]) || - (pt.x > max_pt_[0] || pt.y > max_pt_[1] || pt.z > max_pt_[2]) ) - { - if constexpr(negative_) { - filtered.push_back(index); // outside the cropbox --> push on negative - } - } else if constexpr(!negative_) { - filtered.push_back(index); // inside the cropbox and not negative - } - } - } else { - for (size_t index = 0; index < cloud.points.size(); index++) { - const PointT& pt = cloud[index]; - if( !cloud.is_dense && !isFinite(pt) ) continue; - - if( (pt.x < min_pt_[0] || pt.y < min_pt_[1] || pt.z < min_pt_[2]) || - (pt.x > max_pt_[0] || pt.y > max_pt_[1] || pt.z > max_pt_[2]) ) - { - if constexpr(negative_) { - filtered.push_back(index); // outside the cropbox --> push on negative - } - } else if constexpr(!negative_) { - filtered.push_back(index); // inside the cropbox and not negative - } - } - } + const bool use_selection = !selection.empty(); + + filtered.clear(); + filtered.reserve(use_selection ? selection.size() : cloud.size()); // reserve maximum size + + if(use_selection) { + for (const IntT index : selection) { + const PointT& pt = cloud[index]; + if( !cloud.is_dense && !isFinite(pt) ) continue; + + if( (pt.x < min_pt_[0] || pt.y < min_pt_[1] || pt.z < min_pt_[2]) || + (pt.x > max_pt_[0] || pt.y > max_pt_[1] || pt.z > max_pt_[2]) ) + { + if constexpr(negative_) { + filtered.push_back(index); // outside the cropbox --> push on negative + } + } else if constexpr(!negative_) { + filtered.push_back(index); // inside the cropbox and not negative + } + } + } else { + for (size_t index = 0; index < cloud.points.size(); index++) { + const PointT& pt = cloud[index]; + if( !cloud.is_dense && !isFinite(pt) ) continue; + + if( (pt.x < min_pt_[0] || pt.y < min_pt_[1] || pt.z < min_pt_[2]) || + (pt.x > max_pt_[0] || pt.y > max_pt_[1] || pt.z > max_pt_[2]) ) + { + if constexpr(negative_) { + filtered.push_back(index); // outside the cropbox --> push on negative + } + } else if constexpr(!negative_) { + filtered.push_back(index); // inside the cropbox and not negative + } + } + } } @@ -308,47 +307,47 @@ void cropbox_filter( /** cropbox_filter<>() specialization for sorting exclusively using the z-coordinate */ template< - typename PointT = pcl::PointXYZ, - typename IntT = pcl::index_t, - typename FloatT = float, - bool negative_ = false> + typename PointT = pcl::PointXYZ, + typename IntT = pcl::index_t, + typename FloatT = float, + bool negative_ = false> void carteZ_filter( - const pcl::PointCloud& cloud, - const std::vector& selection, - std::vector& filtered, - const FloatT min_z = -1.f, - const FloatT max_z = 1.f + const pcl::PointCloud& cloud, + const std::vector& selection, + std::vector& filtered, + const FloatT min_z = -1.f, + const FloatT max_z = 1.f ) { - const bool use_selection = !selection.empty(); - - filtered.clear(); - filtered.reserve(use_selection ? selection.size() : cloud.size()); // reserve maximum size - - if(use_selection) { - for (const IntT index : selection) { - const PointT& pt = cloud[index]; - if( !cloud.is_dense && !isFinite(pt) ) continue; - if( pt.z < min_z || pt.z > max_z ) { - if constexpr(negative_) { - filtered.push_back(index); // outside the cropbox --> push on negative - } - } else if constexpr(!negative_) { - filtered.push_back(index); // inside the cropbox and not negative - } - } - } else { - for (size_t index = 0; index < cloud.points.size(); index++) { - const PointT& pt = cloud[index]; - if( !cloud.is_dense && !isFinite(pt) ) continue; - if( pt.z < min_z || pt.z > max_z ) { - if constexpr(negative_) { - filtered.push_back(index); // outside the cropbox --> push on negative - } - } else if constexpr(!negative_) { - filtered.push_back(index); // inside the cropbox and not negative - } - } - } + const bool use_selection = !selection.empty(); + + filtered.clear(); + filtered.reserve(use_selection ? selection.size() : cloud.size()); // reserve maximum size + + if(use_selection) { + for (const IntT index : selection) { + const PointT& pt = cloud[index]; + if( !cloud.is_dense && !isFinite(pt) ) continue; + if( pt.z < min_z || pt.z > max_z ) { + if constexpr(negative_) { + filtered.push_back(index); // outside the cropbox --> push on negative + } + } else if constexpr(!negative_) { + filtered.push_back(index); // inside the cropbox and not negative + } + } + } else { + for (size_t index = 0; index < cloud.points.size(); index++) { + const PointT& pt = cloud[index]; + if( !cloud.is_dense && !isFinite(pt) ) continue; + if( pt.z < min_z || pt.z > max_z ) { + if constexpr(negative_) { + filtered.push_back(index); // outside the cropbox --> push on negative + } + } else if constexpr(!negative_) { + filtered.push_back(index); // inside the cropbox and not negative + } + } + } } @@ -358,168 +357,168 @@ void carteZ_filter( /** PMF filter reimpl -- See */ template< - // bool mirror_z = false, - typename PointT = pcl::PointXYZ, - typename IntT = pcl::index_t> + // bool mirror_z = false, + typename PointT = pcl::PointXYZ, + typename IntT = pcl::index_t> void progressive_morph_filter( - const pcl::PointCloud& cloud_, - const std::vector& selection, - pcl::Indices& ground, - const float base_, - const float max_window_size_, - const float cell_size_, - const float initial_distance_, - const float max_distance_, - const float slope_, - const bool exponential_ + const pcl::PointCloud& cloud_, + const std::vector& selection, + pcl::Indices& ground, + const float base_, + const float max_window_size_, + const float cell_size_, + const float initial_distance_, + const float max_distance_, + const float slope_, + const bool exponential_ ) { - // Compute the series of window sizes and height thresholds - std::vector height_thresholds; - std::vector window_sizes; - - float window_size = 0.0f; - float height_threshold = 0.0f; - for(size_t itr = 0; window_size < max_window_size_; itr++) { - - // Determine the initial window size. - if(exponential_ && base_ >= 1.f) { - window_size = cell_size_ * (2.0f * std::pow(base_, itr) + 1.0f); // << this becomes an issue when base_ is less than 0 since the loop never exits! :O - } else { - window_size = cell_size_ * (2.0f * (itr + 1) * base_ + 1.0f); - } - - // Calculate the height threshold to be used in the next iteration. - if(itr == 0) { - height_threshold = initial_distance_; - } else { - height_threshold = slope_ * (window_size - window_sizes[itr - 1]) * cell_size_ + initial_distance_; - } - - // Enforce max distance on height threshold - if(height_threshold > max_distance_) height_threshold = max_distance_; - - window_sizes.push_back(window_size); - height_thresholds.push_back(height_threshold); - - } - - // Ground indices are initially limited to those points in the input cloud we wish to process - if (selection.size() > 0 && selection.size() <= cloud_.size()) { - ground = selection; - } else { - ground.resize(cloud_.size()); - for (std::size_t i = 0; i < cloud_.size(); i++) { - ground[i] = i; - } - } - - pcl::octree::OctreePointCloudSearch tree{ 1.f }; - const std::shared_ptr< const pcl::PointCloud > - cloud_shared_ref{ &cloud_, [](const pcl::PointCloud*){} }; - const std::shared_ptr< const pcl::Indices > - ground_shared_ref{ &ground, [](const pcl::Indices*){} }; - - // reused buffers - std::vector pt_window_indices{}; - std::vector - zp_temp{}, zp_final{}, zn_temp{}, zn_final{}; - zp_temp.resize(cloud_.size()); - zp_final.resize(cloud_.size()); - zn_temp.resize(cloud_.size()); - zn_final.resize(cloud_.size()); - - // Progressively filter ground returns using morphological open - for(size_t i = 0; i < window_sizes.size(); i++) { - - // reset tree and reinit to new window size and narrowed selection of points - tree.deleteTree(); - tree.setResolution(window_sizes[i]); - tree.setInputCloud(cloud_shared_ref, ground_shared_ref); // points in the tree will be in the domain of the full cloud - tree.addPointsFromInputCloud(); - - pt_window_indices.resize(ground.size()); - - const float half_res = window_sizes[i] / 2.0f; - // calculate points within each window (for each point in the selection) - for(size_t _idx = 0; _idx < ground.size(); _idx++) { - const PointT& _pt = cloud_[ground[_idx]]; // retrieve source (x, y) for each pt in selection - tree.boxSearch( - Eigen::Vector3f{ - _pt.x - half_res, - _pt.y - half_res, - -std::numeric_limits::max() - }, - Eigen::Vector3f{ - _pt.x + half_res, - _pt.y + half_res, - std::numeric_limits::max() - }, - pt_window_indices[_idx] // output into the cache - ); - } - - // morph op stage 1 - for(size_t p_idx = 0; p_idx < ground.size(); p_idx++) { - - const pcl::Indices& pt_indices = pt_window_indices[p_idx]; - float& _zp_temp = zp_temp[ground[p_idx]]; - float& _zn_temp = zn_temp[ground[p_idx]]; - _zp_temp = _zn_temp = cloud_[ground[p_idx]].z; - - for (const pcl::index_t window_idx : pt_indices) { - const float _z = cloud_[window_idx].z; - if (_z < _zp_temp) { - _zp_temp = _z; - } - if (_z > _zn_temp) { - _zn_temp = _z; - } - } - - } - - // morph op stage 2 - for(size_t p_idx = 0; p_idx < ground.size(); p_idx++) { - - const pcl::Indices& pt_indices = pt_window_indices[p_idx]; - float& _zp_final = zp_final[ground[p_idx]]; - float& _zn_final = zn_final[ground[p_idx]]; - _zp_final = zp_temp[ground[p_idx]]; - _zn_final = zn_temp[ground[p_idx]]; - - for (const pcl::index_t window_idx : pt_indices) { - const float - _zp = zp_temp[window_idx], - _zn = zn_temp[window_idx]; - if (_zp > _zp_final) { - _zp_final = _zp; - } - if (_zn < _zn_final) { - _zn_final = _zn; - } - } - - } - - // Find indices of the points whose difference between the source and - // filtered point clouds is less than the current height threshold. - size_t _slot = 0; - for (size_t p_idx = 0; p_idx < ground.size(); p_idx++) { - - const float - diff_p = cloud_[ground[p_idx]].z - zp_final[ground[p_idx]], - diff_n = zn_final[ground[p_idx]] - cloud_[ground[p_idx]].z; - - if (diff_p < height_thresholds[i] && diff_n < height_thresholds[i]) { // pt is part of ground - ground[_slot] = ground[p_idx]; - _slot++; - } - - } - ground.resize(_slot); - - } + // Compute the series of window sizes and height thresholds + std::vector height_thresholds; + std::vector window_sizes; + + float window_size = 0.0f; + float height_threshold = 0.0f; + for(size_t itr = 0; window_size < max_window_size_; itr++) { + + // Determine the initial window size. + if(exponential_ && base_ >= 1.f) { + window_size = cell_size_ * (2.0f * std::pow(base_, itr) + 1.0f); // << this becomes an issue when base_ is less than 0 since the loop never exits! :O + } else { + window_size = cell_size_ * (2.0f * (itr + 1) * base_ + 1.0f); + } + + // Calculate the height threshold to be used in the next iteration. + if(itr == 0) { + height_threshold = initial_distance_; + } else { + height_threshold = slope_ * (window_size - window_sizes[itr - 1]) * cell_size_ + initial_distance_; + } + + // Enforce max distance on height threshold + if(height_threshold > max_distance_) height_threshold = max_distance_; + + window_sizes.push_back(window_size); + height_thresholds.push_back(height_threshold); + + } + + // Ground indices are initially limited to those points in the input cloud we wish to process + if (selection.size() > 0 && selection.size() <= cloud_.size()) { + ground = selection; + } else { + ground.resize(cloud_.size()); + for (std::size_t i = 0; i < cloud_.size(); i++) { + ground[i] = i; + } + } + + pcl::octree::OctreePointCloudSearch tree{ 1.f }; + const std::shared_ptr< const pcl::PointCloud > + cloud_shared_ref{ &cloud_, [](const pcl::PointCloud*){} }; + const std::shared_ptr< const pcl::Indices > + ground_shared_ref{ &ground, [](const pcl::Indices*){} }; + + // reused buffers + std::vector pt_window_indices{}; + std::vector + zp_temp{}, zp_final{}, zn_temp{}, zn_final{}; + zp_temp.resize(cloud_.size()); + zp_final.resize(cloud_.size()); + zn_temp.resize(cloud_.size()); + zn_final.resize(cloud_.size()); + + // Progressively filter ground returns using morphological open + for(size_t i = 0; i < window_sizes.size(); i++) { + + // reset tree and reinit to new window size and narrowed selection of points + tree.deleteTree(); + tree.setResolution(window_sizes[i]); + tree.setInputCloud(cloud_shared_ref, ground_shared_ref); // points in the tree will be in the domain of the full cloud + tree.addPointsFromInputCloud(); + + pt_window_indices.resize(ground.size()); + + const float half_res = window_sizes[i] / 2.0f; + // calculate points within each window (for each point in the selection) + for(size_t _idx = 0; _idx < ground.size(); _idx++) { + const PointT& _pt = cloud_[ground[_idx]]; // retrieve source (x, y) for each pt in selection + tree.boxSearch( + Eigen::Vector3f{ + _pt.x - half_res, + _pt.y - half_res, + -std::numeric_limits::max() + }, + Eigen::Vector3f{ + _pt.x + half_res, + _pt.y + half_res, + std::numeric_limits::max() + }, + pt_window_indices[_idx] // output into the cache + ); + } + + // morph op stage 1 + for(size_t p_idx = 0; p_idx < ground.size(); p_idx++) { + + const pcl::Indices& pt_indices = pt_window_indices[p_idx]; + float& _zp_temp = zp_temp[ground[p_idx]]; + float& _zn_temp = zn_temp[ground[p_idx]]; + _zp_temp = _zn_temp = cloud_[ground[p_idx]].z; + + for (const pcl::index_t window_idx : pt_indices) { + const float _z = cloud_[window_idx].z; + if (_z < _zp_temp) { + _zp_temp = _z; + } + if (_z > _zn_temp) { + _zn_temp = _z; + } + } + + } + + // morph op stage 2 + for(size_t p_idx = 0; p_idx < ground.size(); p_idx++) { + + const pcl::Indices& pt_indices = pt_window_indices[p_idx]; + float& _zp_final = zp_final[ground[p_idx]]; + float& _zn_final = zn_final[ground[p_idx]]; + _zp_final = zp_temp[ground[p_idx]]; + _zn_final = zn_temp[ground[p_idx]]; + + for (const pcl::index_t window_idx : pt_indices) { + const float + _zp = zp_temp[window_idx], + _zn = zn_temp[window_idx]; + if (_zp > _zp_final) { + _zp_final = _zp; + } + if (_zn < _zn_final) { + _zn_final = _zn; + } + } + + } + + // Find indices of the points whose difference between the source and + // filtered point clouds is less than the current height threshold. + size_t _slot = 0; + for (size_t p_idx = 0; p_idx < ground.size(); p_idx++) { + + const float + diff_p = cloud_[ground[p_idx]].z - zp_final[ground[p_idx]], + diff_n = zn_final[ground[p_idx]] - cloud_[ground[p_idx]].z; + + if (diff_p < height_thresholds[i] && diff_n < height_thresholds[i]) { // pt is part of ground + ground[_slot] = ground[p_idx]; + _slot++; + } + + } + ground.resize(_slot); + + } } @@ -528,248 +527,248 @@ void progressive_morph_filter( /** Given a base set of indices A and a subset of indices B, get (A - B). - * prereq: selection indices must be in ascending order */ + * prereq: selection indices must be in ascending order */ template void pc_negate_selection( - const std::vector& base, - const std::vector& selection, - std::vector& negated + const std::vector& base, + const std::vector& selection, + std::vector& negated ) { - if(base.size() <= selection.size()) { - // negated.clear(); - return; - } - // if(selection.size() == 0) { - // negated = base; - // return; - // } - negated.resize(base.size() - selection.size()); - size_t - _base = 0, - _select = 0, - _negate = 0; - for(; _base < base.size() && _negate < negated.size(); _base++) { - if(_select < selection.size() && base[_base] == selection[_select]) { - _select++; - } else { - negated[_negate] = base[_base]; - _negate++; - } - } + if(base.size() <= selection.size()) { + // negated.clear(); + return; + } + // if(selection.size() == 0) { + // negated = base; + // return; + // } + negated.resize(base.size() - selection.size()); + size_t + _base = 0, + _select = 0, + _negate = 0; + for(; _base < base.size() && _negate < negated.size(); _base++) { + if(_select < selection.size() && base[_base] == selection[_select]) { + _select++; + } else { + negated[_negate] = base[_base]; + _negate++; + } + } } /** Given a base set of indices A and a subset of indices B, get (A - B). - * prereq: selection indices must be in ascending order */ + * prereq: selection indices must be in ascending order */ template void pc_negate_selection( - const IntT base_range, - const std::vector& selection, - std::vector& negated + const IntT base_range, + const std::vector& selection, + std::vector& negated ) { - if (base_range <= selection.size()) { - return; - } - negated.resize(base_range - selection.size()); - size_t - _base = 0, - _select = 0, - _negate = 0; - for (; _base < base_range && _negate < negated.size(); _base++) { - if (_select < selection.size() && _base == selection[_select]) { - _select++; - } else /*if (_base < selection[_select])*/ { - negated[_negate] = _base; - _negate++; - } - } + if (base_range <= selection.size()) { + return; + } + negated.resize(base_range - selection.size()); + size_t + _base = 0, + _select = 0, + _negate = 0; + for (; _base < base_range && _negate < negated.size(); _base++) { + if (_select < selection.size() && _base == selection[_select]) { + _select++; + } else /*if (_base < selection[_select])*/ { + negated[_negate] = _base; + _negate++; + } + } } /** Merge two presorted selections into a single sorted selection (non-descending) - * prereq: both selections must be sorted in non-descending order */ + * prereq: both selections must be sorted in non-descending order */ template void pc_combine_sorted( - const std::vector& sel1, - const std::vector& sel2, - std::vector& out + const std::vector& sel1, + const std::vector& sel2, + std::vector& out ) { - out.clear(); - out.reserve(sel1.size() + sel2.size()); - size_t - _p1 = 0, - _p2 = 0; - for(; _p1 < sel1.size() && _p2 < sel2.size();) { - const IntT - _a = sel1[_p1], - _b = sel2[_p2]; - if(_a <= _b) { - out.push_back(_a); - _p1++; - _p2 += (_a == _b); - } else { - out.push_back(_b); - _p2++; - } - } - for(; _p1 < sel1.size(); _p1++) out.push_back( sel1[_p1] ); - for(; _p2 < sel2.size(); _p2++) out.push_back( sel2[_p2] ); + out.clear(); + out.reserve(sel1.size() + sel2.size()); + size_t + _p1 = 0, + _p2 = 0; + for(; _p1 < sel1.size() && _p2 < sel2.size();) { + const IntT + _a = sel1[_p1], + _b = sel2[_p2]; + if(_a <= _b) { + out.push_back(_a); + _p1++; + _p2 += (_a == _b); + } else { + out.push_back(_b); + _p2++; + } + } + for(; _p1 < sel1.size(); _p1++) out.push_back( sel1[_p1] ); + for(; _p2 < sel2.size(); _p2++) out.push_back( sel2[_p2] ); } /** Remove the points at the each index in the provided set. Prereq: selection indices must be sorted in non-descending order! */ template< - typename PointT = pcl::PointXYZ, - typename AllocT = typename pcl::PointCloud::VectorType::allocator_type, - typename IntT = pcl::index_t> + typename PointT = pcl::PointXYZ, + typename AllocT = typename pcl::PointCloud::VectorType::allocator_type, + typename IntT = pcl::index_t> void pc_remove_selection( - std::vector& points, - const std::vector& selection + std::vector& points, + const std::vector& selection ) { - // assert sizes - size_t last = points.size() - 1; - for(size_t i = 0; i < selection.size(); i++) { - memcpy(&points[selection[i]], &points[last], sizeof(PointT)); - last--; - } - points.resize(last + 1); + // assert sizes + size_t last = points.size() - 1; + for(size_t i = 0; i < selection.size(); i++) { + memcpy(&points[selection[i]], &points[last], sizeof(PointT)); + last--; + } + points.resize(last + 1); } /** Normalize the set of points to only include the selected indices. Prereq: selection indices must be sorted in non-descending order! */ template< - typename PointT = pcl::PointXYZ, - typename AllocT = typename pcl::PointCloud::VectorType::allocator_type, - typename IntT = pcl::index_t> + typename PointT = pcl::PointXYZ, + typename AllocT = typename pcl::PointCloud::VectorType::allocator_type, + typename IntT = pcl::index_t> inline void pc_normalize_selection( - std::vector& points, - const std::vector& selection + std::vector& points, + const std::vector& selection ) { - for(size_t i = 0; i < selection.size(); i++) { - memcpy(&points[i], &points[selection[i]], sizeof(PointT)); - } - points.resize(selection.size()); + for(size_t i = 0; i < selection.size(); i++) { + memcpy(&points[i], &points[selection[i]], sizeof(PointT)); + } + points.resize(selection.size()); } /** Generate a set of ranges for each point in the provided cloud */ template< - typename PointT = pcl::PointXYZ, - typename AllocT = typename pcl::PointCloud::VectorType::allocator_type, - typename IntT = pcl::index_t, - typename FloatT = float> + typename PointT = pcl::PointXYZ, + typename AllocT = typename pcl::PointCloud::VectorType::allocator_type, + typename IntT = pcl::index_t, + typename FloatT = float> void pc_generate_ranges( - const std::vector& points, - const std::vector& selection, - std::vector& out_ranges, - const Eigen::Vector3 origin = Eigen::Vector3::Zero() + const std::vector& points, + const std::vector& selection, + std::vector& out_ranges, + const Eigen::Vector3 origin = Eigen::Vector3::Zero() ) { - if (!selection.empty()) { - out_ranges.resize(selection.size()); - for (int i = 0; i < selection.size(); i++) { - out_ranges[i] = (origin - *reinterpret_cast*>(&points[selection[i]])).norm(); - } - } - else { - out_ranges.resize(points.size()); - for (int i = 0; i < points.size(); i++) { - out_ranges[i] = (origin - *reinterpret_cast*>(&points[i])).norm(); - } - } + if (!selection.empty()) { + out_ranges.resize(selection.size()); + for (int i = 0; i < selection.size(); i++) { + out_ranges[i] = (origin - *reinterpret_cast*>(&points[selection[i]])).norm(); + } + } + else { + out_ranges.resize(points.size()); + for (int i = 0; i < points.size(); i++) { + out_ranges[i] = (origin - *reinterpret_cast*>(&points[i])).norm(); + } + } } /** Filter a set of ranges to an inclusive set of indices */ template< - typename FloatT = float, - typename IntT = pcl::index_t> + typename FloatT = float, + typename IntT = pcl::index_t> void pc_filter_ranges( - const std::vector& ranges, - const std::vector& selection, - std::vector& filtered, - const FloatT min, const FloatT max + const std::vector& ranges, + const std::vector& selection, + std::vector& filtered, + const FloatT min, const FloatT max ) { - filtered.clear(); - if(!selection.empty()) { - filtered.reserve(selection.size()); - for(size_t i = 0; i < selection.size(); i++) { - const IntT idx = selection[i]; - const FloatT r = ranges[idx]; - if(r <= max && r >= min) { - filtered.push_back(idx); - } - } - } else { - filtered.reserve(ranges.size()); - for(size_t i = 0; i < ranges.size(); i++) { - const FloatT r = ranges[i]; - if(r <= max && r >= min) { - filtered.push_back(i); - } - } - } + filtered.clear(); + if(!selection.empty()) { + filtered.reserve(selection.size()); + for(size_t i = 0; i < selection.size(); i++) { + const IntT idx = selection[i]; + const FloatT r = ranges[idx]; + if(r <= max && r >= min) { + filtered.push_back(idx); + } + } + } else { + filtered.reserve(ranges.size()); + for(size_t i = 0; i < ranges.size(); i++) { + const FloatT r = ranges[i]; + if(r <= max && r >= min) { + filtered.push_back(i); + } + } + } } /** Filter a set of points by their distance from a specified origin point (<0, 0, 0> by default) */ template< - typename PointT = pcl::PointXYZ, - typename AllocT = typename pcl::PointCloud::VectorType::allocator_type, - typename IntT = pcl::index_t, - typename FloatT = float> + typename PointT = pcl::PointXYZ, + typename AllocT = typename pcl::PointCloud::VectorType::allocator_type, + typename IntT = pcl::index_t, + typename FloatT = float> void pc_filter_distance( - const std::vector& points, - const std::vector& selection, - std::vector& filtered, - const FloatT min, const FloatT max, - const Eigen::Vector3 origin = Eigen::Vector3::Zero() + const std::vector& points, + const std::vector& selection, + std::vector& filtered, + const FloatT min, const FloatT max, + const Eigen::Vector3 origin = Eigen::Vector3::Zero() ) { - filtered.clear(); - if(!selection.empty()) { - filtered.reserve(selection.size()); - for(size_t i = 0; i < selection.size(); i++) { - const IntT idx = selection[i]; - const FloatT r = (origin - *reinterpret_cast*>(&points[idx])).norm(); - if(r <= max && r >= min) { - filtered.push_back(idx); - } - } - } else { - filtered.reserve(points.size()); - for(size_t i = 0; i < points.size(); i++) { - const FloatT r = (origin - *reinterpret_cast*>(&points[i])).norm(); - if(r <= max && r >= min) { - filtered.push_back(i); - } - } - } + filtered.clear(); + if(!selection.empty()) { + filtered.reserve(selection.size()); + for(size_t i = 0; i < selection.size(); i++) { + const IntT idx = selection[i]; + const FloatT r = (origin - *reinterpret_cast*>(&points[idx])).norm(); + if(r <= max && r >= min) { + filtered.push_back(idx); + } + } + } else { + filtered.reserve(points.size()); + for(size_t i = 0; i < points.size(); i++) { + const FloatT r = (origin - *reinterpret_cast*>(&points[i])).norm(); + if(r <= max && r >= min) { + filtered.push_back(i); + } + } + } } #if __cplusplus > 201703L /** Write an element's bytes to a buffer every 'interlace_rep' number of element spans at an offset of 'interlace_off' in elments spans - * (an element span = sizeof(ElemT)) */ + * (an element span = sizeof(ElemT)) */ template< - size_t interlace_rep = 4, - size_t interlace_off = 3, - typename IntT = pcl::index_t, - typename ElemT = int32_t> + size_t interlace_rep = 4, + size_t interlace_off = 3, + typename IntT = pcl::index_t, + typename ElemT = int32_t> void write_interlaced_selection_bytes( - std::span buffer, - const std::vector& selection, - const ElemT selected, const ElemT unselected + std::span buffer, + const std::vector& selection, + const ElemT selected, const ElemT unselected ) { - static_assert(interlace_off < interlace_rep, ""); - size_t - _buff = 0, - _sel = 0; - for(; _buff < buffer.size() / interlace_rep; _buff++) { - const size_t idx = interlace_rep * _buff + interlace_off; - if(_sel < selection.size() && _buff == selection[_sel]) { - if constexpr(sizeof(ElemT) > 8) { - memcpy(buffer.data() + idx, &selected, sizeof(ElemT)); // the function is meant to do a bit/byte-wise copy so might as well use a more efficient transfer when applicable - } else { - buffer[idx] = selected; - } - _sel++; - } else { - if constexpr(sizeof(ElemT) > 8) { - memcpy(buffer.data() + idx, &unselected, sizeof(ElemT)); - } else { - buffer[idx] = unselected; - } - } - } + static_assert(interlace_off < interlace_rep, ""); + size_t + _buff = 0, + _sel = 0; + for(; _buff < buffer.size() / interlace_rep; _buff++) { + const size_t idx = interlace_rep * _buff + interlace_off; + if(_sel < selection.size() && _buff == selection[_sel]) { + if constexpr(sizeof(ElemT) > 8) { + memcpy(buffer.data() + idx, &selected, sizeof(ElemT)); // the function is meant to do a bit/byte-wise copy so might as well use a more efficient transfer when applicable + } else { + buffer[idx] = selected; + } + _sel++; + } else { + if constexpr(sizeof(ElemT) > 8) { + memcpy(buffer.data() + idx, &unselected, sizeof(ElemT)); + } else { + buffer[idx] = unselected; + } + } + } } #endif diff --git a/src/ldrp/grid.hpp b/src/ldrp/grid.hpp index 921b127..4db01d9 100644 --- a/src/ldrp/grid.hpp +++ b/src/ldrp/grid.hpp @@ -16,213 +16,213 @@ /** Generic grid helpers */ namespace GridUtils { - /** Align a point to a box grid of the given resolution and offset origin. Result may be negative if lower than current offset. */ - template - inline static Eigen::Vector2 gridAlign(FloatT x, FloatT y, const Eigen::Vector2& off, FloatT res) { - return Eigen::Vector2{ - static_cast( std::floor((x - off.x()) / res) ), // always floor since grid cells are indexed by their "bottom left" corner's raw position - static_cast( std::floor((y - off.y()) / res) ) - }; - } - template - inline static Eigen::Vector2 gridAlign(const Eigen::Vector4& pt, const Eigen::Vector2& off, FloatT res) { - return gridAlign(pt.x(), pt.y(), off, res); - } - - /** Get a raw buffer idx from a 2d index and buffer size (templated on major-order) */ - template - inline static int64_t gridIdx(const IntT x, const IntT y, const Eigen::Vector2& size) { - if constexpr(X_Major) { - return static_cast(x) * size.y() + y; // x-major = "contiguous blocks along [parallel to] y-axis" --> idx = (x * ymax) + y - } else { - return static_cast(y) * size.x() + x; // y-major = "contiguous blocks along [parallel to] x-axis" --> idx = (y * xmax) + x - } - } - template - inline static int64_t gridIdx(const Eigen::Vector2& loc, const Eigen::Vector2& size) { - return gridIdx(loc.x(), loc.y(), size); - } - - /** Get the 2d location corresponding to a raw buffer idx for the provded grid size (templated on major-order) */ - template - inline static Eigen::Vector2 gridLoc(const size_t idx, const Eigen::Vector2& size) { - if constexpr(X_Major) { - return Eigen::Vector2{ // x-major = "contiguous blocks along [parallel to] y-axis" --> x = idx / ymax, y = idx % ymax - static_cast(idx / size.y()), - static_cast(idx % size.y()) - }; - } else { - return Eigen::Vector2{ // y-major = "contiguous blocks along [parallel to] x-axis" --> x = idx % xmax, y = idx / xmax - static_cast(idx % size.x()), - static_cast(idx / size.x()) - }; - } - } - - /** Copy a 2d windows of elemens - expects element types to be POD (templated on major-order) */ - template - static void memcpyWindow( - T* dest, const T* src, - const Eigen::Vector2& dest_size, const Eigen::Vector2& src_size, - const Eigen::Vector2& diff - ) { - if constexpr(X_Major) { - for(int64_t _x = 0; _x < src_size.x(); _x++) { // iterate through source "rows" of contiguous memory (along y -- for each x) - memcpy( - dest + ((_x + diff.x()) * dest_size.y() + diff.y()), - src + (_x * src_size.y()), - src_size.y() * T_Bytes - ); - } - } else { - for(int64_t _y = 0; _y < src_size.y(); _y++) { // iterate through source "rows" of contiguous memory (along x -- for each y) - memcpy( - dest + ((_y + diff.y()) * dest_size.x() + diff.x()), - src + (_y * src_size.x()), - src_size.x() * T_Bytes - ); - } - } - } + /** Align a point to a box grid of the given resolution and offset origin. Result may be negative if lower than current offset. */ + template + inline static Eigen::Vector2 gridAlign(FloatT x, FloatT y, const Eigen::Vector2& off, FloatT res) { + return Eigen::Vector2{ + static_cast( std::floor((x - off.x()) / res) ), // always floor since grid cells are indexed by their "bottom left" corner's raw position + static_cast( std::floor((y - off.y()) / res) ) + }; + } + template + inline static Eigen::Vector2 gridAlign(const Eigen::Vector4& pt, const Eigen::Vector2& off, FloatT res) { + return gridAlign(pt.x(), pt.y(), off, res); + } + + /** Get a raw buffer idx from a 2d index and buffer size (templated on major-order) */ + template + inline static int64_t gridIdx(const IntT x, const IntT y, const Eigen::Vector2& size) { + if constexpr(X_Major) { + return static_cast(x) * size.y() + y; // x-major = "contiguous blocks along [parallel to] y-axis" --> idx = (x * ymax) + y + } else { + return static_cast(y) * size.x() + x; // y-major = "contiguous blocks along [parallel to] x-axis" --> idx = (y * xmax) + x + } + } + template + inline static int64_t gridIdx(const Eigen::Vector2& loc, const Eigen::Vector2& size) { + return gridIdx(loc.x(), loc.y(), size); + } + + /** Get the 2d location corresponding to a raw buffer idx for the provded grid size (templated on major-order) */ + template + inline static Eigen::Vector2 gridLoc(const size_t idx, const Eigen::Vector2& size) { + if constexpr(X_Major) { + return Eigen::Vector2{ // x-major = "contiguous blocks along [parallel to] y-axis" --> x = idx / ymax, y = idx % ymax + static_cast(idx / size.y()), + static_cast(idx % size.y()) + }; + } else { + return Eigen::Vector2{ // y-major = "contiguous blocks along [parallel to] x-axis" --> x = idx % xmax, y = idx / xmax + static_cast(idx % size.x()), + static_cast(idx / size.x()) + }; + } + } + + /** Copy a 2d windows of elemens - expects element types to be POD (templated on major-order) */ + template + static void memcpyWindow( + T* dest, const T* src, + const Eigen::Vector2& dest_size, const Eigen::Vector2& src_size, + const Eigen::Vector2& diff + ) { + if constexpr(X_Major) { + for(int64_t _x = 0; _x < src_size.x(); _x++) { // iterate through source "rows" of contiguous memory (along y -- for each x) + memcpy( + dest + ((_x + diff.x()) * dest_size.y() + diff.y()), + src + (_x * src_size.y()), + src_size.y() * T_Bytes + ); + } + } else { + for(int64_t _y = 0; _y < src_size.y(); _y++) { // iterate through source "rows" of contiguous memory (along x -- for each y) + memcpy( + dest + ((_y + diff.y()) * dest_size.x() + diff.x()), + src + (_y * src_size.x()), + src_size.x() * T_Bytes + ); + } + } + } }; /** GridBase contains all generic functinality for a dynamic grid cells (template type) */ template< - typename Cell_t, - typename Int_t = int, - typename Float_t = float, - bool X_Major = false, - size_t Max_Alloc_Bytes = (1ULL << 30)> + typename Cell_t, + typename Int_t = int, + typename Float_t = float, + bool X_Major = false, + size_t Max_Alloc_Bytes = (1ULL << 30)> class GridBase { - static_assert(std::is_integral_v, ""); - static_assert(std::is_floating_point_v, ""); - static_assert(Max_Alloc_Bytes >= sizeof(Cell_t), ""); + static_assert(std::is_integral_v, ""); + static_assert(std::is_floating_point_v, ""); + static_assert(Max_Alloc_Bytes >= sizeof(Cell_t), ""); public: - using Cell_T = Cell_t; - using IntT = Int_t; - using FloatT = Float_t; - using This_T = GridBase< Cell_T, IntT, FloatT, X_Major, Max_Alloc_Bytes >; - - using Vec2i = Eigen::Vector2; - using Vec2f = Eigen::Vector2; - - static constexpr bool - X_Major_Order = X_Major; - static constexpr size_t - Cell_Size = sizeof(Cell_T), - Max_Alloc_NCells = Max_Alloc_Bytes / Cell_Size; - - template - constexpr inline static const IntT literalI(T v) { return static_cast(v); } - template - constexpr inline static const FloatT literalF(T v) { return static_cast(v); } - - template - inline static int64_t gridIdx(const IT x, const IT y, const Eigen::Vector2& size) { - return GridUtils::gridIdx(x, y, size); - } - template - inline static int64_t gridIdx(const Eigen::Vector2& loc, const Eigen::Vector2& size) { - return GridUtils::gridIdx(loc, size); - } - template - inline static Eigen::Vector2 gridLoc(const size_t idx, const Eigen::Vector2& size) { - return GridUtils::gridLoc(idx, size); - } - - template - inline static void memcpyWindow( - T* dest, const T* src, - const Eigen::Vector2& dest_size, const Eigen::Vector2& src_size, - const Eigen::Vector2& diff - ) { - return GridUtils::memcpyWindow(dest, src, dest_size, src_size, diff); - } + using Cell_T = Cell_t; + using IntT = Int_t; + using FloatT = Float_t; + using This_T = GridBase< Cell_T, IntT, FloatT, X_Major, Max_Alloc_Bytes >; + + using Vec2i = Eigen::Vector2; + using Vec2f = Eigen::Vector2; + + static constexpr bool + X_Major_Order = X_Major; + static constexpr size_t + Cell_Size = sizeof(Cell_T), + Max_Alloc_NCells = Max_Alloc_Bytes / Cell_Size; + + template + constexpr inline static const IntT literalI(T v) { return static_cast(v); } + template + constexpr inline static const FloatT literalF(T v) { return static_cast(v); } + + template + inline static int64_t gridIdx(const IT x, const IT y, const Eigen::Vector2& size) { + return GridUtils::gridIdx(x, y, size); + } + template + inline static int64_t gridIdx(const Eigen::Vector2& loc, const Eigen::Vector2& size) { + return GridUtils::gridIdx(loc, size); + } + template + inline static Eigen::Vector2 gridLoc(const size_t idx, const Eigen::Vector2& size) { + return GridUtils::gridLoc(idx, size); + } + + template + inline static void memcpyWindow( + T* dest, const T* src, + const Eigen::Vector2& dest_size, const Eigen::Vector2& src_size, + const Eigen::Vector2& diff + ) { + return GridUtils::memcpyWindow(dest, src, dest_size, src_size, diff); + } public: - GridBase() = default; - ~GridBase() { - if (this->grid) delete[] this->grid; - } - - - void reset(FloatT resolution = literalF(1), const Vec2f origin = Vec2f::Zero()) { - if (this->grid) { - delete[] this->grid; - this->grid = nullptr; - } - this->grid_size = Vec2i::Zero(); - this->cell_res = resolution <= literalF(0) ? literalF(0) : resolution; - this->grid_origin = origin; - } - - inline const Vec2i& size() const { - return this->grid_size; - } - inline const int64_t area() const { - return static_cast(this->grid_size.x()) * this->grid_size.y(); - } - inline const Vec2f& origin() const { - return this->grid_origin; - } - inline const FloatT cellRes() const { - return this->cell_res; - } - inline const Cell_T* gridData() const { - return this->grid; - } - - inline const Vec2i boundingLoc(const FloatT x, const FloatT y) const { - return GridUtils::gridAlign(x, y, this->grid_origin, this->cell_res); - } - inline const int64_t cellIdxOf(const FloatT x, const FloatT y) const { - return This_T::gridIdx(this->boundingLoc(x, y), this->grid_size); - } - - - /** Returns false if an invalid realloc was skipped */ - bool resizeToBounds(const Vec2f& min, const Vec2f& max) { - - static const Vec2i - _zero = Vec2i::Zero(), - _one = Vec2i::Ones(); - const Vec2i - _min = this->boundingLoc(min.x(), min.y()), // grid cell locations containing min and max, aligned with current offsets - _max = this->boundingLoc(max.x(), max.y()); - - // if (_min.cwiseLess(_zero).any() || _max.cwiseGreater(this->grid_size).any()) { - if( (_min.x() < _zero.x() || _min.y() < _zero.y()) || (_max.x() > this->grid_size.x() || _max.y() > this->grid_size.y()) ) { - const Vec2i - _low = _min.cwiseMin(_zero), // new high and low bounds for the map - _high = _max.cwiseMax(this->grid_size) + _one, // need to add an additional row + col since indexing happens in the corner, thus by default the difference between corners will not cover the entire size - _size = _high - _low; // new map size - - const int64_t _area = static_cast(_size.x()) * _size.y(); - if (_area > This_T::Max_Alloc_NCells || _area < 0LL) return false; // less than a gigabyte of allocated buffer is ideal - - Cell_T* _grid = new Cell_T[_area]; - memset(_grid, 0x00, _area * This_T::Cell_Size); // :O don't forget this otherwise the map will start with all garbage data - - const Vec2i _diff = _zero - _low; // by how many grid cells did the origin shift - if (this->grid) { - This_T::memcpyWindow(_grid, this->grid, _size, this->grid_size, _diff); - delete[] this->grid; - } - this->grid_origin -= (_diff.template cast() * this->cell_res); - this->grid_size = _size; - this->grid = _grid; - } - return true; - - } + GridBase() = default; + ~GridBase() { + if (this->grid) delete[] this->grid; + } + + + void reset(FloatT resolution = literalF(1), const Vec2f origin = Vec2f::Zero()) { + if (this->grid) { + delete[] this->grid; + this->grid = nullptr; + } + this->grid_size = Vec2i::Zero(); + this->cell_res = resolution <= literalF(0) ? literalF(0) : resolution; + this->grid_origin = origin; + } + + inline const Vec2i& size() const { + return this->grid_size; + } + inline const int64_t area() const { + return static_cast(this->grid_size.x()) * this->grid_size.y(); + } + inline const Vec2f& origin() const { + return this->grid_origin; + } + inline const FloatT cellRes() const { + return this->cell_res; + } + inline const Cell_T* gridData() const { + return this->grid; + } + + inline const Vec2i boundingLoc(const FloatT x, const FloatT y) const { + return GridUtils::gridAlign(x, y, this->grid_origin, this->cell_res); + } + inline const int64_t cellIdxOf(const FloatT x, const FloatT y) const { + return This_T::gridIdx(this->boundingLoc(x, y), this->grid_size); + } + + + /** Returns false if an invalid realloc was skipped */ + bool resizeToBounds(const Vec2f& min, const Vec2f& max) { + + static const Vec2i + _zero = Vec2i::Zero(), + _one = Vec2i::Ones(); + const Vec2i + _min = this->boundingLoc(min.x(), min.y()), // grid cell locations containing min and max, aligned with current offsets + _max = this->boundingLoc(max.x(), max.y()); + + // if (_min.cwiseLess(_zero).any() || _max.cwiseGreater(this->grid_size).any()) { + if( (_min.x() < _zero.x() || _min.y() < _zero.y()) || (_max.x() > this->grid_size.x() || _max.y() > this->grid_size.y()) ) { + const Vec2i + _low = _min.cwiseMin(_zero), // new high and low bounds for the map + _high = _max.cwiseMax(this->grid_size) + _one, // need to add an additional row + col since indexing happens in the corner, thus by default the difference between corners will not cover the entire size + _size = _high - _low; // new map size + + const int64_t _area = static_cast(_size.x()) * _size.y(); + if (_area > This_T::Max_Alloc_NCells || _area < 0LL) return false; // less than a gigabyte of allocated buffer is ideal + + Cell_T* _grid = new Cell_T[_area]; + memset(_grid, 0x00, _area * This_T::Cell_Size); // :O don't forget this otherwise the map will start with all garbage data + + const Vec2i _diff = _zero - _low; // by how many grid cells did the origin shift + if (this->grid) { + This_T::memcpyWindow(_grid, this->grid, _size, this->grid_size, _diff); + delete[] this->grid; + } + this->grid_origin -= (_diff.template cast() * this->cell_res); + this->grid_size = _size; + this->grid = _grid; + } + return true; + + } protected: - Vec2f grid_origin{}; - Vec2i grid_size{}; - FloatT cell_res{ static_cast(1) }; - Cell_T* grid{ nullptr }; + Vec2f grid_origin{}; + Vec2i grid_size{}; + FloatT cell_res{ static_cast(1) }; + Cell_T* grid{ nullptr }; }; @@ -234,184 +234,184 @@ class GridBase { template struct RatioGridCell { public: - using DataT = Data_t; - union { - struct { - DataT - accum, // accumulated [numerator] value - base; // base [denomenator] value - }; - DataT data[2]; - }; + using DataT = Data_t; + union { + struct { + DataT + accum, // accumulated [numerator] value + base; // base [denomenator] value + }; + DataT data[2]; + }; }; template< - typename Ratio_t = float, - typename Int_t = int, - typename Float_t = float, - bool X_Major = false, - size_t Max_Alloc_Bytes = (1ULL << 30)> + typename Ratio_t = float, + typename Int_t = int, + typename Float_t = float, + bool X_Major = false, + size_t Max_Alloc_Bytes = (1ULL << 30)> class RatioGrid : public GridBase< RatioGridCell, Int_t, Float_t, X_Major, Max_Alloc_Bytes > { public: - using This_T = RatioGrid< Ratio_t, Int_t, Float_t >; - using Base_T = GridBase< RatioGridCell, Int_t, Float_t, X_Major, Max_Alloc_Bytes >; - using RatioT = Ratio_t; - using typename Base_T::IntT; - using typename Base_T::FloatT; - using typename Base_T::Cell_T; - using typename Base_T::Vec2i; - using typename Base_T::Vec2f; - using PreciseFloatT = typename std::conditional< - std::is_floating_point::value, // check if the ratio internal type is fp - typename std::conditional< - (sizeof(RatioT) > sizeof(FloatT)), // get the fp type with highest precision - RatioT, - FloatT - >::type, - FloatT // use default fp type - >::type; - - template - constexpr inline static const RatioT literalR(T v) { return static_cast(v); } + using This_T = RatioGrid< Ratio_t, Int_t, Float_t >; + using Base_T = GridBase< RatioGridCell, Int_t, Float_t, X_Major, Max_Alloc_Bytes >; + using RatioT = Ratio_t; + using typename Base_T::IntT; + using typename Base_T::FloatT; + using typename Base_T::Cell_T; + using typename Base_T::Vec2i; + using typename Base_T::Vec2f; + using PreciseFloatT = typename std::conditional< + std::is_floating_point::value, // check if the ratio internal type is fp + typename std::conditional< + (sizeof(RatioT) > sizeof(FloatT)), // get the fp type with highest precision + RatioT, + FloatT + >::type, + FloatT // use default fp type + >::type; + + template + constexpr inline static const RatioT literalR(T v) { return static_cast(v); } public: - RatioGrid() = default; - ~RatioGrid() = default; + RatioGrid() = default; + ~RatioGrid() = default; - /** The accumulation selection must be a subset of the base selection! If not, simply call the individual methods. */ - // template - // void incrementRatio( - // const pcl::PointCloud& cloud, - // const pcl::Indices& accum_selection, - // const pcl::Indices& base_selection - // ) { + /** The accumulation selection must be a subset of the base selection! If not, simply call the individual methods. */ + // template + // void incrementRatio( + // const pcl::PointCloud& cloud, + // const pcl::Indices& accum_selection, + // const pcl::Indices& base_selection + // ) { - // if(accum_selection.size() > base_selection.size() && !base_selection.empty()) return; // definitely not a subset - don't add since we don't want to risk cells having a base of 0 (div by 0 on export) + // if(accum_selection.size() > base_selection.size() && !base_selection.empty()) return; // definitely not a subset - don't add since we don't want to risk cells having a base of 0 (div by 0 on export) - // Vec2f _min, _max; - // minMaxXY(cloud, base_selection, _min, _max); + // Vec2f _min, _max; + // minMaxXY(cloud, base_selection, _min, _max); - // if(this->resizeToBounds(_min, _max)) { - // if(accum_selection.size() == base_selection.size()) { - // this->incrementAllInternal(cloud, base_selection); - // } else { + // if(this->resizeToBounds(_min, _max)) { + // if(accum_selection.size() == base_selection.size()) { + // this->incrementAllInternal(cloud, base_selection); + // } else { - // } - // } + // } + // } - // // accum:all, base:all --> shortcut - // // accum:some, base:all --> iternative - // // accum:some, base:some --> check for shortcut : iterative - // // accum:all, base:some --> NOT VALID + // // accum:all, base:all --> shortcut + // // accum:some, base:all --> iternative + // // accum:some, base:some --> check for shortcut : iterative + // // accum:all, base:some --> NOT VALID - // } + // } public: - template - inline ExportT exportRatio(size_t idx, ExportT normalization = (ExportT)1) { - const Cell_T& _cell = this->grid[idx]; - const PreciseFloatT _ratio = static_cast(_cell.accum) / _cell.base; - return std::isnan(_ratio) ? - static_cast(0) : - static_cast(normalization * _ratio) - ; - } - - inline RatioT operator[](size_t idx) { - return this->exportRatio(idx); - } - - template - inline void incrementAccum( - const pcl::PointCloud& cloud, - const pcl::Indices& selection - ) { - this->increment<0>(cloud, selection); - } - template - inline void incrementBase( - const pcl::PointCloud& cloud, - const pcl::Indices& selection - ) { - this->increment<1>(cloud, selection); - } + template + inline ExportT exportRatio(size_t idx, ExportT normalization = (ExportT)1) { + const Cell_T& _cell = this->grid[idx]; + const PreciseFloatT _ratio = static_cast(_cell.accum) / _cell.base; + return std::isnan(_ratio) ? + static_cast(0) : + static_cast(normalization * _ratio) + ; + } + + inline RatioT operator[](size_t idx) { + return this->exportRatio(idx); + } + + template + inline void incrementAccum( + const pcl::PointCloud& cloud, + const pcl::Indices& selection + ) { + this->increment<0>(cloud, selection); + } + template + inline void incrementBase( + const pcl::PointCloud& cloud, + const pcl::Indices& selection + ) { + this->increment<1>(cloud, selection); + } protected: - /** Does not implement any safety checks! */ - template - inline Cell_T* boundingCell(const PointT& pt) { - const int64_t i = this->cellIdxOf(static_cast(pt.x), static_cast(pt.y)); + /** Does not implement any safety checks! */ + template + inline Cell_T* boundingCell(const PointT& pt) { + const int64_t i = this->cellIdxOf(static_cast(pt.x), static_cast(pt.y)); #ifndef GRID_IMPL_SKIP_BOUND_CHECKING - if (i >= 0 && i < this->area()) + if (i >= 0 && i < this->area()) #endif - { - return this->grid + i; - } - return nullptr; - } - - template - void increment( - const pcl::PointCloud& cloud, - const pcl::Indices& selection - ) { - static_assert(data_off < (sizeof(Cell_T) / sizeof(RatioT)), ""); - - Vec2f _min, _max; - minMaxXY(cloud, selection, _min, _max); - - if(this->resizeToBounds(_min, _max)) { - this->incrementInternal(cloud, selection); - } - - } - - /** Do not use before checking bounds and resizing the grid!!! */ - template - void incrementInternal( - const pcl::PointCloud& cloud, - const pcl::Indices& selection - ) { - Cell_T* _cell; - if(selection.empty()) { - for(const PointT& pt : cloud.points) { - if(_cell = this->boundingCell(pt)) { - _cell->data[data_off] += literalR(1); - } - } - } else { - for(const pcl::index_t idx : selection) { - if(_cell = this->boundingCell(cloud.points[idx])) { - _cell->data[data_off] += literalR(1); - } - } - } - } - - template - void incrementAllInternal( - const pcl::PointCloud& cloud, - const pcl::Indices& selection - ) { - Cell_T* _cell; - if(selection.empty()) { - for(const PointT& pt : cloud.points) { - if((_cell = this->boundingCell(pt))) { - _cell->accum += literalR(1); - _cell->base += literalR(1); - } - } - } else { - for(const pcl::index_t idx : selection) { - if((_cell = this->boundingCell(cloud.points[idx]))) { - _cell->accum += literalR(1); - _cell->base += literalR(1); - } - } - } - } + { + return this->grid + i; + } + return nullptr; + } + + template + void increment( + const pcl::PointCloud& cloud, + const pcl::Indices& selection + ) { + static_assert(data_off < (sizeof(Cell_T) / sizeof(RatioT)), ""); + + Vec2f _min, _max; + minMaxXY(cloud, selection, _min, _max); + + if(this->resizeToBounds(_min, _max)) { + this->incrementInternal(cloud, selection); + } + + } + + /** Do not use before checking bounds and resizing the grid!!! */ + template + void incrementInternal( + const pcl::PointCloud& cloud, + const pcl::Indices& selection + ) { + Cell_T* _cell; + if(selection.empty()) { + for(const PointT& pt : cloud.points) { + if(_cell = this->boundingCell(pt)) { + _cell->data[data_off] += literalR(1); + } + } + } else { + for(const pcl::index_t idx : selection) { + if(_cell = this->boundingCell(cloud.points[idx])) { + _cell->data[data_off] += literalR(1); + } + } + } + } + + template + void incrementAllInternal( + const pcl::PointCloud& cloud, + const pcl::Indices& selection + ) { + Cell_T* _cell; + if(selection.empty()) { + for(const PointT& pt : cloud.points) { + if((_cell = this->boundingCell(pt))) { + _cell->accum += literalR(1); + _cell->base += literalR(1); + } + } + } else { + for(const pcl::index_t idx : selection) { + if((_cell = this->boundingCell(cloud.points[idx]))) { + _cell->accum += literalR(1); + _cell->base += literalR(1); + } + } + } + } }; @@ -423,11 +423,11 @@ class RatioGrid : public GridBase< RatioGridCell, Int_t, Float_t, X_Maj /** constexpr conditional value (v1 = true val, v2 = false val) */ template struct conditional_literal { - static constexpr T value = tV; + static constexpr T value = tV; }; template struct conditional_literal { - static constexpr T value = fV; + static constexpr T value = fV; }; template @@ -438,253 +438,253 @@ struct is_ratio> : std::true_type{}; /** RatioGrid that also stores the result buffer, and updates it inline with ratio updates */ template< - typename Buff_t = uint8_t, - typename Ratio_t = float, - typename Int_t = int, - typename Float_t = float, - bool X_Major = false, - size_t Max_Alloc_Bytes = (1ULL << 30)> + typename Buff_t = uint8_t, + typename Ratio_t = float, + typename Int_t = int, + typename Float_t = float, + bool X_Major = false, + size_t Max_Alloc_Bytes = (1ULL << 30)> class QuantizedRatioGrid : public RatioGrid< Ratio_t, Int_t, Float_t, X_Major, Max_Alloc_Bytes > { public: - using This_T = QuantizedRatioGrid< Buff_t, Ratio_t, Int_t, Float_t, X_Major, Max_Alloc_Bytes >; - using Super_T = RatioGrid< Ratio_t, Int_t, Float_t, X_Major, Max_Alloc_Bytes >; - using Buff_T = Buff_t; - using typename Super_T::Base_T; - using typename Super_T::RatioT; - using typename Super_T::IntT; - using typename Super_T::FloatT; - using typename Super_T::Cell_T; - using typename Super_T::Vec2i; - using typename Super_T::Vec2f; - using typename Super_T::PreciseFloatT; - - static constexpr size_t - Buff_Cell_Size = sizeof(Buff_T), - Max_Alloc_NBlocks = Max_Alloc_Bytes / Buff_Cell_Size; - static constexpr Buff_T - Buff_Norm_Val = conditional_literal< - std::is_integral::value, Buff_T, - std::numeric_limits::max(), // Use the full range for integral types - static_cast(1) // Buff_T is floating point or a special type. Normalize to 1 (or equivalent) - >::value; + using This_T = QuantizedRatioGrid< Buff_t, Ratio_t, Int_t, Float_t, X_Major, Max_Alloc_Bytes >; + using Super_T = RatioGrid< Ratio_t, Int_t, Float_t, X_Major, Max_Alloc_Bytes >; + using Buff_T = Buff_t; + using typename Super_T::Base_T; + using typename Super_T::RatioT; + using typename Super_T::IntT; + using typename Super_T::FloatT; + using typename Super_T::Cell_T; + using typename Super_T::Vec2i; + using typename Super_T::Vec2f; + using typename Super_T::PreciseFloatT; + + static constexpr size_t + Buff_Cell_Size = sizeof(Buff_T), + Max_Alloc_NBlocks = Max_Alloc_Bytes / Buff_Cell_Size; + static constexpr Buff_T + Buff_Norm_Val = conditional_literal< + std::is_integral::value, Buff_T, + std::numeric_limits::max(), // Use the full range for integral types + static_cast(1) // Buff_T is floating point or a special type. Normalize to 1 (or equivalent) + >::value; public: - QuantizedRatioGrid() = default; - ~QuantizedRatioGrid() { - if(this->buffer) delete[] this->buffer; - } - - - /** Calls super reset() and resets buffer */ - void reset(FloatT resolution = Base_T::template literalF<1>(), const Vec2f origin = Vec2f::Zero()) { - if(this->buffer) { - delete[] this->buffer; - this->buffer = nullptr; - } - this->Base_T::reset(resolution, origin); - } - - /** The quantized ratio buffer */ - inline const Buff_T* buffData() { - return this->buffer; - } - - - /** Resize all buffers and copy old data into new viewport */ - bool resizeToBounds(const Vec2f& min, const Vec2f& max) { - - static const Vec2i - _zero = Vec2i::Zero(), - _one = Vec2i::Ones(); - const Vec2i - _min = this->boundingLoc(min.x(), min.y()), // grid cell locations containing min and max, aligned with current offsets - _max = this->boundingLoc(max.x(), max.y()); - - // if (_min.cwiseLess(_zero).any() || _max.cwiseGreater(this->grid_size).any()) { - if( (_min.x() < _zero.x() || _min.y() < _zero.y()) || (_max.x() > this->grid_size.x() || _max.y() > this->grid_size.y()) ) { - const Vec2i - _low = _min.cwiseMin(_zero), // new high and low bounds for the map - _high = _max.cwiseMax(this->grid_size) + _one, // need to add an additional row + col since indexing happens in the corner, thus by default the difference between corners will not cover the entire size - _size = _high - _low; // new map size - - const int64_t _area = static_cast(_size.x()) * _size.y(); - if (_area < 0LL || _area > Base_T::Max_Alloc_NCells || _area > This_T::Max_Alloc_NBlocks) return false; // less than a gigabyte of allocated buffer is ideal - - Cell_T* _grid = new Cell_T[_area]; - Buff_T* _buffer = new Buff_T[_area]; - // std::cout << "[QRG]: Allocated new buffers of area " << _area << " -- diff: " << (_area - this->area()) << std::endl; - memset(_grid, 0x00, _area * Base_T::Cell_Size); // :O don't forget this otherwise the map will start with all garbage data - memset(_buffer, 0x00, _area * This_T::Buff_Cell_Size); - - const Vec2i _diff = _zero - _low; // by how many grid cells did the origin shift - if(this->grid) { - Base_T::memcpyWindow(_grid, this->grid, _size, this->grid_size, _diff); - delete[] this->grid; - // std::cout << "[QRG]: Deleted old grid data with area " << this->area() << std::endl; - } - if(this->buffer) { - Base_T::memcpyWindow(_buffer, this->buffer, _size, this->grid_size, _diff); - delete[] this->buffer; - // std::cout << "[QRG]: Deleted old buffer data with area " << this->area() << std::endl; - } - this->grid_origin -= (_diff.template cast() * this->cell_res); - this->grid_size = _size; - this->grid = _grid; - this->buffer = _buffer; - // std::cout << "[QRG]: Assigned new buffers with area " << this->area() << std::endl; - } - return true; - - } - - - /** Increments the base and accumulator counters for each point in the provided selections, which are determined by: - * 1. if the BASE and ACCUM selections are both null (empty), all points are used to increment both counters - * 2. if the BASE is null (empty) and the ACCUM selection is not, increment BOTH for accum selection - * 3. if the BASE is non-null and the ACCUM is empty, ONLY increment the base selection (no accum increments -- EMPTY TREATED NORMALLY) - * 4. if the BASE and ACCUM selections are both non-null, increment the base selection and the valid accum selection which represents a subset of the base selection (incrementing an ACCUM w/o a BASE is invalid since it leads to a div by 0) - * >> TEMPLATE PARAMS >> - * @param PointT - the point type for the input pointcloud - * @param Quantization_Exponent - the power to which the each ratio is taken to before being quantized -- default 1 (no effect) - * @param Ratio_Normalize_Rebase - the base value to which each ratio will be normalized -- a value of 0 disables this feature - * @param Ratio_Normalize_Thresh - the threshold value (denomentor) for which normalization will occur -- a value of 0 disables this feature - * >> NORMAL PARAMS >> - * @param cloud - the base set of points which will be used to index into the grid - * @param base_selection - the set of indices for which points should be used to increment the DENOMENATOR of the ratio at each corresponding cell - * @param accum_selection - the set of indices for which points should be used to increment the NUMERATOR of the ratio at each corresponding cell (MUST RESOLVE TO BE A SUBSET OF THE BASE SELECTION - although this is internally enforced so deviations will be handled gracefully) - */ - template< - typename PointT, - typename Quantization_Exponent = std::ratio<1, 1>::type, // can't use float until C++20 so use std::ratio, - This_T::IntT Ratio_Normalize_Rebase = static_cast(0), // need to use integers since float template params (if used for ratio type) are invalid below C++20 - This_T::IntT Ratio_Normalize_Thresh = static_cast(0)> // this is find tho since the counters are always integral - void incrementRatio( - const pcl::PointCloud& cloud, - const pcl::Indices& base_selection, - const pcl::Indices& accum_selection - ) { - - Vec2f min, max; - minMaxXY(cloud, base_selection, min, max); - - if(!this->This_T::resizeToBounds(min, max)) return; - - if(base_selection.empty()) { - - if(accum_selection.empty()) { // CASE 1: increment both for all points - for(const PointT& pt : cloud) { - const int64_t i = this->cellIdxOf(static_cast(pt.x), static_cast(pt.y)); + QuantizedRatioGrid() = default; + ~QuantizedRatioGrid() { + if(this->buffer) delete[] this->buffer; + } + + + /** Calls super reset() and resets buffer */ + void reset(FloatT resolution = Base_T::template literalF<1>(), const Vec2f origin = Vec2f::Zero()) { + if(this->buffer) { + delete[] this->buffer; + this->buffer = nullptr; + } + this->Base_T::reset(resolution, origin); + } + + /** The quantized ratio buffer */ + inline const Buff_T* buffData() { + return this->buffer; + } + + + /** Resize all buffers and copy old data into new viewport */ + bool resizeToBounds(const Vec2f& min, const Vec2f& max) { + + static const Vec2i + _zero = Vec2i::Zero(), + _one = Vec2i::Ones(); + const Vec2i + _min = this->boundingLoc(min.x(), min.y()), // grid cell locations containing min and max, aligned with current offsets + _max = this->boundingLoc(max.x(), max.y()); + + // if (_min.cwiseLess(_zero).any() || _max.cwiseGreater(this->grid_size).any()) { + if( (_min.x() < _zero.x() || _min.y() < _zero.y()) || (_max.x() > this->grid_size.x() || _max.y() > this->grid_size.y()) ) { + const Vec2i + _low = _min.cwiseMin(_zero), // new high and low bounds for the map + _high = _max.cwiseMax(this->grid_size) + _one, // need to add an additional row + col since indexing happens in the corner, thus by default the difference between corners will not cover the entire size + _size = _high - _low; // new map size + + const int64_t _area = static_cast(_size.x()) * _size.y(); + if (_area < 0LL || _area > Base_T::Max_Alloc_NCells || _area > This_T::Max_Alloc_NBlocks) return false; // less than a gigabyte of allocated buffer is ideal + + Cell_T* _grid = new Cell_T[_area]; + Buff_T* _buffer = new Buff_T[_area]; + // std::cout << "[QRG]: Allocated new buffers of area " << _area << " -- diff: " << (_area - this->area()) << std::endl; + memset(_grid, 0x00, _area * Base_T::Cell_Size); // :O don't forget this otherwise the map will start with all garbage data + memset(_buffer, 0x00, _area * This_T::Buff_Cell_Size); + + const Vec2i _diff = _zero - _low; // by how many grid cells did the origin shift + if(this->grid) { + Base_T::memcpyWindow(_grid, this->grid, _size, this->grid_size, _diff); + delete[] this->grid; + // std::cout << "[QRG]: Deleted old grid data with area " << this->area() << std::endl; + } + if(this->buffer) { + Base_T::memcpyWindow(_buffer, this->buffer, _size, this->grid_size, _diff); + delete[] this->buffer; + // std::cout << "[QRG]: Deleted old buffer data with area " << this->area() << std::endl; + } + this->grid_origin -= (_diff.template cast() * this->cell_res); + this->grid_size = _size; + this->grid = _grid; + this->buffer = _buffer; + // std::cout << "[QRG]: Assigned new buffers with area " << this->area() << std::endl; + } + return true; + + } + + + /** Increments the base and accumulator counters for each point in the provided selections, which are determined by: + * 1. if the BASE and ACCUM selections are both null (empty), all points are used to increment both counters + * 2. if the BASE is null (empty) and the ACCUM selection is not, increment BOTH for accum selection + * 3. if the BASE is non-null and the ACCUM is empty, ONLY increment the base selection (no accum increments -- EMPTY TREATED NORMALLY) + * 4. if the BASE and ACCUM selections are both non-null, increment the base selection and the valid accum selection which represents a subset of the base selection (incrementing an ACCUM w/o a BASE is invalid since it leads to a div by 0) + * >> TEMPLATE PARAMS >> + * @param PointT - the point type for the input pointcloud + * @param Quantization_Exponent - the power to which the each ratio is taken to before being quantized -- default 1 (no effect) + * @param Ratio_Normalize_Rebase - the base value to which each ratio will be normalized -- a value of 0 disables this feature + * @param Ratio_Normalize_Thresh - the threshold value (denomentor) for which normalization will occur -- a value of 0 disables this feature + * >> NORMAL PARAMS >> + * @param cloud - the base set of points which will be used to index into the grid + * @param base_selection - the set of indices for which points should be used to increment the DENOMENATOR of the ratio at each corresponding cell + * @param accum_selection - the set of indices for which points should be used to increment the NUMERATOR of the ratio at each corresponding cell (MUST RESOLVE TO BE A SUBSET OF THE BASE SELECTION - although this is internally enforced so deviations will be handled gracefully) + */ + template< + typename PointT, + typename Quantization_Exponent = std::ratio<1, 1>::type, // can't use float until C++20 so use std::ratio, + This_T::IntT Ratio_Normalize_Rebase = static_cast(0), // need to use integers since float template params (if used for ratio type) are invalid below C++20 + This_T::IntT Ratio_Normalize_Thresh = static_cast(0)> // this is find tho since the counters are always integral + void incrementRatio( + const pcl::PointCloud& cloud, + const pcl::Indices& base_selection, + const pcl::Indices& accum_selection + ) { + + Vec2f min, max; + minMaxXY(cloud, base_selection, min, max); + + if(!this->This_T::resizeToBounds(min, max)) return; + + if(base_selection.empty()) { + + if(accum_selection.empty()) { // CASE 1: increment both for all points + for(const PointT& pt : cloud) { + const int64_t i = this->cellIdxOf(static_cast(pt.x), static_cast(pt.y)); #ifndef GRID_IMPL_SKIP_BOUND_CHECKING - if (i >= 0 && i < this->area()) + if (i >= 0 && i < this->area()) #endif - { - Cell_T* _cell = this->grid + i; - _cell->accum += Super_T::template literalR(1); - _cell->base += Super_T::template literalR(1); - - this->buffer[i] = this->normalizeAndQuantize(*_cell); - } - } - } else { // CASE 2: increment both for accumulator selection - for(const pcl::index_t _pt : accum_selection) { - const PointT& pt = cloud.points[_pt]; - const int64_t i = this->cellIdxOf(static_cast(pt.x), static_cast(pt.y)); + { + Cell_T* _cell = this->grid + i; + _cell->accum += Super_T::template literalR(1); + _cell->base += Super_T::template literalR(1); + + this->buffer[i] = this->normalizeAndQuantize(*_cell); + } + } + } else { // CASE 2: increment both for accumulator selection + for(const pcl::index_t _pt : accum_selection) { + const PointT& pt = cloud.points[_pt]; + const int64_t i = this->cellIdxOf(static_cast(pt.x), static_cast(pt.y)); #ifndef GRID_IMPL_SKIP_BOUND_CHECKING - if (i >= 0 && i < this->area()) + if (i >= 0 && i < this->area()) #endif - { - Cell_T* _cell = this->grid + i; - _cell->accum += Super_T::template literalR(1); - _cell->base += Super_T::template literalR(1); - - this->buffer[i] = this->normalizeAndQuantize(*_cell); - } - } - } - - } else { - - if(accum_selection.empty()) { // CASE 3: increment only base selection - for(const pcl::index_t _pt : base_selection) { - const PointT& pt = cloud.points[_pt]; - const int64_t i = this->cellIdxOf(static_cast(pt.x), static_cast(pt.y)); + { + Cell_T* _cell = this->grid + i; + _cell->accum += Super_T::template literalR(1); + _cell->base += Super_T::template literalR(1); + + this->buffer[i] = this->normalizeAndQuantize(*_cell); + } + } + } + + } else { + + if(accum_selection.empty()) { // CASE 3: increment only base selection + for(const pcl::index_t _pt : base_selection) { + const PointT& pt = cloud.points[_pt]; + const int64_t i = this->cellIdxOf(static_cast(pt.x), static_cast(pt.y)); #ifndef GRID_IMPL_SKIP_BOUND_CHECKING - if (i >= 0 && i < this->area()) + if (i >= 0 && i < this->area()) #endif - { - Cell_T* _cell = this->grid + i; - _cell->base += Super_T::template literalR(1); - - this->buffer[i] = this->normalizeAndQuantize(*_cell); - } - } - } else { // CASE 4: increment through base and accum selections (in case subset is invalid) - size_t _accum = 0; - for(const pcl::index_t _pt : base_selection) { - const PointT& pt = cloud.points[_pt]; - const int64_t i = this->cellIdxOf(static_cast(pt.x), static_cast(pt.y)); + { + Cell_T* _cell = this->grid + i; + _cell->base += Super_T::template literalR(1); + + this->buffer[i] = this->normalizeAndQuantize(*_cell); + } + } + } else { // CASE 4: increment through base and accum selections (in case subset is invalid) + size_t _accum = 0; + for(const pcl::index_t _pt : base_selection) { + const PointT& pt = cloud.points[_pt]; + const int64_t i = this->cellIdxOf(static_cast(pt.x), static_cast(pt.y)); #ifndef GRID_IMPL_SKIP_BOUND_CHECKING - if (i < 0 || i >= this->area()) { // opposite the usual so that we only need one macro test! - if(_accum < accum_selection.size() && accum_selection[_accum] == _pt) _accum++; // need to test for increment even if point is invalid - } else // else since we tested the opposite! + if (i < 0 || i >= this->area()) { // opposite the usual so that we only need one macro test! + if(_accum < accum_selection.size() && accum_selection[_accum] == _pt) _accum++; // need to test for increment even if point is invalid + } else // else since we tested the opposite! #endif - { - Cell_T* _cell = this->grid + i; - if(_accum < accum_selection.size() && accum_selection[_accum] == _pt) { - _cell->accum += Super_T::template literalR(1); - _accum++; - } - _cell->base += Super_T::template literalR(1); + { + Cell_T* _cell = this->grid + i; + if(_accum < accum_selection.size() && accum_selection[_accum] == _pt) { + _cell->accum += Super_T::template literalR(1); + _accum++; + } + _cell->base += Super_T::template literalR(1); - this->buffer[i] = this->normalizeAndQuantize(*_cell); - } - } - } + this->buffer[i] = this->normalizeAndQuantize(*_cell); + } + } + } - } + } - } + } protected: - template< - typename Quantization_Exponent = std::ratio<1, 1>::type, - This_T::IntT Ratio_Normalize_Rebase = static_cast(0), - This_T::IntT Ratio_Normalize_Thresh = static_cast(0)> - static Buff_T normalizeAndQuantize(Cell_T& cell) { - static_assert(is_ratio::value, "Exponent must be std::ratio<>!"); - static constexpr double - _Exp = static_cast(Quantization_Exponent::num) / Quantization_Exponent::den; - static_assert(_Exp > 0.0, "Only positive, non-zero exponents are allowed!"); // otherwise this breaks the implementation - static_assert( - Ratio_Normalize_Thresh == static_cast(0) && - Ratio_Normalize_Rebase == static_cast(0) || - Ratio_Normalize_Thresh > Ratio_Normalize_Rebase, - "Normalization threshold must be greater than the rebase value!"); // ratio normalization constants must induce a negative feedback loop - - if constexpr(Ratio_Normalize_Rebase > static_cast(0)) { // thresh must also be greater than 0 due to static_assert ^ - static constexpr This_T::PreciseFloatT - Normalization_Factor = (static_cast(Ratio_Normalize_Rebase) / Ratio_Normalize_Thresh); - - if(cell.base == Ratio_Normalize_Thresh) { - cell.accum = static_cast( cell.accum * Normalization_Factor ); - cell.base = static_cast( Ratio_Normalize_Rebase ); - } else if(cell.base > Ratio_Normalize_Thresh) { // catch in case we miss the point where base is exactly at thresh -- need to scale by exact proportion otherwise the ratio could become >1 and overflow the integral type when quantizing - cell.accum = static_cast( cell.accum * (static_cast(Ratio_Normalize_Rebase) / cell.base) ); - cell.base = static_cast( Ratio_Normalize_Rebase ); - } - } - - if constexpr(Quantization_Exponent::num == Quantization_Exponent::den) { - return static_cast( This_T::Buff_Norm_Val * (static_cast(cell.accum) / cell.base) ); - } else { - return static_cast( This_T::Buff_Norm_Val * pow((static_cast(cell.accum) / cell.base), _Exp) ); - } - } + template< + typename Quantization_Exponent = std::ratio<1, 1>::type, + This_T::IntT Ratio_Normalize_Rebase = static_cast(0), + This_T::IntT Ratio_Normalize_Thresh = static_cast(0)> + static Buff_T normalizeAndQuantize(Cell_T& cell) { + static_assert(is_ratio::value, "Exponent must be std::ratio<>!"); + static constexpr double + _Exp = static_cast(Quantization_Exponent::num) / Quantization_Exponent::den; + static_assert(_Exp > 0.0, "Only positive, non-zero exponents are allowed!"); // otherwise this breaks the implementation + static_assert( + Ratio_Normalize_Thresh == static_cast(0) && + Ratio_Normalize_Rebase == static_cast(0) || + Ratio_Normalize_Thresh > Ratio_Normalize_Rebase, + "Normalization threshold must be greater than the rebase value!"); // ratio normalization constants must induce a negative feedback loop + + if constexpr(Ratio_Normalize_Rebase > static_cast(0)) { // thresh must also be greater than 0 due to static_assert ^ + static constexpr This_T::PreciseFloatT + Normalization_Factor = (static_cast(Ratio_Normalize_Rebase) / Ratio_Normalize_Thresh); + + if(cell.base == Ratio_Normalize_Thresh) { + cell.accum = static_cast( cell.accum * Normalization_Factor ); + cell.base = static_cast( Ratio_Normalize_Rebase ); + } else if(cell.base > Ratio_Normalize_Thresh) { // catch in case we miss the point where base is exactly at thresh -- need to scale by exact proportion otherwise the ratio could become >1 and overflow the integral type when quantizing + cell.accum = static_cast( cell.accum * (static_cast(Ratio_Normalize_Rebase) / cell.base) ); + cell.base = static_cast( Ratio_Normalize_Rebase ); + } + } + + if constexpr(Quantization_Exponent::num == Quantization_Exponent::den) { + return static_cast( This_T::Buff_Norm_Val * (static_cast(cell.accum) / cell.base) ); + } else { + return static_cast( This_T::Buff_Norm_Val * pow((static_cast(cell.accum) / cell.base), _Exp) ); + } + } protected: - Buff_T* buffer{ nullptr }; + Buff_T* buffer{ nullptr }; }; @@ -702,125 +702,125 @@ class QuantizedRatioGrid : public RatioGrid< Ratio_t, Int_t, Float_t, X_Major, M /** Statistical grid cell types */ struct StatCellTypes { protected: - template - struct CellBase_ { - using Acc_T = Acc_t; - using Stat_T = Stat_t; + template + struct CellBase_ { + using Acc_T = Acc_t; + using Stat_T = Stat_t; - inline static constexpr bool - IsDense = std::is_same::value; - }; + inline static constexpr bool + IsDense = std::is_same::value; + }; public: - template - struct GridCell_ : CellBase_ { - using typename CellBase_::Acc_T; - using typename CellBase_::Stat_T; - Acc_T val; - union { - struct { Stat_T min_z, max_z, avg_z; }; - Stat_T stat[3]; - }; - }; - template - struct GridCell_Aligned_ : CellBase_ { - using Data_T = Data_t; - union { - struct { - Data_T val; - union { - struct { Data_T min_z, max_z, avg_z; }; - Data_T stat[3]; - }; - }; - Data_T data[4]; - }; - }; - - /** If datatypes are the same, use a "dense" allocation, otherwise use standard layout */ - template - using GridCell = typename std::conditional< - CellBase_::IsDense, - GridCell_Aligned_, - GridCell_ - >::type; + template + struct GridCell_ : CellBase_ { + using typename CellBase_::Acc_T; + using typename CellBase_::Stat_T; + Acc_T val; + union { + struct { Stat_T min_z, max_z, avg_z; }; + Stat_T stat[3]; + }; + }; + template + struct GridCell_Aligned_ : CellBase_ { + using Data_T = Data_t; + union { + struct { + Data_T val; + union { + struct { Data_T min_z, max_z, avg_z; }; + Data_T stat[3]; + }; + }; + Data_T data[4]; + }; + }; + + /** If datatypes are the same, use a "dense" allocation, otherwise use standard layout */ + template + using GridCell = typename std::conditional< + CellBase_::IsDense, + GridCell_Aligned_, + GridCell_ + >::type; }; /** Renamed version of the old grid strategy */ template< - typename Acc_t = float, - typename Int_t = int, - typename Float_t = float, - size_t Max_Alloc_Bytes = (1ULL << 30)> + typename Acc_t = float, + typename Int_t = int, + typename Float_t = float, + size_t Max_Alloc_Bytes = (1ULL << 30)> class StatisticGrid : public GridBase< StatCellTypes::GridCell, Int_t, Float_t, Max_Alloc_Bytes > { public: - using This_T = StatisticGrid< Acc_t, Int_t, Float_t >; - using Base_T = GridBase< StatCellTypes::GridCell, Int_t, Float_t, Max_Alloc_Bytes >; - using Accumulation_T = Acc_t; - using typename Base_T::IntT; - using typename Base_T::FloatT; - using typename Base_T::Cell_T; - using typename Base_T::Vec2i; - using typename Base_T::Vec2f; + using This_T = StatisticGrid< Acc_t, Int_t, Float_t >; + using Base_T = GridBase< StatCellTypes::GridCell, Int_t, Float_t, Max_Alloc_Bytes >; + using Accumulation_T = Acc_t; + using typename Base_T::IntT; + using typename Base_T::FloatT; + using typename Base_T::Cell_T; + using typename Base_T::Vec2i; + using typename Base_T::Vec2f; - inline static constexpr bool - Cell_IsDense = Cell_T::IsDense; + inline static constexpr bool + Cell_IsDense = Cell_T::IsDense; - template - inline static constexpr Accumulation_T AccVal() { return static_cast(val); } + template + inline static constexpr Accumulation_T AccVal() { return static_cast(val); } public: - StatisticGrid() = default; - ~StatisticGrid() = default; + StatisticGrid() = default; + ~StatisticGrid() = default; - template - void insertPoints(const pcl::PointCloud& cloud, const pcl::Indices& selection) { + template + void insertPoints(const pcl::PointCloud& cloud, const pcl::Indices& selection) { - Vec2f _min, _max; - minMaxXY(cloud, selection, _min, _max); + Vec2f _min, _max; + minMaxXY(cloud, selection, _min, _max); - if (this->resizeToBounds(_min, _max)) { - if (!selection.empty()) { - for (const pcl::index_t idx : selection) { - this->insert(cloud.points[idx]); - } - } else { - for (const PointT& pt : cloud.points) { - this->insert(pt); - } - } - } + if (this->resizeToBounds(_min, _max)) { + if (!selection.empty()) { + for (const pcl::index_t idx : selection) { + this->insert(cloud.points[idx]); + } + } else { + for (const PointT& pt : cloud.points) { + this->insert(pt); + } + } + } - } + } protected: - /** returns false if accumulation failed (invalid index) */ - template - bool insert(const PointT& pt) { - const int64_t i = this->cellIdxOf(static_cast(pt.x), static_cast(pt.y)); + /** returns false if accumulation failed (invalid index) */ + template + bool insert(const PointT& pt) { + const int64_t i = this->cellIdxOf(static_cast(pt.x), static_cast(pt.y)); #ifndef GRID_IMPL_SKIP_BOUND_CHECKING - if (i >= 0 && i < this->area()) + if (i >= 0 && i < this->area()) #endif - { - Cell_T& _cell = this->grid[i]; - _cell.val += AccVal<1>(); - if (_cell.val > this->max_weight) this->max_weight = _cell.val; + { + Cell_T& _cell = this->grid[i]; + _cell.val += AccVal<1>(); + if (_cell.val > this->max_weight) this->max_weight = _cell.val; - const FloatT _z = static_cast(pt.z); - if (_z < _cell.min_z) _cell.min_z = _z; - else if (_z > _cell.max_z) _cell.max_z = _z; - _cell.avg_z = (static_cast(_cell.val - AccVal<1>()) * _cell.avg_z + _z) / static_cast(_cell.val); + const FloatT _z = static_cast(pt.z); + if (_z < _cell.min_z) _cell.min_z = _z; + else if (_z > _cell.max_z) _cell.max_z = _z; + _cell.avg_z = (static_cast(_cell.val - AccVal<1>()) * _cell.avg_z + _z) / static_cast(_cell.val); - return true; - } - return false; - } + return true; + } + return false; + } protected: - Accumulation_T max_weight{ AccVal<0>() }; + Accumulation_T max_weight{ AccVal<0>() }; }; diff --git a/src/ldrp/perception.cpp b/src/ldrp/perception.cpp index a4eab91..46869e2 100644 --- a/src/ldrp/perception.cpp +++ b/src/ldrp/perception.cpp @@ -48,125 +48,143 @@ #include #include #include +#include +#include #include #include namespace util { - template - struct identity { typedef T type; }; - - template - void declare_param(rclcpp::Node* node, const std::string param_name, T& param, const typename identity::type& default_value) { - node->declare_parameter(param_name, default_value); - node->get_parameter(param_name, param); - } - - - template - static inline int64_t floatSecondsToIntMicros(const FT v) { - static_assert(std::is_floating_point_v, ""); - return static_cast(v * static_cast(1e6)); - } - - template - static inline int64_t constructTimestampMicros(const T seconds, const T nanoseconds) { - return (static_cast(seconds) * 1000000L) + (static_cast(nanoseconds) / 1000L); - } - - - void _test(rclcpp::Node* node) { - pcl::PointCloud _c; - sensor_msgs::msg::PointCloud2 _c2; - pcl::toROSMsg(_c, _c2); - RCLCPP_INFO(node->get_logger(), - "\n-------------------------------- TESTING --------------------------------" - "\nPointXYZ \"frame_id\": %s" - "\nPointCloud2 total fields: %ld" - "\nPointCloud2 field #1: { name: %s, offset: %d, datatype: %d, count: %d }" - "\nPointCloud2 field #2: { name: %s, offset: %d, datatype: %d, count: %d }" - "\nPointCloud2 field #3: { name: %s, offset: %d, datatype: %d, count: %d }" - "\n-------------------------------------------------------------------------", - _c.header.frame_id.c_str(), - _c2.fields.size(), - _c2.fields[0].name.c_str(), - _c2.fields[0].offset, - _c2.fields[0].datatype, - _c2.fields[0].count, - _c2.fields[1].name.c_str(), - _c2.fields[1].offset, - _c2.fields[1].datatype, - _c2.fields[1].count, - _c2.fields[2].name.c_str(), - _c2.fields[2].offset, - _c2.fields[2].datatype, - _c2.fields[2].count - ); - } + template + struct identity { typedef T type; }; + + template + void declare_param(rclcpp::Node* node, const std::string param_name, T& param, const typename identity::type& default_value) + { + node->declare_parameter(param_name, default_value); + node->get_parameter(param_name, param); + } + + + template + static inline int64_t floatSecondsToIntMicros(const FT v) + { + static_assert(std::is_floating_point_v, ""); + return static_cast(v * static_cast(1e6)); + } + + template + static inline int64_t constructTimestampMicros(const T seconds, const T nanoseconds) + { + return (static_cast(seconds) * 1000000L) + (static_cast(nanoseconds) / 1000L); + } + + inline tf2::TimePoint toTf2TimePoint(const builtin_interfaces::msg::Time& t) + { + return tf2::TimePoint{ + std::chrono::seconds{ t.sec } + + std::chrono::nanoseconds{ t.nanosec } }; + } + + + void _test(rclcpp::Node* node) + { + pcl::PointCloud _c; + sensor_msgs::msg::PointCloud2 _c2; + pcl::toROSMsg(_c, _c2); + RCLCPP_INFO(node->get_logger(), + "\n-------------------------------- TESTING --------------------------------" + "\nPointXYZ \"frame_id\": %s" + "\nPointCloud2 total fields: %ld" + "\nPointCloud2 field #1: { name: %s, offset: %d, datatype: %d, count: %d }" + "\nPointCloud2 field #2: { name: %s, offset: %d, datatype: %d, count: %d }" + "\nPointCloud2 field #3: { name: %s, offset: %d, datatype: %d, count: %d }" + "\n-------------------------------------------------------------------------", + _c.header.frame_id.c_str(), + _c2.fields.size(), + _c2.fields[0].name.c_str(), + _c2.fields[0].offset, + _c2.fields[0].datatype, + _c2.fields[0].count, + _c2.fields[1].name.c_str(), + _c2.fields[1].offset, + _c2.fields[1].datatype, + _c2.fields[1].count, + _c2.fields[2].name.c_str(), + _c2.fields[2].offset, + _c2.fields[2].datatype, + _c2.fields[2].count + ); + } }; -PerceptionNode::PerceptionNode() : rclcpp::Node("perception_node") { - RCLCPP_INFO(this->get_logger(), "Perception Node Initialization!"); - // setup subs/pubs - this->scan_sub = this->create_subscription("/filtered_cloud", 1, // gets remapped when using launchfile - std::bind(&PerceptionNode::scan_cb, this, std::placeholders::_1)); - this->pose_sub = this->create_subscription("/adjusted_pose", 1, // ^ - std::bind(&PerceptionNode::pose_cb, this, std::placeholders::_1)); - this->grid_pub = this->create_publisher("/ldrp/obstacle_grid", 1); - - // util::_test(this); - - //get parameters - util::declare_param(this, "map_resolution_cm", this->_config.map_resolution_cm, this->_config.map_resolution_cm); - util::declare_param(this, "voxel_size_cm", this->_config.voxel_size_cm, this->_config.voxel_size_cm); - util::declare_param(this, "max_z_thresh_cm", this->_config.max_z_thresh_cm, this->_config.max_z_thresh_cm); - util::declare_param(this, "min_z_thresh_cm", this->_config.min_z_thresh_cm, this->_config.min_z_thresh_cm); - util::declare_param(this, "pmf_max_range_cm", this->_config.pmf_max_range_cm, this->_config.pmf_max_range_cm); - util::declare_param(this, "pmf_window_base", this->_config.pmf_window_base, this->_config.pmf_window_base); - util::declare_param(this, "pmf_max_window_size_cm", this->_config.pmf_max_window_size_cm, this->_config.pmf_max_window_size_cm); - util::declare_param(this, "pmf_cell_size_cm", this->_config.pmf_cell_size_cm, this->_config.pmf_cell_size_cm); - util::declare_param(this, "pmf_init_distance_cm", this->_config.pmf_init_distance_cm, this->_config.pmf_init_distance_cm); - util::declare_param(this, "pmf_max_distance_cm", this->_config.pmf_max_distance_cm, this->_config.pmf_max_distance_cm); - util::declare_param(this, "pmf_slope", this->_config.pmf_slope, this->_config.pmf_slope); - util::declare_param(this, "scan_matching_history_s", this->_config.scan_matching_history_range_s, this->_config.scan_matching_history_range_s); - util::declare_param(this, "output_frame", this->_config.output_frame_id, this->_config.output_frame_id); - // util::declare_param(this, "TODO", this->_config.scan_matching_skip_invalid, this->_config.scan_matching_skip_invalid); - - // log params - RCLCPP_INFO(this->get_logger(), - "\n--------- Configured Params ---------" - "\nMap resolution (cm): %f" - "\nVoxel size (cm): %f" - "\nMax Z threshold (cm): %f" - "\nMin Z threshold (cm): %f" - "\nPMF range cutoff (cm): %f" - "\nPMF window base: %f" - "\nPMF max window size (cm): %f" - "\nPMF cell size (cm): %f" - "\nPMF init distance (cm): %f" - "\nPMF max distance (cm): %f" - "\nPMF slope: %f" - "\nScan matching history (s): %f" - "\n-------------------------------------", - this->_config.map_resolution_cm, - this->_config.voxel_size_cm, - this->_config.max_z_thresh_cm, - this->_config.min_z_thresh_cm, - this->_config.pmf_max_range_cm, - this->_config.pmf_window_base, - this->_config.pmf_max_window_size_cm, - this->_config.pmf_cell_size_cm, - this->_config.pmf_init_distance_cm, - this->_config.pmf_max_distance_cm, - this->_config.pmf_slope, - this->_config.scan_matching_history_range_s - ); - - this->accumulator.reset(this->_config.map_resolution_cm * 1e-2f); // use params +PerceptionNode::PerceptionNode() : + rclcpp::Node("perception_node"), + tf_buffer{ std::make_shared(RCL_ROS_TIME) }, + tf_listener{ tf_buffer } +{ + // RCLCPP_INFO(this->get_logger(), "Perception Node Initialization!"); + + std::string scan_topic; + util::declare_param(this, "scan_topic", scan_topic, "lidar_scan"); + // setup subs/pubs + this->scan_sub = this->create_subscription( + scan_topic, rclcpp::SensorDataQoS{}, + [this](const sensor_msgs::msg::PointCloud2::ConstSharedPtr& scan){ this->scan_callback(scan); } ); + this->grid_pub = this->create_publisher( + "obstacle_grid", rclcpp::SensorDataQoS{} ); + + //get parameters + util::declare_param(this, "max_tf_wait_time_s", this->_config.max_tf_wait_time_s, this->_config.max_tf_wait_time_s); + util::declare_param(this, "map_resolution_cm", this->_config.map_resolution_cm, this->_config.map_resolution_cm); + util::declare_param(this, "voxel_size_cm", this->_config.voxel_size_cm, this->_config.voxel_size_cm); + util::declare_param(this, "max_z_thresh_cm", this->_config.max_z_thresh_cm, this->_config.max_z_thresh_cm); + util::declare_param(this, "min_z_thresh_cm", this->_config.min_z_thresh_cm, this->_config.min_z_thresh_cm); + util::declare_param(this, "pmf_max_range_cm", this->_config.pmf_max_range_cm, this->_config.pmf_max_range_cm); + util::declare_param(this, "pmf_window_base", this->_config.pmf_window_base, this->_config.pmf_window_base); + util::declare_param(this, "pmf_max_window_size_cm", this->_config.pmf_max_window_size_cm, this->_config.pmf_max_window_size_cm); + util::declare_param(this, "pmf_cell_size_cm", this->_config.pmf_cell_size_cm, this->_config.pmf_cell_size_cm); + util::declare_param(this, "pmf_init_distance_cm", this->_config.pmf_init_distance_cm, this->_config.pmf_init_distance_cm); + util::declare_param(this, "pmf_max_distance_cm", this->_config.pmf_max_distance_cm, this->_config.pmf_max_distance_cm); + util::declare_param(this, "pmf_slope", this->_config.pmf_slope, this->_config.pmf_slope); + util::declare_param(this, "map_frame", this->_config.map_frame, this->_config.map_frame); + util::declare_param(this, "lidar_frame", this->_config.lidar_origin_frame, this->_config.lidar_origin_frame); + + // log params + RCLCPP_INFO(this->get_logger(), + "\n--------- Configured Params ---------" + "\nMap resolution (cm): %f" + "\nVoxel size (cm): %f" + "\nMax Z threshold (cm): %f" + "\nMin Z threshold (cm): %f" + "\nPMF range cutoff (cm): %f" + "\nPMF window base: %f" + "\nPMF max window size (cm): %f" + "\nPMF cell size (cm): %f" + "\nPMF init distance (cm): %f" + "\nPMF max distance (cm): %f" + "\nPMF slope: %f" + "\nTf wait time (s): %f" + "\n-------------------------------------", + this->_config.map_resolution_cm, + this->_config.voxel_size_cm, + this->_config.max_z_thresh_cm, + this->_config.min_z_thresh_cm, + this->_config.pmf_max_range_cm, + this->_config.pmf_window_base, + this->_config.pmf_max_window_size_cm, + this->_config.pmf_cell_size_cm, + this->_config.pmf_init_distance_cm, + this->_config.pmf_max_distance_cm, + this->_config.pmf_slope, + this->_config.max_tf_wait_time_s + ); + + this->accumulator.reset(this->_config.map_resolution_cm * 1e-2f); // use params } @@ -174,289 +192,229 @@ PerceptionNode::~PerceptionNode() {} -void PerceptionNode::scan_cb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& scan) { - // this->sampler_mutex.lock(); - RCLCPP_INFO(this->get_logger(), "Scan callback called!"); - - pcl::PointCloud::Ptr point_cloud = std::make_shared>(); - pcl::fromROSMsg(*scan, *point_cloud); - - const int64_t ts = util::constructTimestampMicros(scan->header.stamp.sec, scan->header.stamp.nanosec); - this->sampler_mutex.lock(); - this->scan_sampler.insert( ts, point_cloud ); - this->scan_sampler.updateMin(ts - util::floatSecondsToIntMicros(this->_config.scan_matching_history_range_s)); - const size_t current_nsamples = this->scan_sampler.getSamples().size(); - this->sampler_mutex.unlock(); - - RCLCPP_INFO(this->get_logger(), - "\n\tScan sample timestamp (us): %ld" - "\n\tTotal scan samples: %ld", - ts, - current_nsamples - ); - // this->sampler_mutex.unlock(); - +void PerceptionNode::scan_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& scan) +{ + // RCLCPP_INFO(this->get_logger(), "Scan callback called!"); + + pcl::PointCloud point_cloud{}; + Eigen::Vector3f origin = Eigen::Vector3f::Zero(); + try + { + sensor_msgs::msg::PointCloud2 scan_{}; + + auto tf = this->tf_buffer.lookupTransform( + this->_config.map_frame, + scan->header.frame_id, + util::toTf2TimePoint(scan->header.stamp), + std::chrono::nanoseconds{ static_cast(this->_config.max_tf_wait_time_s * 1e9) } ); + + // TODO: use last transform as final attempt + + tf2::doTransform(*scan, scan_, tf); + pcl::fromROSMsg(scan_, point_cloud); + + origin.x() = -tf.transform.translation.x; // use this transform at least for origin + origin.y() = -tf.transform.translation.y; + origin.z() = -tf.transform.translation.z; + } + catch(const std::exception& e) + { + RCLCPP_INFO(this->get_logger(), "[SICK PERCEPTION]: Failed to transform scan.\n\twhat(): %s", e.what()); + return; + } + + try + { + // lookup actual lidar origin + auto tf = this->tf_buffer.lookupTransform( + this->_config.lidar_origin_frame, + this->_config.map_frame, + util::toTf2TimePoint(scan->header.stamp) ); + + origin.x() = tf.transform.translation.x; + origin.y() = tf.transform.translation.y; + origin.z() = tf.transform.translation.z; + } + catch(const std::exception& e) + { + RCLCPP_INFO(this->get_logger(), "[SICK PERCEPTION]: Failed to lookup scan origin."); + } + + this->process_and_export(point_cloud, origin); } +void PerceptionNode::process(const pcl::PointCloud& cloud, const Eigen::Vector3f& origin) +{ + // RCLCPP_INFO(this->get_logger(), "Processing..."); -void PerceptionNode::pose_cb(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& pose) { - // this->sampler_mutex.lock(); - RCLCPP_INFO(this->get_logger(), "Transform callback called!"); - - const int64_t trfm_target_ts = util::constructTimestampMicros(pose->header.stamp.sec, pose->header.stamp.nanosec); - this->sampler_mutex.lock(); - const typename decltype(this->scan_sampler)::ElemT* sample = this->scan_sampler.sampleTimestamped(trfm_target_ts); - this->sampler_mutex.unlock(); - - // if scan doesn't exist - if(!sample) { - RCLCPP_INFO(this->get_logger(), - "Invalid sample recieved!" - "\n\tTransform timestamp (us): %ld", - trfm_target_ts - ); - // this->sampler_mutex.unlock(); - return; - } - - // transform age - scan age - const int64_t ts_diff = trfm_target_ts - sample->first; - - // if scan came after the transform do not use it - // if(ts_diff < 0) { - // RCLCPP_INFO(this->get_logger(), - // "Invalid timestamp delta!" - // "\n\tTransform timestamp (ms): %ld" - // "\n\tSampled timestamp (ms): %ld" - // "\n\tTimestamp difference (ms): %ld", - // trfm_target_ts, - // sample->first, - // ts_diff - // ); - // this->sampler_mutex.unlock(); - // return; - // } - - // valid case to use transform with scan - if(abs(ts_diff) < util::floatSecondsToIntMicros(this->_config.scan_matching_history_range_s)) { // config for 'epsilon' - - pcl::PointCloud::Ptr scan = sample->second; - this->sampler_mutex.lock(); - this->scan_sampler.updateMin(sample->first + 1); // don't use 'sample' after here since it is invalid - const size_t current_nsamples = this->scan_sampler.getSamples().size(); - this->sampler_mutex.unlock(); - - RCLCPP_INFO(this->get_logger(), - "\n\tTransform timestamp (us): %ld" - "\n\tSampled timestamp (us): %ld" - "\n\tTimestamp difference (us): %ld" - "\n\tTotal scan samples: %ld", - trfm_target_ts, - sample->first, - ts_diff, - current_nsamples - ); - // this->sampler_mutex.unlock(); - - const Eigen::Quaternionf quat{ - static_cast(pose->pose.orientation.w), - static_cast(pose->pose.orientation.x), - static_cast(pose->pose.orientation.y), - static_cast(pose->pose.orientation.z) - }; - const Eigen::Translation3f pos{ - static_cast(pose->pose.position.x), - static_cast(pose->pose.position.y), - static_cast(pose->pose.position.z) - }; - - pcl::transformPointCloud( - *scan, *scan, // :O - (pos * quat) // be very careful with the order of these uWu - ); - - this->process_and_export(*scan, *reinterpret_cast(&pos)); - - } - // this->sampler_mutex.unlock(); - -} - - - -void PerceptionNode::process(const pcl::PointCloud& cloud, const Eigen::Vector3f& origin) { - - RCLCPP_INFO(this->get_logger(), "Processing..."); - - static const pcl::Indices DEFAULT_NO_SELECTION = pcl::Indices{}; + static const pcl::Indices DEFAULT_NO_SELECTION = pcl::Indices{}; - pcl::PointCloud - voxel_cloud; - pcl::Indices - z_high_filtered{}, - z_low_subset_filtered{}, - z_mid_filtered_obstacles{}, - pre_pmf_range_filtered{}, - pmf_filtered_ground{}, - pmf_filtered_obstacles{}, - combined_obstacles{}; + pcl::PointCloud + voxel_cloud; + pcl::Indices + z_high_filtered{}, + z_low_subset_filtered{}, + z_mid_filtered_obstacles{}, + pre_pmf_range_filtered{}, + pmf_filtered_ground{}, + pmf_filtered_obstacles{}, + combined_obstacles{}; #if LDRP_ENABLE_TUNING - float - voxel_size_m = (float) this->get_parameter("voxel_size_cm").as_double() * 1e-2f, - max_z_thresh_m = (float) this->get_parameter("max_z_thresh_cm").as_double() * 1e-2f, - min_z_thresh_m = (float) this->get_parameter("min_z_thresh_cm").as_double() * 1e-2f, - max_pmf_range_m = (float) this->get_parameter("pmf_max_range_cm").as_double() * 1e-2f, - pmf_window_base = (float) this->get_parameter("pmf_window_base").as_double(), - pmf_max_window_size_m = (float) this->get_parameter("pmf_max_window_size_cm").as_double() * 1e-2f, - pmf_cell_size_m = (float) this->get_parameter("pmf_cell_size_cm").as_double() * 1e-2f, - pmf_init_distance_m = (float) this->get_parameter("pmf_init_distance_cm").as_double() * 1e-2f, - pmf_max_distance_m = (float) this->get_parameter("pmf_max_distance_cm").as_double() * 1e-2f, - pmf_slope = (float) this->get_parameter("pmf_slope").as_double() - ; + float + voxel_size_m = (float) this->get_parameter("voxel_size_cm").as_double() * 1e-2f, + max_z_thresh_m = (float) this->get_parameter("max_z_thresh_cm").as_double() * 1e-2f, + min_z_thresh_m = (float) this->get_parameter("min_z_thresh_cm").as_double() * 1e-2f, + max_pmf_range_m = (float) this->get_parameter("pmf_max_range_cm").as_double() * 1e-2f, + pmf_window_base = (float) this->get_parameter("pmf_window_base").as_double(), + pmf_max_window_size_m = (float) this->get_parameter("pmf_max_window_size_cm").as_double() * 1e-2f, + pmf_cell_size_m = (float) this->get_parameter("pmf_cell_size_cm").as_double() * 1e-2f, + pmf_init_distance_m = (float) this->get_parameter("pmf_init_distance_cm").as_double() * 1e-2f, + pmf_max_distance_m = (float) this->get_parameter("pmf_max_distance_cm").as_double() * 1e-2f, + pmf_slope = (float) this->get_parameter("pmf_slope").as_double() + ; #else - float - voxel_size_m = (float) this->_config.voxel_size_cm * 1e-2f, - max_z_thresh_m = (float) this->_config.max_z_thresh_cm * 1e-2f, - min_z_thresh_m = (float) this->_config.min_z_thresh_cm * 1e-2f, - max_pmf_range_m = (float) this->_config.pmf_max_range_cm * 1e-2f, - pmf_window_base = (float) this->_config.pmf_window_base, - pmf_max_window_size_m = (float) this->_config.pmf_max_window_size_cm * 1e-2f, - pmf_cell_size_m = (float) this->_config.pmf_cell_size_cm * 1e-2f, - pmf_init_distance_m = (float) this->_config.pmf_init_distance_cm * 1e-2f, - pmf_max_distance_m = (float) this->_config.pmf_max_distance_cm * 1e-2f, - pmf_slope = (float) this->_config.pmf_slope; - ; + float + voxel_size_m = (float) this->_config.voxel_size_cm * 1e-2f, + max_z_thresh_m = (float) this->_config.max_z_thresh_cm * 1e-2f, + min_z_thresh_m = (float) this->_config.min_z_thresh_cm * 1e-2f, + max_pmf_range_m = (float) this->_config.pmf_max_range_cm * 1e-2f, + pmf_window_base = (float) this->_config.pmf_window_base, + pmf_max_window_size_m = (float) this->_config.pmf_max_window_size_cm * 1e-2f, + pmf_cell_size_m = (float) this->_config.pmf_cell_size_cm * 1e-2f, + pmf_init_distance_m = (float) this->_config.pmf_init_distance_cm * 1e-2f, + pmf_max_distance_m = (float) this->_config.pmf_max_distance_cm * 1e-2f, + pmf_slope = (float) this->_config.pmf_slope; + ; #endif - /** WARNING: The below algorithm is tricky since an empty, "null" selection - * accomplishes different things depending on the specific method call! - * The current control-flow is specifically engineered to avoid misinterpretations. */ - - // voxelize points - voxel_filter( - cloud, DEFAULT_NO_SELECTION, voxel_cloud, // <-- default selection = use all points - voxel_size_m, voxel_size_m, voxel_size_m - ); - - // filter points under "high cut" thresh - carteZ_filter( - voxel_cloud, DEFAULT_NO_SELECTION, z_high_filtered, // <-- default selection = use all points - -std::numeric_limits::infinity(), - max_z_thresh_m - ); - if(z_high_filtered.size() <= 0) return; // no points to process - - // further filter points below "low cut" thresh - carteZ_filter( - voxel_cloud, z_high_filtered, z_low_subset_filtered, - -std::numeric_limits::infinity(), - min_z_thresh_m - ); - - const bool - only_z_mid = z_low_subset_filtered.size() <= 0, // no points at "groud level" --> skip advanced processing and just add these to the grid - has_z_mid = z_low_subset_filtered.size() != z_high_filtered.size(); // are there any points at all in the "wall range"? --> if not, don't attempt to negate the selection, or add points to the grid - if(!only_z_mid) { // need points in order to continue (otherwise this gets misinterpretted as "use all points") - - if(has_z_mid) { - // get the points inbetween high and low thresholds --> treated as wall obstacles - pc_negate_selection( - z_high_filtered, - z_low_subset_filtered, - z_mid_filtered_obstacles - ); - } - - // filter close enough points for PMF - pc_filter_distance( - voxel_cloud.points, - z_low_subset_filtered, - pre_pmf_range_filtered, // NOTE: this will contain all indices even if the entire cloud is selected (no default empty select) - 0.f, max_pmf_range_m, - origin - ); - if(pre_pmf_range_filtered.size() > 0) { // need points in order to continue (otherwise this gets reinterpretted as "use all points") - - // apply pmf to selected points - progressive_morph_filter( - voxel_cloud, pre_pmf_range_filtered, pmf_filtered_ground, // see the note above -- an empty output selection means no ground points - pmf_window_base, - pmf_max_window_size_m, - pmf_cell_size_m, - pmf_init_distance_m, - pmf_max_distance_m, - pmf_slope, - false - ); - - // obstacles = (base - ground) - if(pmf_filtered_ground.size() == 0) { // the case where all subset points are obstacles... - pmf_filtered_obstacles.clear(); - std::swap(pre_pmf_range_filtered, pmf_filtered_obstacles); // setup the "accumulator" selection as containing the selection (null base) to trigger "case 2" of incrementRatio best efficiency - } else { - pc_negate_selection( // normal set difference computation - pre_pmf_range_filtered, - pmf_filtered_ground, - pmf_filtered_obstacles - ); - } // in the case that all prefiltered points are ground, incrementRatio opperates correctly by default - - this->accumulator.incrementRatio, 50, 100>( // insert PMF obstacles - voxel_cloud, - pre_pmf_range_filtered, // base - pmf_filtered_obstacles // subset (accumulator) - ); - - } else {} // no points to process for PMF... - - } else {} // no low (groud) points to process - if(has_z_mid) { // only attempt if z_mid_filtered_obstacles is not empty to prevent a misinterprettation by incrementRatio (this would increment all the cells in the grid) - this->accumulator.incrementRatio, 50, 100>( // insert z-thresh obstacles (increment both halves of ratio) - voxel_cloud, - DEFAULT_NO_SELECTION, // trigger "case 2" of incrementRatio (recently changed) - (only_z_mid ? z_high_filtered : z_mid_filtered_obstacles) // accumulator --> increments both for this selection since base is null - ); - } - + /** WARNING: The below algorithm is tricky since an empty, "null" selection + * accomplishes different things depending on the specific method call! + * The current control-flow is specifically engineered to avoid misinterpretations, + * so modifying this code at all may result in unintended and hard to find bugs! */ + + // voxelize points + voxel_filter( + cloud, DEFAULT_NO_SELECTION, voxel_cloud, // <-- default selection = use all points + voxel_size_m, voxel_size_m, voxel_size_m + ); + + // filter points under "high cut" thresh + carteZ_filter( + voxel_cloud, DEFAULT_NO_SELECTION, z_high_filtered, // <-- default selection = use all points + -std::numeric_limits::infinity(), + max_z_thresh_m + ); + if(z_high_filtered.size() <= 0) return; // no points to process + + // further filter points below "low cut" thresh + carteZ_filter( + voxel_cloud, z_high_filtered, z_low_subset_filtered, + -std::numeric_limits::infinity(), + min_z_thresh_m + ); + + const bool + only_z_mid = z_low_subset_filtered.size() <= 0, // no points at "groud level" --> skip advanced processing and just add these to the grid + has_z_mid = z_low_subset_filtered.size() != z_high_filtered.size(); // are there any points at all in the "wall range"? --> if not, don't attempt to negate the selection, or add points to the grid + if(!only_z_mid) { // need points in order to continue (otherwise this gets misinterpretted as "use all points") + + if(has_z_mid) { + // get the points inbetween high and low thresholds --> treated as wall obstacles + pc_negate_selection( + z_high_filtered, + z_low_subset_filtered, + z_mid_filtered_obstacles + ); + } + + // filter close enough points for PMF + pc_filter_distance( + voxel_cloud.points, + z_low_subset_filtered, + pre_pmf_range_filtered, // NOTE: this will contain all indices even if the entire cloud is selected (no default empty select) + 0.f, max_pmf_range_m, + origin + ); + if(pre_pmf_range_filtered.size() > 0) { // need points in order to continue (otherwise this gets reinterpretted as "use all points") + + // apply pmf to selected points + progressive_morph_filter( + voxel_cloud, pre_pmf_range_filtered, pmf_filtered_ground, // see the note above -- an empty output selection means no ground points + pmf_window_base, + pmf_max_window_size_m, + pmf_cell_size_m, + pmf_init_distance_m, + pmf_max_distance_m, + pmf_slope, + false + ); + + // obstacles = (base - ground) + if(pmf_filtered_ground.size() == 0) { // the case where all subset points are obstacles... + pmf_filtered_obstacles.clear(); + std::swap(pre_pmf_range_filtered, pmf_filtered_obstacles); // setup the "accumulator" selection as containing the selection (null base) to trigger "case 2" of incrementRatio best efficiency + } else { + pc_negate_selection( // normal set difference computation + pre_pmf_range_filtered, + pmf_filtered_ground, + pmf_filtered_obstacles + ); + } // in the case that all prefiltered points are ground, incrementRatio opperates correctly by default + + this->accumulator.incrementRatio, 50, 100>( // insert PMF obstacles + voxel_cloud, + pre_pmf_range_filtered, // base + pmf_filtered_obstacles // subset (accumulator) + ); + + } else {} // no points to process for PMF... + + } else {} // no low (groud) points to process + if(has_z_mid) { // only attempt if z_mid_filtered_obstacles is not empty to prevent a misinterprettation by incrementRatio (this would increment all the cells in the grid) + this->accumulator.incrementRatio, 50, 100>( // insert z-thresh obstacles (increment both halves of ratio) + voxel_cloud, + DEFAULT_NO_SELECTION, // trigger "case 2" of incrementRatio (recently changed) + (only_z_mid ? z_high_filtered : z_mid_filtered_obstacles) // accumulator --> increments both for this selection since base is null + ); + } } -void PerceptionNode::process_and_export(const pcl::PointCloud& cloud, const Eigen::Vector3f& origin) { - - this->process(cloud, origin); - - nav_msgs::msg::OccupancyGrid out_grid; - - const size_t _area = static_cast(this->accumulator.area()); - const Eigen::Vector2f& _origin = this->accumulator.origin(); - const Eigen::Vector2i& _grid_size = this->accumulator.size(); - - out_grid.info.resolution = this->accumulator.cellRes(); - out_grid.info.origin.position.x = _origin.x(); - out_grid.info.origin.position.y = _origin.y(); - out_grid.info.width = _grid_size.x(); - out_grid.info.height = _grid_size.y(); - - out_grid.data.resize(_area); - memcpy(out_grid.data.data(), this->accumulator.buffData(), _area); - - out_grid.header.frame_id = this->_config.output_frame_id; - out_grid.header.stamp = this->get_clock()->now(); - - this->grid_pub->publish(out_grid); - - RCLCPP_INFO(this->get_logger(), - "Grid Updated!" - "\n\tOrigin: (%f, %f)" - "\n\tDims: (%d, %d)" - "\n\tSize: (%f, %f)", - _origin.x(), - _origin.y(), - _grid_size.x(), - _grid_size.y(), - _grid_size.x() * this->accumulator.cellRes(), - _grid_size.y() * this->accumulator.cellRes() - ); - +void PerceptionNode::process_and_export(const pcl::PointCloud& cloud, const Eigen::Vector3f& origin) +{ + this->process(cloud, origin); + + nav_msgs::msg::OccupancyGrid out_grid; + + const size_t _area = static_cast(this->accumulator.area()); + const Eigen::Vector2f& _origin = this->accumulator.origin(); + const Eigen::Vector2i& _grid_size = this->accumulator.size(); + + out_grid.info.resolution = this->accumulator.cellRes(); + out_grid.info.origin.position.x = _origin.x(); + out_grid.info.origin.position.y = _origin.y(); + out_grid.info.width = _grid_size.x(); + out_grid.info.height = _grid_size.y(); + + out_grid.data.resize(_area); + memcpy(out_grid.data.data(), this->accumulator.buffData(), _area); + + out_grid.header.frame_id = this->_config.map_frame; + out_grid.header.stamp = this->get_clock()->now(); + + this->grid_pub->publish(out_grid); + + // RCLCPP_INFO(this->get_logger(), + // "Grid Updated!" + // "\n\tOrigin: (%f, %f)" + // "\n\tDims: (%d, %d)" + // "\n\tSize: (%f, %f)", + // _origin.x(), + // _origin.y(), + // _grid_size.x(), + // _grid_size.y(), + // _grid_size.x() * this->accumulator.cellRes(), + // _grid_size.y() * this->accumulator.cellRes() + // ); } diff --git a/src/ldrp/perception.hpp b/src/ldrp/perception.hpp index 67dc4c1..8b51730 100644 --- a/src/ldrp/perception.hpp +++ b/src/ldrp/perception.hpp @@ -2,29 +2,29 @@ /** Default macro definitions -- configurable via CMake */ #ifndef LDRP_ENABLE_LOGGING // enable/disable all logging output - #define LDRP_ENABLE_LOGGING true +#define LDRP_ENABLE_LOGGING true #endif #ifndef LDRP_DEBUG_LOGGING // enable/disable debug level logging - #define LDRP_DEBUG_LOGGING false +#define LDRP_DEBUG_LOGGING false #endif #ifndef LDRP_SAFETY_CHECKS // enable/disable additional safety checking (ex. bound checking) - #define LDRP_SAFETY_CHECKS true +#define LDRP_SAFETY_CHECKS true #endif #ifndef LDRP_USE_UESIM // whether or not WPILib is being compiled into the library - for build system internal use only - #define LDRP_USE_UESIM false +#define LDRP_USE_UESIM false #endif #ifndef LDRP_USE_UESIM // enable/disable using simulation as the source of points, and set which simulation source to use (1 = internal, 2 = UE simulator) - #define LDRP_USE_UESIM false +#define LDRP_USE_UESIM false #endif #ifndef LDRP_ENABLE_TUNING // enable/disable live tuning using networktables (requires WPILib) - #define LDRP_ENABLE_TUNING false +#define LDRP_ENABLE_TUNING false #endif #ifndef LDRP_ENABLE_PROFILING // enable/disable live and logged filter pipeline profiling over networktables - #define LDRP_ENABLE_PROFILING false +#define LDRP_ENABLE_PROFILING false #endif #if !LDRP_SAFETY_CHECKS - #define GRID_IMPL_SKIP_BOUND_CHECKING // disables array bound checking within QRG +#define GRID_IMPL_SKIP_BOUND_CHECKING // disables array bound checking within QRG #endif #include "./grid.hpp" #include "./timestamp_sampler.hpp" @@ -36,63 +36,60 @@ #include #include +#include +#include #include -#include #include class PerceptionNode : public rclcpp::Node { public: - PerceptionNode(); - ~PerceptionNode(); + PerceptionNode(); + ~PerceptionNode(); protected: - void scan_cb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& scan); - void pose_cb(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& pose); + void scan_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& scan); - void process(const pcl::PointCloud& cloud, const Eigen::Vector3f& origin); - void process_and_export(const pcl::PointCloud& cloud, const Eigen::Vector3f& origin); + void process(const pcl::PointCloud& cloud, const Eigen::Vector3f& origin); + void process_and_export(const pcl::PointCloud& cloud, const Eigen::Vector3f& origin); protected: - rclcpp::Subscription::SharedPtr scan_sub; - rclcpp::Subscription::SharedPtr pose_sub; - rclcpp::Publisher::SharedPtr grid_pub; - - // last pointcloud and transform? - TimestampSampler::Ptr, int64_t> scan_sampler{}; - QuantizedRatioGrid accumulator{}; - - std::mutex sampler_mutex{}; - - struct { - - // bool - // scan_matching_skip_invalid = false; - double - scan_matching_history_range_s = 0.25, - map_resolution_cm = 5., - voxel_size_cm = 3., - max_z_thresh_cm = 100., - min_z_thresh_cm = 25., - pmf_max_range_cm = 250., - pmf_window_base = 2., - pmf_max_window_size_cm = 48., - pmf_cell_size_cm = 5., - pmf_init_distance_cm = 5., - pmf_max_distance_cm = 12., - pmf_slope = 2.; - - std::string - output_frame_id = "world"; - - } _config; + tf2_ros::Buffer tf_buffer; + tf2_ros::TransformListener tf_listener; + + rclcpp::Subscription::SharedPtr scan_sub; + rclcpp::Publisher::SharedPtr grid_pub; + + QuantizedRatioGrid accumulator{}; + + struct { + + double + max_tf_wait_time_s = 0.25, + map_resolution_cm = 5., + voxel_size_cm = 3., + max_z_thresh_cm = 100., + min_z_thresh_cm = 25., + pmf_max_range_cm = 250., + pmf_window_base = 2., + pmf_max_window_size_cm = 48., + pmf_cell_size_cm = 5., + pmf_init_distance_cm = 5., + pmf_max_distance_cm = 12., + pmf_slope = 2.; + + std::string + map_frame = "map", + lidar_origin_frame = "lidar_link"; + + } _config; #if LDRP_ENABLE_PROFILING - struct { - - } _profile; + struct { + + } _profile; #endif diff --git a/src/ldrp/timestamp_sampler.hpp b/src/ldrp/timestamp_sampler.hpp index 27b3a4e..6cb586e 100644 --- a/src/ldrp/timestamp_sampler.hpp +++ b/src/ldrp/timestamp_sampler.hpp @@ -9,91 +9,91 @@ template class TimestampSampler { public: - using This_T = TimestampSampler; - using Type = T; - using TimeT = Time_t; - using ElemT = std::pair; + using This_T = TimestampSampler; + using Type = T; + using TimeT = Time_t; + using ElemT = std::pair; private: - static bool t_less__(TimeT t, const ElemT& elem) { - return t < elem.first; - } - static bool t_greater__(const ElemT& elem, TimeT t) { - return t > elem.first; - } + static bool t_less__(TimeT t, const ElemT& elem) { + return t < elem.first; + } + static bool t_greater__(const ElemT& elem, TimeT t) { + return t > elem.first; + } public: - TimestampSampler() = default; - ~TimestampSampler() = default; + TimestampSampler() = default; + ~TimestampSampler() = default; - /** Update the minimum bound timestamp such as to limit the number of entities stored in the buffer */ - void updateMin(TimeT min) { - this->absolute_bound = min; - this->enforceBound(); - } + /** Update the minimum bound timestamp such as to limit the number of entities stored in the buffer */ + void updateMin(TimeT min) { + this->absolute_bound = min; + this->enforceBound(); + } - /** Add a new sample to the buffer */ - inline void insert(TimeT time, const Type& sample) - { this->_insert(time, Type{ sample }); } - /** Add a new sample to the buffer */ - inline void insert(TimeT time, Type&& sample) - { this->_insert(time, sample); } + /** Add a new sample to the buffer */ + inline void insert(TimeT time, const Type& sample) + { this->_insert(time, Type{ sample }); } + /** Add a new sample to the buffer */ + inline void insert(TimeT time, Type&& sample) + { this->_insert(time, sample); } - /** Erase all the elements */ - inline void clear() { this->samples.clear(); } + /** Erase all the elements */ + inline void clear() { this->samples.clear(); } - /** Sample the element with a timestamp closest to that given as a parameter */ - inline const Type* sample(TimeT time) const { - const ElemT* elem = this->sampleTimestamped(time); - return elem ? elem->second : nullptr; - } - /** Sample the closest element, but returns the full pair so that the element's timestamp can be accessed */ - const ElemT* sampleTimestamped(TimeT time) const { - if( this->samples.empty() ) return nullptr; - if( time <= this->samples.front().first ) return &this->samples.front(); - if( time >= this->samples.back().first ) return &this->samples.back(); - if( this->samples.size() == 1 ) return &this->samples[0]; + /** Sample the element with a timestamp closest to that given as a parameter */ + inline const Type* sample(TimeT time) const { + const ElemT* elem = this->sampleTimestamped(time); + return elem ? elem->second : nullptr; + } + /** Sample the closest element, but returns the full pair so that the element's timestamp can be accessed */ + const ElemT* sampleTimestamped(TimeT time) const { + if( this->samples.empty() ) return nullptr; + if( time <= this->samples.front().first ) return &this->samples.front(); + if( time >= this->samples.back().first ) return &this->samples.back(); + if( this->samples.size() == 1 ) return &this->samples[0]; - auto greater = std::lower_bound(this->samples.begin(), this->samples.end(), time, &This_T::t_greater__); // "lower bound" of times greater than t - if( greater == this->samples.begin() ) return &*greater; + auto greater = std::lower_bound(this->samples.begin(), this->samples.end(), time, &This_T::t_greater__); // "lower bound" of times greater than t + if( greater == this->samples.begin() ) return &*greater; - auto less = greater - 1; - return abs(time - less->first) < abs(greater->first - time) ? &*less : &*greater; // return closer sample - } + auto less = greater - 1; + return abs(time - less->first) < abs(greater->first - time) ? &*less : &*greater; // return closer sample + } - /** Return a const reference to the internal element buffer */ - inline const std::vector& getSamples() const { return this->samples; } + /** Return a const reference to the internal element buffer */ + inline const std::vector& getSamples() const { return this->samples; } protected: - void _insert(TimeT time, Type&& sample) { - if(this->samples.size() <= 0 || time > this->samples.back().first) { - this->samples.emplace_back(time, std::forward(sample)); - } else { - auto after = std::upper_bound(this->samples.begin(), this->samples.end(), time, &This_T::t_less__); - auto before = after - 1; - if(after == this->samples.begin()) { - this->samples.insert(after, std::pair{time, std::forward(sample)}); - } else { - if(before == this->samples.begin() || before->first < time) { - this->samples.insert(after, std::pair{time, std::forward(sample)}); - } else { - before->second = sample; - } - } - } - } - - void enforceBound() { - auto last = this->samples.begin(); - for(; last < this->samples.end() && last->first < this->absolute_bound; last++); - this->samples.erase(this->samples.begin(), last); - } + void _insert(TimeT time, Type&& sample) { + if(this->samples.size() <= 0 || time > this->samples.back().first) { + this->samples.emplace_back(time, std::forward(sample)); + } else { + auto after = std::upper_bound(this->samples.begin(), this->samples.end(), time, &This_T::t_less__); + auto before = after - 1; + if(after == this->samples.begin()) { + this->samples.insert(after, std::pair{time, std::forward(sample)}); + } else { + if(before == this->samples.begin() || before->first < time) { + this->samples.insert(after, std::pair{time, std::forward(sample)}); + } else { + before->second = sample; + } + } + } + } + + void enforceBound() { + auto last = this->samples.begin(); + for(; last < this->samples.end() && last->first < this->absolute_bound; last++); + this->samples.erase(this->samples.begin(), last); + } protected: - TimeT absolute_bound = static_cast(0); // the absolute minimum bounding time - std::vector samples{}; // use a tree for better search performance + TimeT absolute_bound = static_cast(0); // the absolute minimum bounding time + std::vector samples{}; // use a tree for better search performance }; diff --git a/src/main.cpp b/src/main.cpp index df86eff..fac0c72 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -5,12 +5,11 @@ #include -int main(int argc, char** argv) { - - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - - return 0; +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; }