[Fix] Bug in horizontal plane mapper

snt-arg · Apr 19, 2024 · e884c4c · e884c4c
1 parent e49172b
commit e884c4c
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 2 deletions.
diff --git 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
@@ -143,7 +143,8 @@ std::vector<pcl::PointCloud<PointNormal>::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);