From e884c4ccdc4c30decaed70504bedd57997867b74 Mon Sep 17 00:00:00 2001 From: Hriday Bavle Date: Fri, 19 Apr 2024 17:11:53 +0200 Subject: [PATCH] [Fix] Bug in horizontal plane mapper --- s_graphs/src/s_graphs/backend/plane_mapper.cpp | 2 +- s_graphs/src/s_graphs/frontend/plane_analyzer.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/s_graphs/src/s_graphs/backend/plane_mapper.cpp b/s_graphs/src/s_graphs/backend/plane_mapper.cpp index 75178106..6035d736 100644 --- a/s_graphs/src/s_graphs/backend/plane_mapper.cpp +++ b/s_graphs/src/s_graphs/backend/plane_mapper.cpp @@ -503,7 +503,7 @@ int PlaneMapper::associate_plane( maha_dist = sqrt(error.transpose() * cov * error); } if (maha_dist < hort_min_maha_dist) { - vert_min_maha_dist = maha_dist; + hort_min_maha_dist = maha_dist; data_association = hort_plane.second.id; } } diff --git a/s_graphs/src/s_graphs/frontend/plane_analyzer.cpp b/s_graphs/src/s_graphs/frontend/plane_analyzer.cpp index 46bbaa7b..b87a44f8 100644 --- a/s_graphs/src/s_graphs/frontend/plane_analyzer.cpp +++ b/s_graphs/src/s_graphs/frontend/plane_analyzer.cpp @@ -143,7 +143,8 @@ std::vector::Ptr> PlaneAnalyzer::extract_segmented_ // segmented_cloud->back().b = 177; } - extracted_cloud_vec.push_back(extracted_cloud_filtered); + if (extracted_cloud_filtered->points.size() > min_seg_points_) + extracted_cloud_vec.push_back(extracted_cloud_filtered); // sensor_msgs::PointCloud2 extracted_cloud_msg; // pcl::toROSMsg(*extracted_cloud_filtered, extracted_cloud_msg);