fix

Signed-off-by: Takahiro Ishikawa <sykwer@gmail.com>
tier4 · Apr 11, 2023 · 82262b9 · 82262b9
1 parent 7213591
commit 82262b9
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 15 deletions.
diff --git a/...eprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/...eprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp
@@ -54,25 +54,16 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
   rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector<rclcpp::Parameter> & p);
 
   bool isCluster(
-    const PointCloud2ConstPtr & input, std::pair<int, int> data_idx_both_ends, int walk_size,
-    const TransformInfo & transform_info)
+    const PointCloud2ConstPtr & input, std::pair<int, int> data_idx_both_ends, int walk_size)
   {
     if (walk_size > num_points_threshold_) return true;
 
     auto first_point = reinterpret_cast<const PointXYZI *>(&input->data[data_idx_both_ends.first]);
     auto last_point = reinterpret_cast<const PointXYZI *>(&input->data[data_idx_both_ends.second]);
 
-    Eigen::Vector4f p1(first_point->x, first_point->y, first_point->z, 1);
-    Eigen::Vector4f p2(last_point->x, last_point->y, last_point->z, 1);
-
-    if (transform_info.need_transform) {
-      p1 = transform_info.eigen_transform * p1;
-      p2 = transform_info.eigen_transform * p2;
-    }
-
-    auto x = p1[0] - p2[0];
-    auto y = p1[1] - p2[1];
-    auto z = p1[2] - p2[2];
+    const auto x = first_point->x - last_point->x;
+    const auto y = first_point->y - last_point->y;
+    const auto z = first_point->z - last_point->z;
 
     return x * x + y * y + z * z >= object_length_threshold_ * object_length_threshold_;
   }

diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp
@@ -118,7 +118,7 @@ void RingOutlierFilterComponent::faster_filter(
 
       if (isCluster(
             input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]),
-            walk_last_idx - walk_first_idx + 1, transform_info)) {
+            walk_last_idx - walk_first_idx + 1)) {
         for (int i = walk_first_idx; i <= walk_last_idx; i++) {
           auto output_ptr = reinterpret_cast<PointXYZI *>(&output.data[output_size]);
           auto input_ptr = reinterpret_cast<const PointXYZI *>(&input->data[indices[i]]);
@@ -145,7 +145,7 @@ void RingOutlierFilterComponent::faster_filter(
 
     if (isCluster(
           input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]),
-          walk_last_idx - walk_first_idx + 1, transform_info)) {
+          walk_last_idx - walk_first_idx + 1)) {
       for (int i = walk_first_idx; i <= walk_last_idx; i++) {
         auto output_ptr = reinterpret_cast<PointXYZI *>(&output.data[output_size]);
         auto input_ptr = reinterpret_cast<const PointXYZI *>(&input->data[indices[i]]);