diff --git a/avstack/modules/perception/fov_estimator.py b/avstack/modules/perception/fov_estimator.py index 6450f73..d8875d3 100644 --- a/avstack/modules/perception/fov_estimator.py +++ b/avstack/modules/perception/fov_estimator.py @@ -12,6 +12,8 @@ from avstack.geometry import GlobalOrigin3D, Polygon from avstack.modules import BaseModule from avstack.utils.decorators import apply_hooks +from collections import defaultdict + class _LidarFovEstimator(BaseModule): @@ -97,6 +99,8 @@ def __call__( z_min=self.z_min, z_max=self.z_max, ) + + self._eliminate_isolated_pts(pc_bev, 10, 30) # center the lidar data if centering: @@ -152,6 +156,24 @@ def _estimate_fov_from_polar_lidar( ) -> "Polygon": """To be implemented in subclass""" raise NotImplementedError + + def _eliminate_isolated_pts(self, pc_bev, m_away, num_pts): + ptMap = defaultdict(int) + usable_pts = [] + for p1 in pc_bev.data.x: + p1x, p1y = p1[0], p1[1] + for p2 in pc_bev.data.x: + p2x, p2y = p2[0], p2[1] + if p1x == p2x and p1y == p2y: + continue + dis = np.linalg.norm([p1x - p2x, p1y - p2y]) + if (dis < m_away): + ptMap[(p1x, p1y)] += 1 + if (ptMap[(p1x, p1y)] == num_pts): + usable_pts.append([p1x, p1y]) + break + usable_pts = np.array(usable_pts) + pc_bev.data.x = usable_pts @MODELS.register_module()