Work putting one object over the other

dora_yolo_object_detector/dora_yolo_object_detector/main.py

@@ -185,6 +185,60 @@ def _sample_point(
     return np.median(np.stack(samples, axis=0), axis=0)
 
 
+def _estimate_object_height(
+    point_cloud: np.ndarray, bbox: List[int], cfg: DetectionConfig, sample_step: int = 4
+) -> Optional[float]:
+    """Estimate object height by sampling Z values within the bounding box.
+
+    Samples points in a grid within the bbox, finds the Z range (max - min),
+    which corresponds to the object height.
+
+    Args:
+        point_cloud: The point cloud array (H, W, channels) with XYZ in mm.
+        bbox: Bounding box [x1, y1, x2, y2] in pixels.
+        cfg: Detection config for depth validation.
+        sample_step: Step size for grid sampling (smaller = more samples).
+
+    Returns:
+        Estimated height in mm, or None if not enough valid points.
+    """
+    x1, y1, x2, y2 = bbox
+    h, w, _ = point_cloud.shape
+
+    # Clamp bbox to image bounds
+    x1 = max(0, x1)
+    y1 = max(0, y1)
+    x2 = min(w, x2)
+    y2 = min(h, y2)
+
+    if x2 <= x1 or y2 <= y1:
+        return None
+
+    # Sample points in a grid within the bounding box
+    z_values = []
+    for y in range(y1, y2, sample_step):
+        for x in range(x1, x2, sample_step):
+            point_xyz = point_cloud[y, x, :3].astype(np.float64)
+            if _valid_point(point_xyz, cfg):
+                z_values.append(point_xyz[2])
+
+    if len(z_values) < 5:
+        return None
+
+    # Use percentiles to filter outliers (table surface, noise)
+    z_array = np.array(z_values)
+    z_min = np.percentile(z_array, 10)  # Top of object (closer to camera = smaller Z)
+    z_max = np.percentile(z_array, 90)  # Bottom/table level (farther = larger Z)
+
+    height = z_max - z_min
+
+    # Sanity check: height should be positive and reasonable (5mm to 200mm)
+    if height < 5.0 or height > 200.0:
+        return None
+
+    return float(height)
+
+
 def _dominant_color(image: np.ndarray, bbox: List[int]) -> Tuple[int, int, int]:
     x1, y1, x2, y2 = bbox
     x1 = max(0, x1)
@@ -482,22 +536,27 @@ def main() -> None:
             area = max(1, (bbox[2] - bbox[0]) * (bbox[3] - bbox[1]))
             size_label = "big" if area >= cfg.size_threshold else "small"
 
-            objects.append(
-                {
-                    "object_type": results.names[int(r.cls.item())],
-                    "confidence": float(r.conf.item()),
-                    "color": color_name,
-                    "size": size_label,
-                    "bbox": bbox,
-                    "center_px": [cx, cy],
-                    "position_mm": [
-                        float(point_base_mm[0]),
-                        float(point_base_mm[1]),
-                        float(point_base_mm[2]),
-                    ],
-                    "timestamp_ns": time.time_ns(),
-                }
-            )
+            # Estimate object height from point cloud
+            height_mm = _estimate_object_height(latest_point_cloud, bbox, cfg)
+
+            obj_data = {
+                "object_type": results.names[int(r.cls.item())],
+                "confidence": float(r.conf.item()),
+                "color": color_name,
+                "size": size_label,
+                "bbox": bbox,
+                "center_px": [cx, cy],
+                "position_mm": [
+                    float(point_base_mm[0]),
+                    float(point_base_mm[1]),
+                    float(point_base_mm[2]),
+                ],
+                "timestamp_ns": time.time_ns(),
+            }
+            if height_mm is not None:
+                obj_data["height_mm"] = height_mm
+
+            objects.append(obj_data)
 
         payload = json.dumps({"objects": objects, "timestamp_ns": time.time_ns()})
         node.send_output(