dasha_f
0d32f32db0
Замкнутый контур "поток -> CV -> механика": товары идут по конвейеру с шагом 700 мм, класс определяется стереопайплайном во время движения, пушер и плуг реагируют физически. Состав: * control_test/ - ячейка и CV. run_sorting_cv.py + cv_worker.py (два процесса, потому что torch внутри Isaac роняет сцену), cell.py (физика лент, плуга, пушера), measure_plane.py (замер габаритов), README.md и .memory.md с замерами, проблемами и ловушками * robozon_sorter/ - модули симуляции, scripts/ - утилиты, scene/ - сцены * assets/ - меши товаров, плуг, объекты Objaverse Бейзлайн CV: DEFOM-Stereo vitl, вход 480, iters 24, кроп зоны осмотра, без сегментации. На потоке 700 мм - классы 8/9, габариты MAE 32.8 мм, 469 мс на товар при такте 700 мс. Веса моделей (4.5 ГБ) и пропсы конвейера NVIDIA (274 МБ) не включены - источники и команды скачивания в MODELS.md. Выход прогонов (captures/, runtime/) не включён: воспроизводится. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
562 lines
32 KiB
Python
562 lines
32 KiB
Python
"""ВАРИАНТ БЕЗ СЕГМЕНТАЦИИ: CRE-Stereo по всему ROI, товар отделяется от полотна по высоте.
|
||
|
||
Зачем. Разбор показал, что и раздутые габариты, и непойманный класс D идут от сегментации:
|
||
у bag, bucket и detergent FastSAM выделял ПОЛОТНО, а не товар (маска 174-267 тыс. px против
|
||
17-143 тыс. у нормальных, выход точек падал с 96 % до 0.5-6.6 %). На кадре видно, что ведро
|
||
не выделено ни одной маской - залито всё полотно.
|
||
|
||
Здесь сегментации нет вовсе. CRE считается по общей зоне ленты, диспаратность переводится
|
||
в 3D, и товаром считается то, что ВЫСТУПАЕТ над плоскостью полотна выше порога. Плоскость
|
||
известна из калибровки (z = BELT_Z), поэтому её не надо ни искать, ни подгонять.
|
||
|
||
Это тот же приём, что дал рабочий результат на физическом стенде: там отказ от сегментации
|
||
снял две ошибки сразу - выбор маски и проекцию силуэта.
|
||
|
||
Порог над полотном 12 мм: измеренный разброс самого полотна в облаке меньше, а товары
|
||
здесь от 122 мм высотой. Дополнительно точки чистятся по связности в плане, чтобы соседний
|
||
товар потока (при шаге 700 мм он попадает в кадр) не приклеился к целевому.
|
||
"""
|
||
import os, sys, json, time
|
||
import numpy as np
|
||
import cv2
|
||
import torch
|
||
|
||
CV = "/home/dasha/isaac_assets/cv"
|
||
CT = "/home/dasha/robozon-sorter/control_test"
|
||
CAP = f"{CT}/captures/flow"
|
||
OUT = f"{CT}/diag/plane"
|
||
os.makedirs(OUT, exist_ok=True)
|
||
sys.path.insert(0, CT)
|
||
os.environ.setdefault("MASK_MODE", "gate")
|
||
import measure_flow as MF # берём cre_batch, backproj-математику, POLY3
|
||
import classify as CL
|
||
|
||
man = MF.man; calib = MF.calib
|
||
TARGET, BELT_Z, RIGS = MF.TARGET, MF.BELT_Z, MF.RIGS
|
||
FLOOR = 0.020 # порог над полотном, м
|
||
DENS_CELL = 0.010 # ячейка сетки плотности, м
|
||
DENS_MIN = 12 # точек в ячейке, чтобы считать её товаром
|
||
GATE_R = 0.28 # радиус вокруг точки осмотра, м - отсекает соседа по потоку
|
||
PITCH_S = 0.70
|
||
|
||
RIGCOL = {RIGS[0]: (0, 140, 255), RIGS[1]: (60, 230, 90), RIGS[2]: (240, 120, 240)}
|
||
|
||
|
||
def cloud_from_roi(disp, win, cam):
|
||
"""всё окно -> 3D, без маски. Товар выделяется превышением над полотном."""
|
||
x0, y0, x1, y1 = win
|
||
depth = np.where(disp > 0.5, cam["fx"] * cam["baseline"] / np.maximum(disp, 1e-6), np.nan)
|
||
vs, us = np.mgrid[y0:y0 + depth.shape[0], x0:x0 + depth.shape[1]]
|
||
ok = np.isfinite(depth) & (depth > 1e-3)
|
||
u, v, z = us[ok], vs[ok], depth[ok]
|
||
P = np.stack([(u - cam["cx"]) * z / cam["fx"],
|
||
-(v - cam["cy"]) * z / cam["fy"], -z, np.ones_like(z)], 1)
|
||
P = (P @ np.array(cam["M"]))[:, :3]
|
||
near = (np.hypot(P[:, 0] - TARGET[0], P[:, 1] - TARGET[1]) < GATE_R)
|
||
above = (P[:, 2] > BELT_Z + FLOOR) & (P[:, 2] < BELT_Z + 0.60)
|
||
return P[near & above], P[near & (P[:, 2] > BELT_Z - 0.03) & (P[:, 2] <= BELT_Z + FLOOR)]
|
||
|
||
|
||
def dense_only(P, cell=DENS_CELL, need=DENS_MIN):
|
||
"""оставить только ПЛОТНЫЕ ячейки.
|
||
|
||
Снимок облака показал, из чего состоит ошибка: сам товар - плотное пятно верного
|
||
размера, а вокруг него разреженный ореол на все 556 x 517 мм. Это точки полотна,
|
||
пережившие порог по высоте, и борта ленты. Связность их склеивает с товаром в один
|
||
сгусток, и габарит раздувается до размера зоны осмотра.
|
||
|
||
Полотно даёт РЕДКИЕ выбросы (шум диспаратности на однородной поверхности), товар -
|
||
сплошную поверхность. Поэтому отбор идёт по числу точек в ячейке, а не по связности.
|
||
"""
|
||
if len(P) < 60:
|
||
return np.ones(len(P), bool)
|
||
key = np.floor(P[:, :3] / cell).astype(np.int64)
|
||
key2 = key[:, :2]
|
||
uniq, inv, cnt = np.unique(key2, axis=0, return_inverse=True, return_counts=True)
|
||
return cnt[inv] >= need
|
||
|
||
|
||
def biggest_blob_idx(P, grid=0.012):
|
||
"""самый крупный связный сгусток в плане - целевой товар, а не сосед потока"""
|
||
if len(P) < 40:
|
||
return np.ones(len(P), bool)
|
||
cx = np.round(P[:, 0] / grid).astype(int); cy = np.round(P[:, 1] / grid).astype(int)
|
||
im = np.zeros((cy.max() - cy.min() + 3, cx.max() - cx.min() + 3), np.uint8)
|
||
im[cy - cy.min() + 1, cx - cx.min() + 1] = 255
|
||
im = cv2.morphologyEx(im, cv2.MORPH_CLOSE, np.ones((3, 3), np.uint8))
|
||
n, lab = cv2.connectedComponents(im)
|
||
li = lab[cy - cy.min() + 1, cx - cx.min() + 1]
|
||
best, bn = None, 0
|
||
for k in range(1, n):
|
||
m = li == k
|
||
if m.sum() > bn:
|
||
best, bn = m, m.sum()
|
||
return np.ones(len(P), bool) if best is None else best
|
||
|
||
|
||
def scatter(P, cols, ax=(0, 1), w=460, h=460, title=""):
|
||
a, b = ax
|
||
x, y = P[:, a], P[:, b]
|
||
pad = 26
|
||
sx = (w - 2 * pad) / max(1e-6, x.max() - x.min())
|
||
sy = (h - 2 * pad) / max(1e-6, y.max() - y.min())
|
||
s = min(sx, sy)
|
||
img = np.zeros((h, w, 3), np.uint8)
|
||
for (px, py), c in zip(np.stack([x, y], 1), cols):
|
||
u = int((px - x.min()) * s) + pad
|
||
v = h - (int((py - y.min()) * s) + pad)
|
||
if 0 <= u < w and 0 <= v < h:
|
||
cv2.circle(img, (u, v), 1, (int(c[0]), int(c[1]), int(c[2])), -1)
|
||
cv2.putText(img, title, (8, 18), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (210, 210, 210), 1)
|
||
cv2.putText(img, f"{(x.max()-x.min())*1000:.0f} x {(y.max()-y.min())*1000:.0f} mm",
|
||
(8, h - 8), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (150, 150, 150), 1)
|
||
return img
|
||
|
||
|
||
|
||
def k_three_sections(P):
|
||
"""k по ТРЁМ взаимно перпендикулярным сечениям, берётся максимум.
|
||
|
||
Правило класса D в README: "k > 0.8 ХОТЯ БЫ В ОДНОМ СЕЧЕНИИ". До сих пор считалось
|
||
только горизонтальное сечение на середине высоты, и этого достаточно лишь для тел,
|
||
стоящих вертикально. Ведро на прогоне ЛЕЖИТ НА БОКУ: его горизонтальный срез - это
|
||
прямоугольник "длина x хорда", и k = 0.43 для него верен, просто отвечает не на тот
|
||
вопрос. Круглое сечение лежащего цилиндра - вертикальное, поперёк оси.
|
||
|
||
Замерено, что дело не в камерах: восстановленное полотно садится на эталонную
|
||
плоскость со смещением 0.6-1.1 мм и наклоном 0.35-1.72 градуса по всем трём ригам,
|
||
а дуга у ведра покрыта на 295-360 градусов. То есть ни интринсики, ни расположение
|
||
ригов, ни слияние тут ни при чём - не хватало именно второго и третьего сечения.
|
||
"""
|
||
if len(P) < 60:
|
||
return float("nan"), -1
|
||
best, bax = float("nan"), -1
|
||
for ax in range(3): # секущая плоскость перпендикулярна оси ax
|
||
keep = [i for i in range(3) if i != ax]
|
||
lo, hi = np.percentile(P[:, ax], 2), np.percentile(P[:, ax], 98)
|
||
mid = (lo + hi) / 2.0
|
||
half = max(0.012, (hi - lo) * 0.08)
|
||
band = P[np.abs(P[:, ax] - mid) < half][:, keep]
|
||
if len(band) < 25:
|
||
continue
|
||
pts = (band - band.mean(0)) * 1000.0
|
||
hull = cv2.convexHull(np.ascontiguousarray(pts.astype(np.float32)))
|
||
(_, _), R_out = cv2.minEnclosingCircle(hull)
|
||
if R_out < 1e-6:
|
||
continue
|
||
sc = 2.0
|
||
sh = int(np.ceil((pts.max() - pts.min()) * sc)) + 20
|
||
if sh < 8 or sh > 4000:
|
||
continue
|
||
img = np.zeros((sh, sh), np.uint8)
|
||
cv2.fillConvexPoly(img, np.int32((hull.reshape(-1, 2) - pts.min()) * sc + 10), 255)
|
||
k = min(1.0, float(cv2.distanceTransform(img, cv2.DIST_L2, 5).max()) / sc / R_out)
|
||
if np.isnan(best) or k > best:
|
||
best, bax = k, ax
|
||
return best, bax
|
||
|
||
|
||
|
||
K_SECTORS = 48 # угловых секторов, 7.5 градуса каждый
|
||
K_MIN_PER = 4 # точек в секторе, иначе сектор не в счёт
|
||
|
||
|
||
def k_smooth(P, mode="h"):
|
||
"""k по СГЛАЖЕННОМУ контуру сечения: радиус как функция угла, в секторе - медиана.
|
||
|
||
Прежний k брался как вписанный радиус к описанному по выпуклой ОБОЛОЧКЕ. Оболочка
|
||
строится по крайним точкам, а край товара размазан шумом диспаратности: разброс самого
|
||
полотна замерен в 7-10 мм, и такой же разброс сидит на границе предмета. Показатель
|
||
страдает дважды - выброс наружу увеличивает R_out, выброс внутрь уменьшает r_in,
|
||
поэтому k занижался у всего: у коробки с истинным 0.72 читался 0.59, у ведра с 0.99 - 0.43.
|
||
|
||
Здесь контур сглаживается по углу: точки сечения разбиваются на сектора вокруг центра,
|
||
в каждом берётся МЕДИАНА радиуса, и k считается по этим медианам. Единичный выброс в
|
||
секторе из десятков точек не проходит. Смысл сохраняется: у круга профиль радиуса
|
||
плоский -> k ~ 1, у квадрата меняется от a до a*sqrt2 -> k ~ 0.707.
|
||
|
||
Центр берётся медианой координат, а не средним: среднее тянется в сторону той дуги,
|
||
где точек больше.
|
||
"""
|
||
if len(P) < 60:
|
||
return float("nan")
|
||
top = np.percentile(P[:, 2], 98)
|
||
zmid = BELT_Z + (top - BELT_Z) * 0.5
|
||
band = P[np.abs(P[:, 2] - zmid) < 0.02][:, :2]
|
||
if len(band) < 40:
|
||
return float("nan")
|
||
c = np.median(band, axis=0)
|
||
d = (band - c) * 1000.0
|
||
r = np.hypot(d[:, 0], d[:, 1])
|
||
a = np.arctan2(d[:, 1], d[:, 0])
|
||
idx = ((a + np.pi) / (2 * np.pi) * K_SECTORS).astype(int) % K_SECTORS
|
||
prof = []
|
||
for i in range(K_SECTORS):
|
||
m = idx == i
|
||
if m.sum() >= K_MIN_PER:
|
||
prof.append(np.median(r[m]))
|
||
if len(prof) < K_SECTORS // 2: # дуга меньше половины круга - судить нельзя
|
||
return float("nan")
|
||
prof = np.array(prof)
|
||
lo, hi = np.percentile(prof, 10), np.percentile(prof, 90)
|
||
return float(min(1.0, lo / hi)) if hi > 1e-6 else float("nan")
|
||
|
||
|
||
|
||
# ---------------------------------------------------------------------------------------
|
||
# ПРОВЕРЕННЫЙ ПОКАЗАТЕЛЬ КРУГОВОГО СЕЧЕНИЯ, перенесён из isaac_assets/cv/circular_section.py
|
||
#
|
||
# Мои три попытки поднять k провалились подряд (выпуклая оболочка по одному горизонтальному
|
||
# сечению 0.85, три мировых сечения 0.83, сглаживание по секторам 0.67), и все три отличались
|
||
# от этой реализации одним и тем же: они резали облако по МИРОВЫМ осям и на ОДНОЙ высоте.
|
||
#
|
||
# Здесь облако сначала выравнивается по СОБСТВЕННЫМ главным осям (SVD), и только потом
|
||
# режется - на пяти высотах вдоль каждой из трёх осей, толщиной 7 % размаха. Для лежащего
|
||
# на боку ведра главная ось это и есть ось цилиндра, поэтому перпендикулярное сечение
|
||
# оказывается тем самым кругом; при резке по мировым осям такого сечения не существует.
|
||
#
|
||
# Плюс защита от вытянутых пятен: невязка подгонки окружности (Kasa) должна быть < 0.15,
|
||
# иначе скруглённый овал прошёл бы как круг.
|
||
#
|
||
# На эталонной геометрии мешей эта версия давала bucket 0.934 при истинных 0.995, bag 0.889
|
||
# при 0.896, и корректно отвергала коробки (0.685-0.688 при пороге 0.8).
|
||
# ---------------------------------------------------------------------------------------
|
||
K_ROUND = 0.80
|
||
|
||
STEREO = os.environ.get("STEREO", "defom") # cre | defom (бейзлайн: defom)
|
||
DEFOM_CKPT = os.environ.get("DEFOM_CKPT", "vitl") # vitl | vits
|
||
DEFOM_ITERS = int(os.environ.get("DEFOM_ITERS", "24"))
|
||
DEFOM_SITERS = int(os.environ.get("DEFOM_SITERS", "8"))
|
||
_defom = None
|
||
|
||
|
||
def _defom_model():
|
||
global _defom
|
||
if _defom is None:
|
||
import types, torch
|
||
DR = "/home/dasha/isaac_assets/cv/defom-stereo"
|
||
if DR not in sys.path:
|
||
sys.path.insert(0, DR); sys.path.insert(0, DR + "/core")
|
||
from core.defom_stereo import DEFOMStereo
|
||
a = types.SimpleNamespace(
|
||
dinov2_encoder=DEFOM_CKPT, idepth_scale=0.5, hidden_dims=[128] * 3,
|
||
corr_implementation="reg", shared_backbone=False, corr_levels=2, corr_radius=4,
|
||
scale_list=[0.125, 0.25, 0.5, 0.75, 1.0, 1.25, 1.5, 2.0], scale_corr_radius=2,
|
||
n_downsample=2, context_norm="batch", n_gru_layers=3, mixed_precision=False)
|
||
m = DEFOMStereo(a)
|
||
ck = torch.load(f"{DR}/checkpoints/defomstereo_{DEFOM_CKPT}_sceneflow.pth",
|
||
map_location="cpu", weights_only=False)
|
||
m.load_state_dict({k.replace("module.", ""): v for k, v in ck.items()}, strict=False)
|
||
_defom = m.to("cuda").eval()
|
||
return _defom
|
||
|
||
|
||
def defom_batch(pairs, iters=None, scale_iters=None):
|
||
"""DEFOM одним проходом по нескольким парам. Кропы дополняются нулями до общего размера,
|
||
кратного 32; диспаратность читается только внутри исходных границ каждого кропа."""
|
||
import torch
|
||
if not pairs:
|
||
return []
|
||
m = _defom_model()
|
||
iters = DEFOM_ITERS if iters is None else iters
|
||
scale_iters = DEFOM_SITERS if scale_iters is None else scale_iters
|
||
hs = [q[0].shape[0] for q in pairs]; ws = [q[0].shape[1] for q in pairs]
|
||
Hp = (max(hs) + 31) // 32 * 32; Wp = (max(ws) + 31) // 32 * 32
|
||
n = len(pairs)
|
||
A = np.zeros((n, 3, Hp, Wp), np.float32); B = np.zeros((n, 3, Hp, Wp), np.float32)
|
||
for i, (L, R) in enumerate(pairs):
|
||
h, w = L.shape[:2]
|
||
A[i, :, :h, :w] = L.transpose(2, 0, 1)
|
||
B[i, :, :h, :w] = R.transpose(2, 0, 1)
|
||
iL = torch.from_numpy(A).cuda(); iR = torch.from_numpy(B).cuda()
|
||
with torch.inference_mode():
|
||
d = m(iL, iR, iters=iters, scale_iters=scale_iters, test_mode=True)
|
||
d = np.abs(d.squeeze(1).detach().cpu().numpy())
|
||
return [d[i][:hs[i], :ws[i]] for i in range(n)]
|
||
|
||
|
||
|
||
CROP = os.environ.get("CROP", "1") == "1" # считать CRE по КРОПУ зоны осмотра, а не всей ленты
|
||
CROP_H = 0.45 # запас по высоте товара, м
|
||
SW = int(os.environ.get("SW", "480")) # ширина входа сети (0 - без понижения)
|
||
|
||
|
||
def cre_scaled(pairs, sw):
|
||
"""CRE на пониженном разрешении с обратным масштабом диспаратности.
|
||
|
||
Приём взят из прежнего DEFOM-скрипта проекта (flow_defom_cache.py): кадр сжимается до
|
||
ширины sw, сеть считает по нему, диспаратность возвращается к исходному размеру и
|
||
УМНОЖАЕТСЯ на W/sw. Диспаратность измеряется в пикселях, поэтому при сжатии кадра в k
|
||
раз она сжимается во столько же - без этого множителя глубина уехала бы ровно в k раз.
|
||
Пропорции сохраняются: масштаб по обеим осям один, иначе ломается эпиполярная геометрия.
|
||
"""
|
||
engine = defom_batch if STEREO == "defom" else MF.cre_batch
|
||
if sw <= 0:
|
||
return engine(pairs)
|
||
small, meta = [], []
|
||
for (L, R) in pairs:
|
||
h, w = L.shape[:2]
|
||
sh = max(8, int(round(h * sw / w / 8)) * 8)
|
||
small.append((cv2.resize(L, (sw, sh)), cv2.resize(R, (sw, sh))))
|
||
meta.append((w, h, w / float(sw)))
|
||
ds = engine(small)
|
||
out = []
|
||
for d, (w, h, f) in zip(ds, meta):
|
||
out.append(cv2.resize(d, (w, h), interpolation=cv2.INTER_LINEAR) * f)
|
||
return out
|
||
|
||
|
||
def gate_crop_px(cam, r=GATE_R, hmax=CROP_H, pad=24):
|
||
"""окно кадра, куда проецируется зона осмотра - цилиндр радиусом r над лентой.
|
||
|
||
Зона ленты, по которой сейчас считается CRE, занимает 84-92 % кадра, хотя товар всегда
|
||
внутри круга радиусом 280 мм вокруг точки осмотра. Проекция этого круга (с запасом по
|
||
высоте на сам товар) даёт окно в разы меньше, и стереосети достаётся во столько же раз
|
||
меньше пикселей. Локализация тут ГЕОМЕТРИЧЕСКАЯ - ни сегментации, ни грубого прохода
|
||
не нужно, поэтому лишнего вызова сети не появляется.
|
||
"""
|
||
th = np.linspace(0, 2 * np.pi, 24, endpoint=False)
|
||
ring = np.c_[TARGET[0] + r * np.cos(th), TARGET[1] + r * np.sin(th)]
|
||
pts = np.vstack([np.c_[ring, np.full(len(ring), BELT_Z)],
|
||
np.c_[ring, np.full(len(ring), BELT_Z + hmax)]])
|
||
Minv = np.linalg.inv(np.array(cam["M"]))
|
||
c = (np.c_[pts, np.ones(len(pts))] @ Minv)[:, :3]
|
||
z = -c[:, 2]
|
||
u = c[:, 0] / np.maximum(z, 1e-9) * cam["fx"] + cam["cx"]
|
||
v = -c[:, 1] / np.maximum(z, 1e-9) * cam["fy"] + cam["cy"]
|
||
x0 = int(max(0, np.floor(u.min()) - pad)); x1 = int(min(cam["width"], np.ceil(u.max()) + pad))
|
||
y0 = int(max(0, np.floor(v.min()) - pad)); y1 = int(min(cam["height"], np.ceil(v.max()) + pad))
|
||
return x0, y0, x1, y1
|
||
|
||
|
||
|
||
LR_THR = float(os.environ.get("LR_THR", "0")) # порог проверки лево-право, px (0 = выкл)
|
||
CYL = os.environ.get("CYL", "0") == "1" # подгонка цилиндра как второй признак
|
||
CYL_TOL = 0.008 # допуск на радиус, м
|
||
CYL_MIN_INLIER = 0.60 # доля точек в допуске, чтобы счесть цилиндром
|
||
|
||
|
||
def disp_lr(L, R):
|
||
"""диспаратность в обе стороны одним батчем: прямая пара и зеркально отражённая.
|
||
|
||
Отражение по горизонтали превращает задачу "справа налево" в обычную "слева направо",
|
||
поэтому вторую карту даёт та же сеть без правок: отражаем оба кадра, меняем их местами,
|
||
считаем, отражаем результат обратно.
|
||
"""
|
||
Lf = np.ascontiguousarray(L[:, ::-1])
|
||
Rf = np.ascontiguousarray(R[:, ::-1])
|
||
dL, dRf = MF.cre_batch([(L, R), (Rf, Lf)])
|
||
dR = np.ascontiguousarray(dRf[:, ::-1])
|
||
return dL, dR
|
||
|
||
|
||
def lr_valid(dL, dR, thr):
|
||
"""маска согласованных точек: d_L(x) должна совпасть с d_R в точке x - d_L(x)."""
|
||
h, w = dL.shape
|
||
xs = np.arange(w)[None, :].repeat(h, 0)
|
||
xr = np.rint(xs - dL).astype(int)
|
||
ok = (xr >= 0) & (xr < w)
|
||
xr = np.clip(xr, 0, w - 1)
|
||
dRs = np.take_along_axis(dR, xr, axis=1)
|
||
return ok & (np.abs(dL - dRs) <= thr)
|
||
|
||
|
||
def cylinder_ransac(P, tol=CYL_TOL, n_axis=64, seed=0):
|
||
"""RANSAC по МОДЕЛИ цилиндра (не по совмещению облаков).
|
||
|
||
Ось ищется перебором направлений: главные оси облака плюс случайные. Точки проецируются
|
||
на плоскость, перпендикулярную оси, туда подгоняется окружность, и считается доля точек,
|
||
чей радиус попал в допуск. Нормали не нужны - они на рыхлом облаке сами шумят.
|
||
"""
|
||
if len(P) < 200:
|
||
return 0.0, float("nan"), float("nan")
|
||
rng = np.random.default_rng(seed)
|
||
Q = P - P.mean(0)
|
||
_, _, V = np.linalg.svd(Q, full_matrices=False)
|
||
axes = [V[0], V[1], V[2]]
|
||
for _ in range(n_axis):
|
||
v = rng.normal(size=3); axes.append(v / (np.linalg.norm(v) + 1e-12))
|
||
best = (0.0, float("nan"), float("nan"))
|
||
for d in axes:
|
||
d = d / (np.linalg.norm(d) + 1e-12)
|
||
a = np.array([1.0, 0.0, 0.0])
|
||
if abs(d @ a) > 0.9:
|
||
a = np.array([0.0, 1.0, 0.0])
|
||
e1 = np.cross(d, a); e1 /= np.linalg.norm(e1)
|
||
e2 = np.cross(d, e1)
|
||
xy = np.c_[Q @ e1, Q @ e2]
|
||
x, y = xy[:, 0], xy[:, 1]
|
||
A = np.c_[2 * x, 2 * y, np.ones(len(x))]
|
||
try:
|
||
sol, *_ = np.linalg.lstsq(A, x * x + y * y, rcond=None)
|
||
except np.linalg.LinAlgError:
|
||
continue
|
||
cx, cy, cc = sol
|
||
R = np.sqrt(max(cc + cx * cx + cy * cy, 1e-12))
|
||
r = np.hypot(x - cx, y - cy)
|
||
inl = float(np.mean(np.abs(r - R) <= tol))
|
||
if inl > best[0]:
|
||
best = (inl, float(R), float(np.median(np.abs(r - R))))
|
||
return best
|
||
|
||
|
||
|
||
def _kasa(P):
|
||
x,y=P[:,0],P[:,1]; A=np.c_[2*x,2*y,np.ones(len(x))]; b=x*x+y*y
|
||
s,*_=np.linalg.lstsq(A,b,rcond=None); cx,cy,cc=s; r=np.sqrt(max(cc+cx*cx+cy*cy,1e-12))
|
||
return cx,cy,r,np.abs(np.hypot(x-cx,y-cy)-r).mean()
|
||
|
||
|
||
def section_K(xy):
|
||
"""True r_inscribed/R_circumscribed of a full cross-section outline via the convex-hull
|
||
incenter (largest inscribed circle) and the circumscribed radius from that centre.
|
||
K=1 for a circle, b/a for an ellipse, 0.707 for a square, short/long for a rectangle."""
|
||
from scipy.spatial import ConvexHull
|
||
if len(xy)<20: return None,0.0,1.0
|
||
try: h=ConvexHull(xy)
|
||
except Exception: return None,0.0,1.0
|
||
V=xy[h.vertices] # CCW hull vertices
|
||
A=V; B=np.roll(V,-1,axis=0); E=B-A; L=np.linalg.norm(E,axis=1)+1e-12
|
||
mn=xy.min(0); mx=xy.max(0)
|
||
G=np.stack(np.meshgrid(np.linspace(mn[0],mx[0],40),np.linspace(mn[1],mx[1],40)),-1).reshape(-1,2)
|
||
# signed distance from each grid point to each hull edge (CCW -> interior side positive)
|
||
d=(E[:,0][None,:]*(G[:,1][:,None]-A[:,1][None,:]) - E[:,1][None,:]*(G[:,0][:,None]-A[:,0][None,:]))/L[None,:]
|
||
inside=(d>0).all(1)
|
||
if inside.sum()<3: return 0.0,1.0,1.0
|
||
rin=float(d[inside].min(1).max()) # max inscribed circle radius (its own centre)
|
||
# min enclosing circle radius (its own centre): grid centre minimising max distance to hull vertices
|
||
Gd=np.stack(np.meshgrid(np.linspace(mn[0],mx[0],48),np.linspace(mn[1],mx[1],48)),-1).reshape(-1,2)
|
||
Rout=float(np.linalg.norm(Gd[:,None,:]-V[None,:,:],axis=2).max(1).min())
|
||
return rin/max(Rout,1e-9),1.0,0.0
|
||
|
||
|
||
def circular_section_K(points):
|
||
"""Max K over cross-sections sampled along each principal axis (a circle in ANY section
|
||
-> round). Returns (max_K, is_round, best_section)."""
|
||
if len(points)<60: return 0.0,False,None
|
||
c=points.mean(0); Q=points-c; _,_,V=np.linalg.svd(Q,full_matrices=False); proj=Q@V.T
|
||
best=0.0; best_sec=None
|
||
for a in range(3):
|
||
o=[i for i in range(3) if i!=a]; ca=proj[:,a]; sp=np.ptp(ca)+1e-9
|
||
for frac in (0.25,0.375,0.5,0.625,0.75): # sample slices along the axis
|
||
lvl=np.percentile(ca,frac*100)
|
||
sl=proj[np.abs(ca-lvl)<0.07*sp][:,o]
|
||
K,cov,rr=section_K(sl)
|
||
if K is not None and rr<0.15 and K>best:
|
||
best=K; best_sec=(a,round(frac,2),round(cov,2),rr)
|
||
return round(best,3), (best>K_ROUND), best_sec
|
||
|
||
|
||
|
||
# При импорте из рабочего процесса замкнутого контура прогон по девяти товарам не нужен -
|
||
# нужны только функции (defom_batch, gate_crop_px, cloud_from_roi, circular_section_K...).
|
||
if os.environ.get("IMPORT_ONLY") == "1":
|
||
import sys as _s
|
||
_s.modules[__name__].__dict__.setdefault("_ready", True)
|
||
else:
|
||
print(f"БЕЗ СЕГМЕНТАЦИИ: CRE по зоне ленты, товар выше полотна на {FLOOR*1000:.0f} мм | "
|
||
f"проверка лево-право {LR_THR if LR_THR>0 else 'выкл'} px | цилиндр {'вкл' if CYL else 'выкл'}"
|
||
f" | окно {'КРОП зоны осмотра' if CROP else 'вся лента'}"
|
||
f" | вход сети {SW if SW>0 else 'исходный'} | движок {STEREO.upper()}\n")
|
||
print(f" {'товар':18s} {'эталон, мм':>18s} {'предсказано, мм':>20s} {'MAE':>6s} "
|
||
f"{'k':>6s} {'ось':>3s} {'класс':>12s} {'точек':>7s} {'мс':>6s}")
|
||
print(" " + "-" * 100)
|
||
|
||
rows, times = [], []
|
||
FOCUS = {"bag", "bucket", "box_300x200x200"}
|
||
for name, e in man["items"].items():
|
||
t0 = time.time()
|
||
pairs, metas = [], []
|
||
for rig in RIGS:
|
||
camL = calib[f"{rig}_Left"]
|
||
IL = cv2.imread(e["files"][f"{rig}_Left"])
|
||
IR = cv2.imread(e["files"][f"{rig}_Right"])
|
||
if IL is None or IR is None:
|
||
continue
|
||
x0, y0, x1, y1 = (gate_crop_px(camL) if CROP else MF.belt_roi_px(camL, MF.POLY3))
|
||
maxd = int(np.ceil(MF.DPAD * camL["fx"] * camL["baseline"] / MF.ZMIN))
|
||
x0 = max(0, x0 - maxd) # запас влево на максимальную диспаратность: правый
|
||
# двойник обязан попасть в ТО ЖЕ окно колонок
|
||
pairs.append((IL[y0:y1, x0:x1].astype(np.float32), IR[y0:y1, x0:x1].astype(np.float32)))
|
||
metas.append(((x0, y0, x1, y1), camL, rig, IL))
|
||
if LR_THR > 0:
|
||
# обе карты считаются ДО проекции: несогласованные пиксели вообще не становятся
|
||
# точками, а не удаляются потом из облака
|
||
disps, rejected = [], []
|
||
for (L, R) in pairs:
|
||
dL, dR = disp_lr(L, R)
|
||
good = lr_valid(dL, dR, LR_THR)
|
||
rejected.append(1.0 - float(good.mean()))
|
||
dd = dL.copy(); dd[~good] = 0.0 # 0 -> пиксель не пройдёт порог disp > 0.5
|
||
disps.append(dd)
|
||
else:
|
||
disps = cre_scaled(pairs, SW); rejected = []
|
||
clouds, cols, belts = [], [], []
|
||
for disp, (win, cam, rig, IL) in zip(disps, metas):
|
||
obj, belt = cloud_from_roi(disp, win, cam)
|
||
if len(obj):
|
||
clouds.append(obj); cols += [RIGCOL[rig]] * len(obj)
|
||
belts.append(len(belt))
|
||
if name in FOCUS:
|
||
dn = cv2.normalize(disp, None, 0, 255, cv2.NORM_MINMAX).astype(np.uint8)
|
||
hm = cv2.applyColorMap(dn, cv2.COLORMAP_TURBO)
|
||
cv2.imwrite(f"{OUT}/{name}_{rig}_disp.png", hm)
|
||
dt = (time.time() - t0) * 1000
|
||
times.append(dt)
|
||
gt = e["gt"]; gtd = sorted(gt["dims_mm"], reverse=True)
|
||
if not clouds:
|
||
print(f" {name:18s} {str([round(v) for v in gtd]):>18s} облако пустое"); continue
|
||
P = np.vstack(clouds); C = list(cols)
|
||
# самый крупный сгусток в плане: при шаге 700 мм сосед потока попадает в кадр
|
||
sel = dense_only(P)
|
||
if sel.sum() >= 60:
|
||
P = P[sel]; C = [C[i] for i in np.nonzero(sel)[0]]
|
||
sel = biggest_blob_idx(P)
|
||
P = P[sel]; C = [C[i] for i in np.nonzero(sel)[0]]
|
||
out = MF.dims_and_k(P)
|
||
if out is None:
|
||
print(f" {name:18s} габариты не взялись"); continue
|
||
dims, _k_hull = out
|
||
k, _is_round, _sec = circular_section_K(P)
|
||
kax = _sec[0] if _sec else -1
|
||
if not k:
|
||
k = _k_hull
|
||
cyl_inl = cyl_R = float("nan")
|
||
if CYL:
|
||
cyl_inl, cyl_R, cyl_res = cylinder_ransac(P)
|
||
if cyl_inl >= CYL_MIN_INLIER:
|
||
k = max(k, 0.81) # цилиндр подтверждён - признак D сработал
|
||
mae = float(np.mean(np.abs(np.array(dims) - np.array(gtd))))
|
||
cls = CL.classify(dims, 0.0 if np.isnan(k) else k)
|
||
ok = "верно" if cls == gt["zone_scene"] else f"ОШ({gt['zone_scene']})"
|
||
print(f" {name:18s} {str([round(v) for v in gtd]):>18s} "
|
||
f"{str([round(v) for v in dims]):>20s} {mae:6.1f} {k:6.2f} {'xyz'[kax] if kax>=0 else '-':>3s} "
|
||
f"{cls+' '+ok:>12s} {len(P):7d} {dt:6.0f}"
|
||
+ (f" цил {cyl_inl*100:3.0f}% R={cyl_R*1000:5.0f}мм" if CYL else "")
|
||
+ (f" отсев ЛП {np.mean(rejected)*100:3.0f}%" if LR_THR > 0 and rejected else ""))
|
||
rows.append(dict(name=name, gt=gtd, gt_cls=gt["zone_scene"], gt_k=gt["k"],
|
||
pred=[round(v, 1) for v in dims], pred_k=round(float(k), 3),
|
||
pred_cls=cls, mae=round(mae, 1), n=len(P), ms=round(dt)))
|
||
if name in FOCUS:
|
||
cv2.imwrite(f"{OUT}/{name}_cloud_top.png", scatter(P, C, (0, 1), title=f"{name} top"))
|
||
cv2.imwrite(f"{OUT}/{name}_cloud_side.png", scatter(P, C, (0, 2), title=f"{name} side"))
|
||
|
||
print("\n === МЕТРИКИ (без сегментации) ===")
|
||
if rows:
|
||
maes = [r["mae"] for r in rows]
|
||
acc = sum(1 for r in rows if r["pred_cls"] == r["gt_cls"])
|
||
print(f" габариты: MAE медиана {np.median(maes):.1f} мм, среднее {np.mean(maes):.1f}, "
|
||
f"худший {max(maes):.1f} ({max(rows, key=lambda r: r['mae'])['name']})")
|
||
print(f" классы: {acc}/{len(rows)} = {100.0*acc/len(rows):.0f}%")
|
||
print(f" k макс {max(r['pred_k'] for r in rows):.2f}")
|
||
lab = ["B", "C", "D"]
|
||
print(" матрица (строки истина, столбцы предсказание):")
|
||
print(" " + "".join(f"{c:>5s}" for c in lab))
|
||
for a in lab:
|
||
print(f" {a:3s} " + "".join(
|
||
f"{sum(1 for r in rows if r['gt_cls']==a and r['pred_cls']==b):5d}" for b in lab))
|
||
print(f" время: медиана {np.median(times):.0f} мс, такт {PITCH_S*1000:.0f} мс -> "
|
||
f"{'УКЛАДЫВАЕТСЯ' if np.median(times) < PITCH_S*1000 else 'НЕ УКЛАДЫВАЕТСЯ'}")
|
||
json.dump(rows, open(f"{OUT}/metrics.json", "w"), indent=1, ensure_ascii=False)
|
||
print(f"\n картинки -> {OUT}")
|