添加状态分类和液面分割

angle_base_seg/bushu.py

@@ -0,0 +1,206 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+import os
+import cv2
+import numpy as np
+import argparse
+import torch
+import torch.nn.functional as F
+import torchvision
+
+# ---------------- 配置 ----------------
+OBJ_THRESH = 0.25
+NMS_THRESH = 0.45
+MAX_DETECT = 300
+IMG_SIZE = (640, 640)  # (W,H)
+
+OUTPUT_DIR = "result"
+os.makedirs(OUTPUT_DIR, exist_ok=True)
+
+
+# ---------------- 工具函数 ----------------
+def sigmoid(x):
+    return 1 / (1 + np.exp(-x))
+
+def dfl(position):
+    x = torch.tensor(position)
+    n, c, h, w = x.shape
+    y = x.reshape(n, 4, c // 4, h, w)
+    y = y.softmax(2)
+    acc_metrix = torch.arange(c // 4).float().reshape(1, 1, c // 4, 1, 1)
+    y = (y * acc_metrix).sum(2)
+    return y.numpy()
+
+def box_process(position):
+    grid_h, grid_w = position.shape[2:4]
+    col, row = np.meshgrid(np.arange(0, grid_w), np.arange(0, grid_h))
+    col, row = col.reshape(1, 1, grid_h, grid_w), row.reshape(1, 1, grid_h, grid_w)
+    grid = np.concatenate((col, row), axis=1)
+    stride = np.array([IMG_SIZE[1] // grid_h, IMG_SIZE[0] // grid_w]).reshape(1, 2, 1, 1)
+
+    position = dfl(position)
+    box_xy = grid + 0.5 - position[:, 0:2, :, :]
+    box_xy2 = grid + 0.5 + position[:, 2:4, :, :]
+    xyxy = np.concatenate((box_xy * stride, box_xy2 * stride), axis=1)
+    return xyxy
+
+def _crop_mask(masks, boxes):
+    n, h, w = masks.shape
+    x1, y1, x2, y2 = torch.chunk(boxes[:, :, None], 4, 1)
+    r = torch.arange(w, device=masks.device, dtype=x1.dtype)[None, None, :]
+    c = torch.arange(h, device=masks.device, dtype=x1.dtype)[None, :, None]
+    return masks * ((r >= x1) * (r < x2) * (c >= y1) * (c < y2))
+
+def post_process(input_data):
+    proto = input_data[-1]
+    boxes, scores, seg_part = [], [], []
+    default_branch = 3
+    pair_per_branch = len(input_data) // default_branch
+
+    for i in range(default_branch):
+        boxes.append(box_process(input_data[pair_per_branch * i]))
+        scores.append(np.ones_like(input_data[pair_per_branch * i + 1][:, :1, :, :], dtype=np.float32))
+        seg_part.append(input_data[pair_per_branch * i + 3])
+
+    def sp_flatten(_in):
+        ch = _in.shape[1]
+        _in = _in.transpose(0, 2, 3, 1)
+        return _in.reshape(-1, ch)
+
+    boxes = np.concatenate([sp_flatten(v) for v in boxes])
+    scores = np.concatenate([sp_flatten(v) for v in scores])
+    seg_part = np.concatenate([sp_flatten(v) for v in seg_part])
+
+    # 阈值过滤
+    keep = np.where(scores.reshape(-1) >= OBJ_THRESH)
+    boxes, scores, seg_part = boxes[keep], scores[keep], seg_part[keep]
+
+    # NMS
+    ids = torchvision.ops.nms(torch.tensor(boxes, dtype=torch.float32),
+                              torch.tensor(scores, dtype=torch.float32), NMS_THRESH)
+    ids = ids.tolist()[:MAX_DETECT]
+    boxes, scores, seg_part = boxes[ids], scores[ids], seg_part[ids]
+
+    # mask decode
+    ph, pw = proto.shape[-2:]
+    proto = proto.reshape(seg_part.shape[-1], -1)
+    seg_img = np.matmul(seg_part, proto)
+    seg_img = sigmoid(seg_img)
+    seg_img = seg_img.reshape(-1, ph, pw)
+    seg_img = F.interpolate(torch.tensor(seg_img)[None], torch.Size([640, 640]), mode='bilinear', align_corners=False)[0]
+    seg_img_t = _crop_mask(seg_img, torch.tensor(boxes))
+    seg_img = seg_img_t.numpy() > 0.5
+
+    return boxes, scores, seg_img
+
+
+# ---------------- 角度计算 ----------------
+def compute_angle(boxes, seg_img, h, w, filename, mode="show"):
+    composite_mask = np.zeros((h, w), dtype=np.uint8)
+    jaws = []
+
+    for i, box in enumerate(boxes):
+        x1, y1, x2, y2 = map(int, box)
+        x1, y1 = max(0, x1), max(0, y1)
+        x2, y2 = min(w, x2), min(h, y2)
+
+        obj_mask = np.zeros((h, w), dtype=np.uint8)
+        mask_resized = cv2.resize(seg_img[i].astype(np.uint8), (w, h))
+        obj_mask[y1:y2, x1:x2] = mask_resized[y1:y2, x1:x2] * 255
+
+        contours, _ = cv2.findContours(obj_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
+        if len(contours) == 0:
+            continue
+
+        largest_contour = max(contours, key=cv2.contourArea)
+        area = cv2.contourArea(largest_contour)
+        if area < 100:
+            continue
+
+        rect = cv2.minAreaRect(largest_contour)
+        jaws.append({'contour': largest_contour, 'rect': rect, 'area': area})
+        composite_mask = np.maximum(composite_mask, obj_mask)
+
+    if len(jaws) < 2:
+        print(f"❌ 检测到的夹具少于2个（共{len(jaws)}个）")
+        return None
+
+    jaws.sort(key=lambda x: x['area'], reverse=True)
+    jaw1, jaw2 = jaws[0], jaws[1]
+
+    def get_long_edge_vector(rect):
+        center, (w_, h_), angle = rect
+        rad = np.radians(angle + (0 if w_ >= h_ else 90))
+        return np.array([np.cos(rad), np.sin(rad)])
+
+    def get_center(contour):
+        M = cv2.moments(contour)
+        return np.array([M['m10']/M['m00'], M['m01']/M['m00']]) if M['m00'] != 0 else np.array([0, 0])
+
+    dir1, dir2 = get_long_edge_vector(jaw1['rect']), get_long_edge_vector(jaw2['rect'])
+    center1, center2 = get_center(jaw1['contour']), get_center(jaw2['contour'])
+    fixture_center = (center1 + center2) / 2
+
+    to_center1, to_center2 = fixture_center - center1, fixture_center - center2
+    if np.linalg.norm(to_center1) > 1e-6 and np.dot(dir1, to_center1) < 0:
+        dir1 = -dir1
+    if np.linalg.norm(to_center2) > 1e-6 and np.dot(dir2, to_center2) < 0:
+        dir2 = -dir2
+
+    cos_angle = np.clip(np.dot(dir1, dir2), -1.0, 1.0)
+    angle = np.degrees(np.arccos(cos_angle))
+    opening_angle = min(angle, 180 - angle)
+
+    if mode == "show":
+        vis_img = np.stack([composite_mask]*3, axis=-1)
+        vis_img[composite_mask > 0] = [255, 255, 255]
+
+        box1, box2 = np.int32(cv2.boxPoints(jaw1['rect'])), np.int32(cv2.boxPoints(jaw2['rect']))
+        cv2.drawContours(vis_img, [box1], 0, (0, 0, 255), 2)
+        cv2.drawContours(vis_img, [box2], 0, (255, 0, 0), 2)
+
+        scale = 60
+        c1, c2 = tuple(np.int32(center1)), tuple(np.int32(center2))
+        end1, end2 = tuple(np.int32(center1 + scale * dir1)), tuple(np.int32(center2 + scale * dir2))
+        cv2.arrowedLine(vis_img, c1, end1, (0, 255, 0), 2, tipLength=0.3)
+        cv2.arrowedLine(vis_img, c2, end2, (0, 255, 0), 2, tipLength=0.3)
+
+        cv2.putText(vis_img, f"Angle: {opening_angle:.2f}°", (20, 50),
+                    cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
+
+        save_path = os.path.join(OUTPUT_DIR, f'angle_{filename}')
+        cv2.imwrite(save_path, vis_img)
+        print(f"✅ 结果已保存: {save_path}")
+
+    return round(opening_angle, 2)
+
+
+# ---------------- 主程序 ----------------
+def main():
+    # 固定路径（写死）
+    MODEL_PATH = "/userdata/bushu/seg.rknn"
+    IMG_PATH   = "/userdata/bushu/test.jpg"
+
+    from py_utils.rknn_executor import RKNN_model_container
+    model = RKNN_model_container(MODEL_PATH, target='rk3588', device_id=None)
+
+    img_src = cv2.imread(IMG_PATH)
+    if img_src is None:
+        print("❌ 图片路径错误:", IMG_PATH)
+        return
+    h, w = img_src.shape[:2]
+    img = cv2.resize(img_src, IMG_SIZE)
+
+    outputs = model.run([img])
+    boxes, scores, seg_img = post_process(outputs)
+
+    filename = os.path.basename(IMG_PATH)
+    angle = compute_angle(boxes, seg_img, h, w, filename, mode="show")
+    if angle is not None:
+        print(f"🎉 检测到的夹具开合角度: {angle}°")
+
+    model.release()
+
+
+if __name__ == "__main__":
+    main()