diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
index 12961c1470eb9..aef7220680d67 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -30,7 +30,6 @@
-
@@ -74,7 +73,6 @@
-
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
index 3a24ec2fdf500..23f3bd0b532a0 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
@@ -4,7 +4,6 @@
-
@@ -60,7 +59,6 @@
-
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
index 6d67df5fa7a1f..9ab48a678240d 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
@@ -5,7 +5,6 @@
-
@@ -34,7 +33,6 @@
-
diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index 204fb561b10ea..58ee60b9409e0 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -58,7 +58,6 @@
-
-
diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py
index 82ce5605b67cd..ba6e9e72107c8 100644
--- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py
+++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py
@@ -16,10 +16,9 @@
from launch.actions import DeclareLaunchArgument
from launch.actions import OpaqueFunction
from launch.conditions import IfCondition
-from launch.substitutions import AndSubstitution
+from launch.conditions import UnlessCondition
from launch.substitutions import AnonName
from launch.substitutions import LaunchConfiguration
-from launch.substitutions import NotSubstitution
from launch_ros.actions import ComposableNodeContainer
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode
@@ -165,7 +164,7 @@ def load_composable_node_param(param_path):
)
# set euclidean cluster as a component
- use_downsample_euclidean_cluster_component = ComposableNode(
+ euclidean_cluster_component = ComposableNode(
package=pkg,
namespace=ns,
plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode",
@@ -177,108 +176,40 @@ def load_composable_node_param(param_path):
parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")],
)
- use_map_disuse_downsample_euclidean_component = ComposableNode(
- package=pkg,
- namespace=ns,
- plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode",
- name="euclidean_cluster",
- remappings=[
- ("input", "map_filter/pointcloud"),
- ("output", LaunchConfiguration("output_clusters")),
- ],
- parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")],
- )
- disuse_map_disuse_downsample_euclidean_component = ComposableNode(
- package=pkg,
- namespace=ns,
- plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode",
- name="euclidean_cluster",
- remappings=[
- ("input", LaunchConfiguration("input_pointcloud")),
- ("output", LaunchConfiguration("output_clusters")),
- ],
- parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")],
- )
-
container = ComposableNodeContainer(
name="euclidean_cluster_container",
package="rclcpp_components",
namespace=ns,
executable="component_container",
- composable_node_descriptions=[],
+ composable_node_descriptions=[
+ voxel_grid_filter_component,
+ outlier_filter_component,
+ downsample_concat_component,
+ euclidean_cluster_component,
+ ],
output="screen",
)
- use_map_use_downsample_loader = LoadComposableNodes(
+ use_map_loader = LoadComposableNodes(
composable_node_descriptions=[
compare_map_filter_component,
use_map_short_range_crop_box_filter_component,
use_map_long_range_crop_box_filter_component,
- voxel_grid_filter_component,
- outlier_filter_component,
- downsample_concat_component,
- use_downsample_euclidean_cluster_component,
],
target_container=container,
- condition=IfCondition(
- AndSubstitution(
- LaunchConfiguration("use_pointcloud_map"),
- LaunchConfiguration("use_downsample_pointcloud"),
- )
- ),
+ condition=IfCondition(LaunchConfiguration("use_pointcloud_map")),
)
- disuse_map_use_downsample_loader = LoadComposableNodes(
+ disuse_map_loader = LoadComposableNodes(
composable_node_descriptions=[
disuse_map_short_range_crop_box_filter_component,
disuse_map_long_range_crop_box_filter_component,
- voxel_grid_filter_component,
- outlier_filter_component,
- downsample_concat_component,
- use_downsample_euclidean_cluster_component,
],
target_container=container,
- condition=IfCondition(
- AndSubstitution(
- NotSubstitution(LaunchConfiguration("use_pointcloud_map")),
- LaunchConfiguration("use_downsample_pointcloud"),
- )
- ),
+ condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")),
)
- use_map_disuse_downsample_loader = LoadComposableNodes(
- composable_node_descriptions=[
- compare_map_filter_component,
- use_map_disuse_downsample_euclidean_component,
- ],
- target_container=container,
- condition=IfCondition(
- AndSubstitution(
- (LaunchConfiguration("use_pointcloud_map")),
- NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")),
- )
- ),
- )
-
- disuse_map_disuse_downsample_loader = LoadComposableNodes(
- composable_node_descriptions=[
- disuse_map_disuse_downsample_euclidean_component,
- ],
- target_container=container,
- condition=IfCondition(
- AndSubstitution(
- NotSubstitution(LaunchConfiguration("use_pointcloud_map")),
- NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")),
- )
- ),
- )
- return [
- container,
- use_map_use_downsample_loader,
- disuse_map_use_downsample_loader,
- use_map_disuse_downsample_loader,
- disuse_map_disuse_downsample_loader,
- ]
+ return [container, use_map_loader, disuse_map_loader]
def generate_launch_description():
@@ -291,7 +222,6 @@ def add_launch_arg(name: str, default_value=None):
add_launch_arg("input_map", "/map/pointcloud_map"),
add_launch_arg("output_clusters", "clusters"),
add_launch_arg("use_pointcloud_map", "false"),
- add_launch_arg("use_downsample_pointcloud", "false"),
add_launch_arg(
"voxel_grid_param_path",
[
diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml
index 2833fed81c28e..6ae70b2612836 100644
--- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml
+++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml
@@ -4,7 +4,6 @@
-
@@ -15,7 +14,6 @@
-
diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt
index c5f15ec4e49d5..b14d3a181e587 100644
--- a/sensing/pointcloud_preprocessor/CMakeLists.txt
+++ b/sensing/pointcloud_preprocessor/CMakeLists.txt
@@ -41,21 +41,6 @@ ament_target_dependencies(pointcloud_preprocessor_filter_base
pcl_ros
)
-add_library(faster_voxel_grid_downsample_filter SHARED
- src/downsample_filter/faster_voxel_grid_downsample_filter.cpp
-)
-
-target_include_directories(faster_voxel_grid_downsample_filter PUBLIC
- "$"
- "$"
-)
-
-ament_target_dependencies(faster_voxel_grid_downsample_filter
- pcl_conversions
- rclcpp
- sensor_msgs
-)
-
ament_auto_add_library(pointcloud_preprocessor_filter SHARED
src/utility/utilities.cpp
src/concatenate_data/concatenate_and_time_sync_nodelet.cpp
@@ -82,7 +67,6 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED
target_link_libraries(pointcloud_preprocessor_filter
pointcloud_preprocessor_filter_base
- faster_voxel_grid_downsample_filter
${Boost_LIBRARIES}
${OpenCV_LIBRARIES}
${PCL_LIBRARIES}
@@ -195,13 +179,6 @@ install(
RUNTIME DESTINATION bin
)
-install(
- TARGETS faster_voxel_grid_downsample_filter EXPORT export_${PROJECT_NAME}
- ARCHIVE DESTINATION lib
- LIBRARY DESTINATION lib
- RUNTIME DESTINATION bin
-)
-
install(
DIRECTORY include/
DESTINATION include/${PROJECT_NAME}
diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp
index 1406679ce0dba..3b392401de49b 100644
--- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp
+++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp
@@ -54,7 +54,6 @@
#define POINTCLOUD_PREPROCESSOR__CROP_BOX_FILTER__CROP_BOX_FILTER_NODELET_HPP_
#include "pointcloud_preprocessor/filter.hpp"
-#include "pointcloud_preprocessor/transform_info.hpp"
#include
diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp
deleted file mode 100644
index d92b511d40c69..0000000000000
--- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp
+++ /dev/null
@@ -1,93 +0,0 @@
-// Copyright 2023 TIER IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#pragma once
-
-#include "pointcloud_preprocessor/transform_info.hpp"
-
-#include
-#include
-#include
-
-#include
-#include
-
-namespace pointcloud_preprocessor
-{
-
-class FasterVoxelGridDownsampleFilter
-{
- using PointCloud2 = sensor_msgs::msg::PointCloud2;
- using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr;
-
-public:
- FasterVoxelGridDownsampleFilter();
- void set_voxel_size(float voxel_size_x, float voxel_size_y, float voxel_size_z);
- void set_field_offsets(const PointCloud2ConstPtr & input);
- void filter(
- const PointCloud2ConstPtr & input, PointCloud2 & output, const TransformInfo & transform_info,
- const rclcpp::Logger & logger);
-
-private:
- struct Centroid
- {
- float x;
- float y;
- float z;
- uint32_t point_count_;
-
- Centroid() : x(0), y(0), z(0) {}
- Centroid(float _x, float _y, float _z) : x(_x), y(_y), z(_z) { this->point_count_ = 1; }
-
- void add_point(float _x, float _y, float _z)
- {
- this->x += _x;
- this->y += _y;
- this->z += _z;
- this->point_count_++;
- }
-
- Eigen::Vector4f calc_centroid() const
- {
- Eigen::Vector4f centroid(
- (this->x / this->point_count_), (this->y / this->point_count_),
- (this->z / this->point_count_), 1.0f);
- return centroid;
- }
- };
-
- Eigen::Vector3f inverse_voxel_size_;
- std::vector xyz_fields_;
- int x_offset_;
- int y_offset_;
- int z_offset_;
- int intensity_offset_;
- bool offset_initialized_;
-
- Eigen::Vector3f get_point_from_global_offset(
- const PointCloud2ConstPtr & input, size_t global_offset);
-
- bool get_min_max_voxel(
- const PointCloud2ConstPtr & input, Eigen::Vector3i & min_voxel, Eigen::Vector3i & max_voxel);
-
- std::unordered_map calc_centroids_each_voxel(
- const PointCloud2ConstPtr & input, const Eigen::Vector3i & max_voxel,
- const Eigen::Vector3i & min_voxel);
-
- void copy_centroids_to_output(
- std::unordered_map & voxel_centroid_map, PointCloud2 & output,
- const TransformInfo & transform_info);
-};
-
-} // namespace pointcloud_preprocessor
diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp
index ae285b4fa9239..7ebf80c2da958 100644
--- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp
+++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp
@@ -1,4 +1,4 @@
-// Copyright 2023 TIER IV, Inc.
+// Copyright 2020 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -52,7 +52,6 @@
#define POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_
#include "pointcloud_preprocessor/filter.hpp"
-#include "pointcloud_preprocessor/transform_info.hpp"
#include
#include
@@ -67,16 +66,10 @@ class VoxelGridDownsampleFilterComponent : public pointcloud_preprocessor::Filte
void filter(
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override;
- // TODO(atsushi421): Temporary Implementation: Remove this interface when all the filter nodes
- // conform to new API
- virtual void faster_filter(
- const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output,
- const TransformInfo & transform_info);
-
private:
- float voxel_size_x_;
- float voxel_size_y_;
- float voxel_size_z_;
+ double voxel_size_x_;
+ double voxel_size_y_;
+ double voxel_size_z_;
/** \brief Parameter service callback result : needed to be hold */
OnSetParametersCallbackHandle::SharedPtr set_param_res_;
diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp
index 47c0f2b403faa..f995be741272c 100644
--- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp
+++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp
@@ -52,8 +52,6 @@
#ifndef POINTCLOUD_PREPROCESSOR__FILTER_HPP_
#define POINTCLOUD_PREPROCESSOR__FILTER_HPP_
-#include "pointcloud_preprocessor/transform_info.hpp"
-
#include
#include
#include
@@ -136,6 +134,18 @@ class Filter : public rclcpp::Node
const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
protected:
+ struct TransformInfo
+ {
+ TransformInfo()
+ {
+ eigen_transform = Eigen::Matrix4f::Identity(4, 4);
+ need_transform = false;
+ }
+
+ Eigen::Matrix4f eigen_transform;
+ bool need_transform;
+ };
+
/** \brief The input PointCloud2 subscriber. */
rclcpp::Subscription::SharedPtr sub_input_;
diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp
index ab7d4e0012304..a906eb913d863 100644
--- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp
+++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp
@@ -17,7 +17,6 @@
#include "autoware_point_types/types.hpp"
#include "pointcloud_preprocessor/filter.hpp"
-#include "pointcloud_preprocessor/transform_info.hpp"
#include
diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp
deleted file mode 100644
index a4806555695b5..0000000000000
--- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp
+++ /dev/null
@@ -1,43 +0,0 @@
-// Copyright 2023 TIER IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#pragma once
-
-#include
-
-namespace pointcloud_preprocessor
-{
-
-/**
- * This holds the coordinate transformation information of the point cloud.
- * Usage example:
- * \code
- * if (transform_info.need_transform) {
- * point = transform_info.eigen_transform * point;
- * }
- * \endcode
- */
-struct TransformInfo
-{
- TransformInfo()
- {
- eigen_transform = Eigen::Matrix4f::Identity(4, 4);
- need_transform = false;
- }
-
- Eigen::Matrix4f eigen_transform;
- bool need_transform;
-};
-
-} // namespace pointcloud_preprocessor
diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp
deleted file mode 100644
index 7c302eec20431..0000000000000
--- a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp
+++ /dev/null
@@ -1,178 +0,0 @@
-// Copyright 2023 TIER IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include "pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp"
-
-namespace pointcloud_preprocessor
-{
-
-FasterVoxelGridDownsampleFilter::FasterVoxelGridDownsampleFilter()
-{
- pcl::for_each_type::type>(
- pcl::detail::FieldAdder(xyz_fields_));
- offset_initialized_ = false;
-}
-
-void FasterVoxelGridDownsampleFilter::set_voxel_size(
- float voxel_size_x, float voxel_size_y, float voxel_size_z)
-{
- inverse_voxel_size_ =
- Eigen::Array3f::Ones() / Eigen::Array3f(voxel_size_x, voxel_size_y, voxel_size_z);
-}
-
-void FasterVoxelGridDownsampleFilter::set_field_offsets(const PointCloud2ConstPtr & input)
-{
- x_offset_ = input->fields[pcl::getFieldIndex(*input, "x")].offset;
- y_offset_ = input->fields[pcl::getFieldIndex(*input, "y")].offset;
- z_offset_ = input->fields[pcl::getFieldIndex(*input, "z")].offset;
- intensity_offset_ = input->fields[pcl::getFieldIndex(*input, "intensity")].offset;
- offset_initialized_ = true;
-}
-
-void FasterVoxelGridDownsampleFilter::filter(
- const PointCloud2ConstPtr & input, PointCloud2 & output, const TransformInfo & transform_info,
- const rclcpp::Logger & logger)
-{
- // Check if the field offset has been set
- if (!offset_initialized_) {
- set_field_offsets(input);
- }
-
- // Compute the minimum and maximum voxel coordinates
- Eigen::Vector3i min_voxel, max_voxel;
- if (!get_min_max_voxel(input, min_voxel, max_voxel)) {
- RCLCPP_ERROR(
- logger,
- "Voxel size is too small for the input dataset. "
- "Integer indices would overflow.");
- output = *input;
- return;
- }
-
- // Storage for mapping voxel coordinates to centroids
- auto voxel_centroid_map = calc_centroids_each_voxel(input, max_voxel, min_voxel);
-
- // Initialize the output
- output.row_step = voxel_centroid_map.size() * input->point_step;
- output.data.resize(output.row_step);
- output.width = voxel_centroid_map.size();
- pcl_conversions::fromPCL(xyz_fields_, output.fields);
- output.is_dense = true; // we filter out invalid points
- output.height = input->height;
- output.is_bigendian = input->is_bigendian;
- output.point_step = input->point_step;
- output.header = input->header;
-
- // Copy the centroids to the output
- copy_centroids_to_output(voxel_centroid_map, output, transform_info);
-}
-
-Eigen::Vector3f FasterVoxelGridDownsampleFilter::get_point_from_global_offset(
- const PointCloud2ConstPtr & input, size_t global_offset)
-{
- Eigen::Vector3f point(
- *reinterpret_cast(&input->data[global_offset + x_offset_]),
- *reinterpret_cast(&input->data[global_offset + y_offset_]),
- *reinterpret_cast(&input->data[global_offset + z_offset_]));
- return point;
-}
-
-bool FasterVoxelGridDownsampleFilter::get_min_max_voxel(
- const PointCloud2ConstPtr & input, Eigen::Vector3i & min_voxel, Eigen::Vector3i & max_voxel)
-{
- // Compute the minimum and maximum point coordinates
- Eigen::Vector3f min_point, max_point;
- min_point.setConstant(FLT_MAX);
- max_point.setConstant(-FLT_MAX);
- for (size_t global_offset = 0; global_offset + input->point_step <= input->data.size();
- global_offset += input->point_step) {
- Eigen::Vector3f point = get_point_from_global_offset(input, global_offset);
- if (std::isfinite(point[0]) && std::isfinite(point[1]), std::isfinite(point[2])) {
- min_point = min_point.cwiseMin(point);
- max_point = max_point.cwiseMax(point);
- }
- }
-
- // Check that the voxel size is not too small, given the size of the data
- if (
- ((static_cast((max_point[0] - min_point[0]) * inverse_voxel_size_[0]) + 1) *
- (static_cast((max_point[1] - min_point[1]) * inverse_voxel_size_[1]) + 1) *
- (static_cast((max_point[2] - min_point[2]) * inverse_voxel_size_[2]) + 1)) >
- static_cast(std::numeric_limits::max())) {
- return false;
- }
-
- // Compute the minimum and maximum voxel coordinates
- min_voxel[0] = static_cast(std::floor(min_point[0] * inverse_voxel_size_[0]));
- min_voxel[1] = static_cast(std::floor(min_point[1] * inverse_voxel_size_[1]));
- min_voxel[2] = static_cast(std::floor(min_point[2] * inverse_voxel_size_[2]));
- max_voxel[0] = static_cast(std::floor(max_point[0] * inverse_voxel_size_[0]));
- max_voxel[1] = static_cast(std::floor(max_point[1] * inverse_voxel_size_[1]));
- max_voxel[2] = static_cast(std::floor(max_point[2] * inverse_voxel_size_[2]));
-
- return true;
-}
-
-std::unordered_map
-FasterVoxelGridDownsampleFilter::calc_centroids_each_voxel(
- const PointCloud2ConstPtr & input, const Eigen::Vector3i & max_voxel,
- const Eigen::Vector3i & min_voxel)
-{
- std::unordered_map voxel_centroid_map;
- // Compute the number of divisions needed along all axis
- Eigen::Vector3i div_b = max_voxel - min_voxel + Eigen::Vector3i::Ones();
- // Set up the division multiplier
- Eigen::Vector3i div_b_mul(1, div_b[0], div_b[0] * div_b[1]);
-
- for (size_t global_offset = 0; global_offset + input->point_step <= input->data.size();
- global_offset += input->point_step) {
- Eigen::Vector3f point = get_point_from_global_offset(input, global_offset);
- if (std::isfinite(point[0]) && std::isfinite(point[1]), std::isfinite(point[2])) {
- // Calculate the voxel index to which the point belongs
- int ijk0 = static_cast(std::floor(point[0] * inverse_voxel_size_[0]) - min_voxel[0]);
- int ijk1 = static_cast(std::floor(point[1] * inverse_voxel_size_[1]) - min_voxel[1]);
- int ijk2 = static_cast(std::floor(point[2] * inverse_voxel_size_[2]) - min_voxel[2]);
- uint32_t voxel_id = ijk0 * div_b_mul[0] + ijk1 * div_b_mul[1] + ijk2 * div_b_mul[2];
-
- // Add the point to the corresponding centroid
- if (voxel_centroid_map.find(voxel_id) == voxel_centroid_map.end()) {
- voxel_centroid_map[voxel_id] = Centroid(point[0], point[1], point[2]);
- } else {
- voxel_centroid_map[voxel_id].add_point(point[0], point[1], point[2]);
- }
- }
- }
-
- return voxel_centroid_map;
-}
-
-void FasterVoxelGridDownsampleFilter::copy_centroids_to_output(
- std::unordered_map & voxel_centroid_map, PointCloud2 & output,
- const TransformInfo & transform_info)
-{
- size_t output_data_size = 0;
- for (const auto & pair : voxel_centroid_map) {
- Eigen::Vector4f centroid = pair.second.calc_centroid();
- if (transform_info.need_transform) {
- centroid = transform_info.eigen_transform * centroid;
- }
- *reinterpret_cast(&output.data[output_data_size + x_offset_]) = centroid[0];
- *reinterpret_cast(&output.data[output_data_size + y_offset_]) = centroid[1];
- *reinterpret_cast(&output.data[output_data_size + z_offset_]) = centroid[2];
- *reinterpret_cast(&output.data[output_data_size + intensity_offset_]) = centroid[3];
- output_data_size += output.point_step;
- }
-}
-
-} // namespace pointcloud_preprocessor
diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp
index f11f37397a142..5b22986a1b6b8 100644
--- a/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp
+++ b/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp
@@ -1,4 +1,4 @@
-// Copyright 2023 TIER IV, Inc.
+// Copyright 2020 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -50,8 +50,6 @@
#include "pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp"
-#include "pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp"
-
#include
#include
#include
@@ -66,9 +64,9 @@ VoxelGridDownsampleFilterComponent::VoxelGridDownsampleFilterComponent(
{
// set initial parameters
{
- voxel_size_x_ = static_cast(declare_parameter("voxel_size_x", 0.3));
- voxel_size_y_ = static_cast(declare_parameter("voxel_size_y", 0.3));
- voxel_size_z_ = static_cast(declare_parameter("voxel_size_z", 0.1));
+ voxel_size_x_ = static_cast(declare_parameter("voxel_size_x", 0.3));
+ voxel_size_y_ = static_cast(declare_parameter("voxel_size_y", 0.3));
+ voxel_size_z_ = static_cast(declare_parameter("voxel_size_z", 0.1));
}
using std::placeholders::_1;
@@ -76,8 +74,6 @@ VoxelGridDownsampleFilterComponent::VoxelGridDownsampleFilterComponent(
std::bind(&VoxelGridDownsampleFilterComponent::paramCallback, this, _1));
}
-// TODO(atsushi421): Temporary Implementation: Delete this function definition when all the filter
-// nodes conform to new API.
void VoxelGridDownsampleFilterComponent::filter(
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output)
{
@@ -99,19 +95,6 @@ void VoxelGridDownsampleFilterComponent::filter(
output.header = input->header;
}
-// TODO(atsushi421): Temporary Implementation: Rename this function to `filter()` when all the
-// filter nodes conform to new API. Then delete the old `filter()` defined above.
-void VoxelGridDownsampleFilterComponent::faster_filter(
- const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
- PointCloud2 & output, const TransformInfo & transform_info)
-{
- std::scoped_lock lock(mutex_);
- FasterVoxelGridDownsampleFilter faster_voxel_filter;
- faster_voxel_filter.set_voxel_size(voxel_size_x_, voxel_size_y_, voxel_size_z_);
- faster_voxel_filter.set_field_offsets(input);
- faster_voxel_filter.filter(input, output, transform_info, this->get_logger());
-}
-
rcl_interfaces::msg::SetParametersResult VoxelGridDownsampleFilterComponent::paramCallback(
const std::vector & p)
{
diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp
index d5843be395adf..f0b716c73b5f1 100644
--- a/sensing/pointcloud_preprocessor/src/filter.cpp
+++ b/sensing/pointcloud_preprocessor/src/filter.cpp
@@ -124,8 +124,7 @@ void pointcloud_preprocessor::Filter::subscribe(const std::string & filter_name)
// TODO(sykwer): Change the corresponding node to subscribe to `faster_input_indices_callback`
// each time a child class supports the faster version.
// When all the child classes support the faster version, this workaround is deleted.
- std::set supported_nodes = {
- "CropBoxFilter", "RingOutlierFilter", "VoxelGridDownsampleFilter"};
+ std::set supported_nodes = {"CropBoxFilter", "RingOutlierFilter"};
auto callback = supported_nodes.find(filter_name) != supported_nodes.end()
? &Filter::faster_input_indices_callback
: &Filter::input_indices_callback;