HiveCoreRD/hivecore_robot_os
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add iqr

Browse Source
This commit is contained in:
liangyuxuan
2025-08-08 11:13:27 +08:00
parent 1f8384ceb0
commit 7238e23159
1 changed files with 19 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
HiveCoreR0/src/robot_vision/src/detect_part/detect_part/detect.py
View File

@@ -187,6 +187,8 @@ class DetectNode(Node):
if len(valid_depths) == 0:
self.get_logger().warning(f'No valid depth in window at ({u}, {v})')
return 0.0, 0.0, 0.0
valid_depths = self.iqr(valid_depths)
# x = depth_img[v, u] / 1e3
depth = np.median(valid_depths) / 1e3
@@ -212,6 +214,23 @@ class DetectNode(Node):
int(max(0, u + w - half)):int(min(u + w + half + 1, depth_img.shape[1])):2].flatten())
return np.concatenate(patch)
def iqr(self, depths, threshold: float = 1.5):
if len(depths) < 7:
return depths
q1 = np.percentile(depths, 25)
q3 = np.percentile(depths, 75)
iqr = q3 - q1
lower_bound = q1 - iqr * threshold
upper_bound = q3 + iqr * threshold
iqr_depths = depths[(depths >= lower_bound) & (depths <= upper_bound)]
return iqr_depths
def main(args=None):