Сортировочная ячейка Isaac Sim: CV-пайплайн и меши товаров
Замкнутый контур "поток -> 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>
This commit is contained in:
dasha_f
@@ -0,0 +1,561 @@
|
||||
"""ВАРИАНТ БЕЗ СЕГМЕНТАЦИИ: 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}")
|
||||
Reference in New Issue
Block a user