diff --git a/pm2d/_jit_kernels.py b/pm2d/_jit_kernels.py index b504819..d1e7069 100644 --- a/pm2d/_jit_kernels.py +++ b/pm2d/_jit_kernels.py @@ -271,6 +271,108 @@ if HAS_NUMBA: acc[y, x] = 0.0 return acc + @nb.njit(cache=True, parallel=True, fastmath=True, boundscheck=False) + def _jit_score_bitmap_rescored_window( + spread: np.ndarray, # uint8 (H, W) - scena INTERA + dx: np.ndarray, dy: np.ndarray, bins: np.ndarray, + bit_active: np.uint8, + bg: np.ndarray, # float32 (H, W) - scena intera + y0: nb.int64, x0: nb.int64, + wh: nb.int64, ww: nb.int64, + ) -> np.ndarray: + """Score rescored valutato SOLO nella finestra (y0, x0, wh, ww). + + Le feature campionano lo spread dell'intera scena (bounds-checked + sui bordi scena): a differenza di chiamare il kernel su un crop, + le feature che escono dalla finestra NON contano come miss. + Usato dal path pyramid_propagate: costo ∝ area finestra. + """ + H, W = spread.shape + N = dx.shape[0] + acc = np.zeros((wh, ww), dtype=np.float32) + for yi in nb.prange(wh): + y = y0 + yi + for i in range(N): + b = bins[i] + mask = np.uint8(1) << b + if (bit_active & mask) == 0: + continue + yy = y + dy[i] + if yy < 0 or yy >= H: + continue + ddx = dx[i] + xi_lo = 0 + xi_hi = ww + lo = -(x0 + ddx) + if lo > xi_lo: + xi_lo = lo + hi = W - (x0 + ddx) + if hi < xi_hi: + xi_hi = hi + for xi in range(xi_lo, xi_hi): + if spread[yy, x0 + xi + ddx] & mask: + acc[yi, xi] += 1.0 + if N > 0: + inv = 1.0 / N + for yi in nb.prange(wh): + for xi in range(ww): + v = acc[yi, xi] * inv + bgv = bg[y0 + yi, x0 + xi] + if bgv < 1.0: + r = (v - bgv) / (1.0 - bgv + 1e-6) + acc[yi, xi] = r if r > 0.0 else 0.0 + else: + acc[yi, xi] = 0.0 + return acc + + @nb.njit(cache=True, parallel=True, fastmath=True, boundscheck=False) + def _jit_score_bitmap_rescored_window_u16( + spread: np.ndarray, # uint16 (H, W) + dx: np.ndarray, dy: np.ndarray, bins: np.ndarray, + bit_active: np.uint16, + bg: np.ndarray, + y0: nb.int64, x0: nb.int64, + wh: nb.int64, ww: nb.int64, + ) -> np.ndarray: + """Versione uint16 (polarity 16-bin) del kernel windowed.""" + H, W = spread.shape + N = dx.shape[0] + acc = np.zeros((wh, ww), dtype=np.float32) + for yi in nb.prange(wh): + y = y0 + yi + for i in range(N): + b = bins[i] + mask = np.uint16(1) << b + if (bit_active & mask) == 0: + continue + yy = y + dy[i] + if yy < 0 or yy >= H: + continue + ddx = dx[i] + xi_lo = 0 + xi_hi = ww + lo = -(x0 + ddx) + if lo > xi_lo: + xi_lo = lo + hi = W - (x0 + ddx) + if hi < xi_hi: + xi_hi = hi + for xi in range(xi_lo, xi_hi): + if spread[yy, x0 + xi + ddx] & mask: + acc[yi, xi] += 1.0 + if N > 0: + inv = 1.0 / N + for yi in nb.prange(wh): + for xi in range(ww): + v = acc[yi, xi] * inv + bgv = bg[y0 + yi, x0 + xi] + if bgv < 1.0: + r = (v - bgv) / (1.0 - bgv + 1e-6) + acc[yi, xi] = r if r > 0.0 else 0.0 + else: + acc[yi, xi] = 0.0 + return acc + @nb.njit(cache=True, parallel=True, fastmath=True, boundscheck=False) def _jit_top_max_per_variant( spread: np.ndarray, # uint8 (H, W) @@ -426,6 +528,9 @@ if HAS_NUMBA: _jit_top_max_per_variant( spread, dx, dy, b, offsets, np.uint8(0xFF), bg_pv, scale_idx, ) + _jit_score_bitmap_rescored_window( + spread, dx, dy, b, np.uint8(0xFF), bg, 4, 4, 8, 8, + ) _jit_popcount_density(spread) spread16 = np.zeros((32, 32), dtype=np.uint16) _jit_score_bitmap_rescored_u16( @@ -447,6 +552,12 @@ else: # pragma: no cover def _jit_score_bitmap_rescored_strided(spread, dx, dy, bins, bit_active, bg, stride): raise RuntimeError("numba non disponibile") + def _jit_score_bitmap_rescored_window(spread, dx, dy, bins, bit_active, bg, y0, x0, wh, ww): + raise RuntimeError("numba non disponibile") + + def _jit_score_bitmap_rescored_window_u16(spread, dx, dy, bins, bit_active, bg, y0, x0, wh, ww): + raise RuntimeError("numba non disponibile") + def _jit_score_bitmap_greedy(spread, dx, dy, bins, bit_active, min_score, greediness): raise RuntimeError("numba non disponibile") @@ -524,6 +635,39 @@ def score_bitmap_rescored( return np.maximum(0.0, out).astype(np.float32) +def score_bitmap_rescored_window( + spread: np.ndarray, dx: np.ndarray, dy: np.ndarray, bins: np.ndarray, + bit_active: int, bg: np.ndarray, + y0: int, x0: int, wh: int, ww: int, +) -> np.ndarray: + """Score rescored solo nella finestra (y0, x0, wh, ww) della scena. + + Le feature campionano l'INTERA scena: feature fuori finestra ma dentro + scena contano correttamente (chiamare il kernel su un crop le tratta + come miss e azzera lo score — il bug che rendeva inutilizzabile il + path pyramid_propagate). Fallback no-numba: kernel pieno + slice. + """ + if HAS_NUMBA and len(dx) > 0: + dx_c = np.ascontiguousarray(dx, dtype=np.int32) + dy_c = np.ascontiguousarray(dy, dtype=np.int32) + bins_c = np.ascontiguousarray(bins, dtype=np.int8) + bg_c = np.ascontiguousarray(bg, dtype=np.float32) + if spread.dtype == np.uint16: + return _jit_score_bitmap_rescored_window_u16( + np.ascontiguousarray(spread, dtype=np.uint16), + dx_c, dy_c, bins_c, np.uint16(bit_active), bg_c, + int(y0), int(x0), int(wh), int(ww), + ) + return _jit_score_bitmap_rescored_window( + np.ascontiguousarray(spread, dtype=np.uint8), + dx_c, dy_c, bins_c, np.uint8(bit_active), bg_c, + int(y0), int(x0), int(wh), int(ww), + ) + # Fallback (lento, solo senza numba): score full-frame + slice finestra + full = score_bitmap_rescored(spread, dx, dy, bins, bit_active, bg) + return full[y0:y0 + wh, x0:x0 + ww] + + def score_bitmap_greedy( spread: np.ndarray, dx: np.ndarray, dy: np.ndarray, bins: np.ndarray, bit_active: int, min_score: float, greediness: float, diff --git a/pm2d/line_matcher.py b/pm2d/line_matcher.py index 15967bd..543e0c6 100644 --- a/pm2d/line_matcher.py +++ b/pm2d/line_matcher.py @@ -40,6 +40,7 @@ from pm2d._jit_kernels import ( score_by_shift as _jit_score_by_shift, score_bitmap as _jit_score_bitmap, score_bitmap_rescored as _jit_score_bitmap_rescored, + score_bitmap_rescored_window as _jit_score_bitmap_rescored_window, score_bitmap_greedy as _jit_score_bitmap_greedy, top_max_per_variant as _jit_top_max_per_variant, popcount_density as _jit_popcount, @@ -172,7 +173,7 @@ class LineShapeMatcher: scale_step: float = 0.1, spread_radius: int = 4, min_feature_spacing: int = 3, - pyramid_levels: int = 2, + pyramid_levels: int = 3, top_score_factor: float = 0.5, n_threads: int | None = None, use_polarity: bool = False, @@ -473,8 +474,46 @@ class LineShapeMatcher: step = self._effective_angle_step() if step <= 0 or a0 >= a1: return [float(a0)] - n = int(np.floor((a1 - a0) / step)) - return [float(a0 + i * step) for i in range(n)] + # Include l'estremo superiore: con range parziali (es. ±15°) il + # +15° deve essere testato quanto il -15°. Se il range copre 360° + # interi l'estremo coincide con a0 (mod 360) e viene escluso per + # non duplicare la variante. + n = int(np.floor((a1 - a0) / step + 1e-9)) + 1 + angles = [float(a0 + i * step) for i in range(n)] + if a1 - a0 >= 360.0: + angles = [a for a in angles if a - a0 < 360.0 - 1e-9] + return angles + + def _prepare_padded_template( + self, template_gray: np.ndarray, mask_full: np.ndarray, scale: float, + ) -> tuple[np.ndarray, np.ndarray, tuple[float, float], int]: + """Scala + padda template e mask; ritorna (gray_p, mask_p, center, diag). + + `center` e' il centro REALE del template dentro l'immagine paddata + (px + sw/2, py + sh/2): con padding floor differisce da diag/2 fino + a 0.5 px. Ruotare attorno a diag/2 (come si faceva prima) faceva + orbitare il centro-modello attorno al centro di rotazione, con un + bias di posizione dipendente dall'angolo. Tutti i percorsi che + ricostruiscono il template ruotato devono usare QUESTO helper. + """ + h, w = template_gray.shape + sw = max(16, int(round(w * scale))) + sh = max(16, int(round(h * scale))) + gray_s = cv2.resize(template_gray, (sw, sh), interpolation=cv2.INTER_LINEAR) + mask_s = cv2.resize(mask_full, (sw, sh), interpolation=cv2.INTER_NEAREST) + diag = int(np.ceil(np.hypot(sh, sw))) + 6 + py = (diag - sh) // 2 + px = (diag - sw) // 2 + gray_p = cv2.copyMakeBorder( + gray_s, py, diag - sh - py, px, diag - sw - px, + cv2.BORDER_REPLICATE, + ) + mask_p = cv2.copyMakeBorder( + mask_s, py, diag - sh - py, px, diag - sw - px, + cv2.BORDER_CONSTANT, value=0, + ) + center = (px + sw / 2.0, py + sh / 2.0) + return gray_p, mask_p, center, diag # --- Training ------------------------------------------------------ @@ -504,6 +543,13 @@ class LineShapeMatcher: h, w = gray.shape self.template_size = (w, h) self.template_gray = gray.copy() + # Clamp livelli piramide alla dimensione template: al top-level il + # lato minimo deve restare >= 12 px, sotto le feature collassano + # tutte negli stessi (dx,dy) e lo score top diventa rumore. + max_lv = 1 + while min(w, h) / (2 ** max_lv) >= 12 and max_lv < 4: + max_lv += 1 + self.pyramid_levels = max(1, min(self.pyramid_levels, max_lv)) if mask is None: mask_full = np.full((h, w), 255, dtype=np.uint8) else: @@ -566,24 +612,10 @@ class LineShapeMatcher: Estrazione algorithm identica al train() originale, separato per riuso da add_template_view (multi-template ensemble). """ - h, w = gray.shape for s in self._scale_list(): - sw = max(16, int(round(w * s))) - sh = max(16, int(round(h * s))) - gray_s = cv2.resize(gray, (sw, sh), interpolation=cv2.INTER_LINEAR) - mask_s = cv2.resize(mask_full, (sw, sh), interpolation=cv2.INTER_NEAREST) - diag = int(np.ceil(np.hypot(sh, sw))) + 6 - py = (diag - sh) // 2 - px = (diag - sw) // 2 - gray_p = cv2.copyMakeBorder( - gray_s, py, diag - sh - py, px, diag - sw - px, - cv2.BORDER_REPLICATE, + gray_p, mask_p, center, diag = self._prepare_padded_template( + gray, mask_full, s, ) - mask_p = cv2.copyMakeBorder( - mask_s, py, diag - sh - py, px, diag - sw - px, - cv2.BORDER_CONSTANT, value=0, - ) - center = (diag / 2.0, diag / 2.0) for ang in self._angle_list(): M = cv2.getRotationMatrix2D(center, ang, 1.0) @@ -600,10 +632,10 @@ class LineShapeMatcher: if len(fx) < 8: continue - cx_c = diag / 2.0 - cy_c = diag / 2.0 - dx = (fx - cx_c).astype(np.int32) - dy = (fy - cy_c).astype(np.int32) + # round (non truncation): astype(int32) tronca verso zero + # e introduceva un bias sistematico ~0.25 px verso il centro. + dx = np.round(fx - center[0]).astype(np.int32) + dy = np.round(fy - center[1]).astype(np.int32) x0 = int(dx.min()); x1 = int(dx.max()) y0 = int(dy.min()); y1 = int(dy.max()) @@ -687,8 +719,13 @@ class LineShapeMatcher: try: import hashlib h = hashlib.md5() - sample = gray.tobytes()[:65536] - h.update(sample) + # Hash dell'INTERA scena: hashare solo i primi 64KB (prime + # ~80 righe a 830px) faceva collidere scene con la stessa + # banda superiore (es. sfondo uniforme da camera fissa) → + # find() ritornava i risultati della scena sbagliata. + # tobytes() copiava gia' tutto il buffer, il costo extra + # dell'md5 completo e' ~1ms. + h.update(gray.tobytes()) h.update(f"|{gray.shape}|{gray.dtype}".encode()) h.update( f"|{self.weak_grad}|{self.strong_grad}" @@ -717,11 +754,17 @@ class LineShapeMatcher: while len(self._scene_cache) > self._SCENE_CACHE_SIZE: self._scene_cache.popitem(last=False) - def _spread_bitmap(self, gray: np.ndarray) -> np.ndarray: + def _spread_bitmap( + self, gray: np.ndarray, radius: int | None = None, + ) -> np.ndarray: """Spread bitmap: bit b acceso dove bin b è presente nel raggio. dtype: uint8 per N_BINS=8, uint16 per N_BINS_POL=16 (use_polarity). Se use_gpu=True: Sobel + dilate via cv2.UMat (OpenCL kernel GPU). + radius: override del raggio di spread (default self.spread_radius). + radius=0/1 produce una bitmap "fine" senza tolleranza, usata nel + refine finale: sulla bitmap dilatata lo score satura e il refine + non distingue pose entro ±spread_radius px / ±atan(spread/R) gradi. """ if self.use_gpu and not isinstance(gray, cv2.UMat): gray_in = cv2.UMat(np.ascontiguousarray(gray)) @@ -729,7 +772,8 @@ class LineShapeMatcher: gray_in = gray mag, bins = self._gradient(gray_in) valid = mag >= self.weak_grad - k = 2 * self.spread_radius + 1 + r = self.spread_radius if radius is None else max(0, int(radius)) + k = 2 * r + 1 kernel = np.ones((k, k), dtype=np.uint8) H, W = (gray.shape if isinstance(gray, np.ndarray) else (gray.get().shape[0], gray.get().shape[1])) @@ -755,7 +799,9 @@ class LineShapeMatcher: if not bin_present[b]: continue # XX: nessun pixel di questo bin sopra weak_grad mask_b = ((bins == b) & valid).astype(np.uint8) - if self.use_gpu: + if r == 0: + d_np = mask_b + elif self.use_gpu: d = cv2.dilate(cv2.UMat(mask_b), kernel) d_np = d.get() else: @@ -828,111 +874,9 @@ class LineShapeMatcher: oy = float(np.clip(oy, -0.5, 0.5)) return x + ox, y + oy - def _refine_pose_joint( - self, - spread0: np.ndarray, - template_gray: np.ndarray, - cx: float, cy: float, - angle_deg: float, scale: float, - mask_full: np.ndarray, - max_iter: int = 24, - tol: float = 1e-3, - ) -> tuple[float, float, float, float]: - """Refine congiunto (cx, cy, angle) via Nelder-Mead 3D. - - Ottimizza simultaneamente posizione e angolo (vs golden search 1D - sull'angolo poi quadratico 2D su xy che alterna assi). Halcon-style: - un singolo iter LM stila il match a precisione sub-pixel + sub-step. - Ritorna (angle, score, cx, cy) dove score e quello calcolato sulla - scena spread (no template gray). - """ - h, w = template_gray.shape - sw = max(16, int(round(w * scale))) - sh = max(16, int(round(h * scale))) - gray_s = cv2.resize(template_gray, (sw, sh), interpolation=cv2.INTER_LINEAR) - mask_s = cv2.resize(mask_full, (sw, sh), interpolation=cv2.INTER_NEAREST) - diag = int(np.ceil(np.hypot(sh, sw))) + 6 - py = (diag - sh) // 2; px = (diag - sw) // 2 - gray_p = cv2.copyMakeBorder(gray_s, py, diag - sh - py, px, diag - sw - px, - cv2.BORDER_REPLICATE) - mask_p = cv2.copyMakeBorder(mask_s, py, diag - sh - py, px, diag - sw - px, - cv2.BORDER_CONSTANT, value=0) - center = (diag / 2.0, diag / 2.0) - H, W = spread0.shape - - def _score(params: tuple[float, float, float]) -> float: - ddx, ddy, dang = params - ang = angle_deg + dang - M = cv2.getRotationMatrix2D(center, ang, 1.0) - gray_r = cv2.warpAffine(gray_p, M, (diag, diag), - flags=cv2.INTER_LINEAR, - borderMode=cv2.BORDER_REPLICATE) - mask_r = cv2.warpAffine(mask_p, M, (diag, diag), - flags=cv2.INTER_NEAREST, borderValue=0) - mag, bins = self._gradient(gray_r) - fx, fy, fb = self._extract_features(mag, bins, mask_r) - if len(fx) < 8: - return 0.0 - cxe = cx + ddx; cye = cy + ddy - ix = int(round(cxe)); iy = int(round(cye)) - tot = 0 - valid = 0 - for i in range(len(fx)): - xs = ix + int(fx[i] - center[0]) - ys = iy + int(fy[i] - center[1]) - if 0 <= xs < W and 0 <= ys < H: - bit = np.uint8(1 << int(fb[i])) - if spread0[ys, xs] & bit: - tot += 1 - valid += 1 - return -float(tot) / max(1, valid) # minimize -score - - # Nelder-Mead 3D inline (no scipy). Simplex iniziale: vertice + offset - # dx=±0.5px, dy=±0.5px, dθ=±step/2. - step_a = self.angle_step_deg / 2.0 if self.angle_step_deg > 0 else 1.0 - x0 = np.array([0.0, 0.0, 0.0]) - simplex = np.array([ - x0, - x0 + [0.5, 0.0, 0.0], - x0 + [0.0, 0.5, 0.0], - x0 + [0.0, 0.0, step_a], - ]) - fvals = np.array([_score(tuple(s)) for s in simplex]) - for _ in range(max_iter): - order = np.argsort(fvals) - simplex = simplex[order]; fvals = fvals[order] - if abs(fvals[-1] - fvals[0]) < tol: - break - centroid = simplex[:-1].mean(axis=0) - xr = centroid + 1.0 * (centroid - simplex[-1]) - fr = _score(tuple(xr)) - if fvals[0] <= fr < fvals[-2]: - simplex[-1] = xr; fvals[-1] = fr - continue - if fr < fvals[0]: - xe = centroid + 2.0 * (centroid - simplex[-1]) - fe = _score(tuple(xe)) - if fe < fr: - simplex[-1] = xe; fvals[-1] = fe - else: - simplex[-1] = xr; fvals[-1] = fr - continue - xc = centroid + 0.5 * (simplex[-1] - centroid) - fc = _score(tuple(xc)) - if fc < fvals[-1]: - simplex[-1] = xc; fvals[-1] = fc - continue - for k in range(1, 4): - simplex[k] = simplex[0] + 0.5 * (simplex[k] - simplex[0]) - fvals[k] = _score(tuple(simplex[k])) - best_i = int(np.argmin(fvals)) - ddx, ddy, dang = simplex[best_i] - return (angle_deg + float(dang), -float(fvals[best_i]), - cx + float(ddx), cy + float(ddy)) - def _refine_angle( self, - spread0: np.ndarray, # bitmap uint8 (H, W) + spread0: np.ndarray, # bitmap uint8/uint16 (H, W) - spread pieno bit_active: int, template_gray: np.ndarray, cx: float, cy: float, @@ -941,33 +885,31 @@ class LineShapeMatcher: angle_fine_step: float = 0.5, search_radius: float | None = None, original_score: float | None = None, + spread_fine: np.ndarray | None = None, ) -> tuple[float, float, float, float]: """Ricerca angolare fine (sub-step) attorno al match grezzo. - Genera 5 template temporanei a angle ± {0.5, 1.0} * step e sceglie - l'angolo con score massimo (parabolic fit sulle 3 score centrali). + Golden-section sull'angolo + argmax posizione in finestra ±3 px. Ritorna (angle_refined, score, cx_refined, cy_refined). + + L'ottimizzazione gira sulla bitmap FINE (spread_fine, raggio 1): + sulla bitmap dilatata (spread0, raggio 4-5) lo score satura a 1.0 + per qualunque posa entro ±spread px / ±atan(spread/R) gradi e il + refine non vede alcun gradiente (l'angolo restava quello grezzo + quantizzato e cv2.minMaxLoc sul plateau saturo spostava il centro + sull'angolo in alto a sinistra della finestra: errore misurato + 3·sqrt(2) ≈ 4.24 px). Lo score RITORNATO e' ricalcolato alla posa + raffinata su spread0, per mantenere la semantica precedente + (tolleranza spread_radius) su soglie/min_score. """ - # NB: rimosso early-skip su score >= 0.99. Lo score linemod/shape - # satura facilmente a 1.0 (specie con pyramid_propagate o spread - # ampio) ma NON garantisce angolo preciso: l'angolo grezzo della - # variante e' quantizzato a multipli di angle_step (5 deg default). - # Refine angolare e' essenziale per orientamento sub-step. if search_radius is None: search_radius = self._effective_angle_step() + # Bitmap su cui ottimizzare: fine se disponibile, altrimenti spread0. + opt_map = spread_fine if spread_fine is not None else spread0 - h, w = template_gray.shape - sw = max(16, int(round(w * scale))) - sh = max(16, int(round(h * scale))) - gray_s = cv2.resize(template_gray, (sw, sh), interpolation=cv2.INTER_LINEAR) - mask_s = cv2.resize(mask_full, (sw, sh), interpolation=cv2.INTER_NEAREST) - diag = int(np.ceil(np.hypot(sh, sw))) + 6 - py = (diag - sh) // 2; px = (diag - sw) // 2 - gray_p = cv2.copyMakeBorder(gray_s, py, diag - sh - py, px, diag - sw - px, - cv2.BORDER_REPLICATE) - mask_p = cv2.copyMakeBorder(mask_s, py, diag - sh - py, px, diag - sw - px, - cv2.BORDER_CONSTANT, value=0) - center = (diag / 2.0, diag / 2.0) + gray_p, mask_p, center, diag = self._prepare_padded_template( + template_gray, mask_full, scale, + ) H, W = spread0.shape margin = 3 @@ -982,35 +924,44 @@ class LineShapeMatcher: feat_cache = self._refine_feat_cache cache_scale_key = round(scale * 1000) - def _score_at_angle(off: float) -> tuple[float, float, float]: - """Ritorna (score, best_cx, best_cy) per angolo = angle_deg + off.""" - ang = angle_deg + off + def _feats_at_angle(ang: float): ck = (round(ang * 20), cache_scale_key) cached = feat_cache.get(ck) if cached is not None: - fx, fy, fb = cached - else: - M = cv2.getRotationMatrix2D(center, ang, 1.0) - gray_r = cv2.warpAffine(gray_p, M, (diag, diag), - flags=cv2.INTER_LINEAR, - borderMode=cv2.BORDER_REPLICATE) - mask_r = cv2.warpAffine(mask_p, M, (diag, diag), - flags=cv2.INTER_NEAREST, borderValue=0) - mag, bins = self._gradient(gray_r) - fx, fy, fb = self._extract_features(mag, bins, mask_r) - # LRU semplice: limita cache a ~256 angoli (8 angoli * 32 candidati) - if len(feat_cache) > 256: - feat_cache.pop(next(iter(feat_cache))) - feat_cache[ck] = (fx, fy, fb) + return cached + M = cv2.getRotationMatrix2D(center, ang, 1.0) + gray_r = cv2.warpAffine(gray_p, M, (diag, diag), + flags=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_REPLICATE) + mask_r = cv2.warpAffine(mask_p, M, (diag, diag), + flags=cv2.INTER_NEAREST, borderValue=0) + mag, bins = self._gradient(gray_r) + fx, fy, fb = self._extract_features(mag, bins, mask_r) + # LRU semplice: limita cache a ~256 angoli (8 angoli * 32 candidati) + if len(feat_cache) > 256: + feat_cache.pop(next(iter(feat_cache))) + feat_cache[ck] = (fx, fy, fb) + return fx, fy, fb + + def _score_at_angle(off: float) -> tuple[float, float, float]: + """Ritorna (score, best_cx, best_cy) per angolo = angle_deg + off. + + Score = max su finestra ±margin px attorno a (cx, cy) sulla + bitmap di ottimizzazione; posizione = picco sub-pixel della + finestra (centroide plateau / fit quadratico, NON minMaxLoc + che sul plateau e' biased verso l'angolo top-left). + """ + ang = angle_deg + off + fx, fy, fb = _feats_at_angle(ang) if len(fx) < 8: return (0.0, cx, cy) - dx = (fx - center[0]).astype(np.int32) - dy = (fy - center[1]).astype(np.int32) + dx = np.round(fx - center[0]).astype(np.int32) + dy = np.round(fy - center[1]).astype(np.int32) y_lo = int(cy) - margin; y_hi = int(cy) + margin + 1 x_lo = int(cx) - margin; x_hi = int(cx) + margin + 1 sh_w = y_hi - y_lo; sw_w = x_hi - x_lo acc = np.zeros((sh_w, sw_w), dtype=np.float32) - spread_dtype = spread0.dtype.type + spread_dtype = opt_map.dtype.type for i in range(len(dx)): ddx = int(dx[i]); ddy = int(dy[i]); b = int(fb[i]) bit = spread_dtype(1 << b) @@ -1021,14 +972,19 @@ class LineShapeMatcher: s_y0 = max(0, sy0); s_y1 = min(H, sy1) s_x0 = max(0, sx0); s_x1 = min(W, sx1) if s_y1 > s_y0 and s_x1 > s_x0: - region = spread0[s_y0:s_y1, s_x0:s_x1] + region = opt_map[s_y0:s_y1, s_x0:s_x1] acc[a_y0:a_y1, a_x0:a_x1] += ( (region & bit) != 0 ).astype(np.float32) acc /= len(dx) _, max_val, _, max_loc = cv2.minMaxLoc(acc) - return (float(max_val), - float(x_lo + max_loc[0]), float(y_lo + max_loc[1])) + if max_val <= 0.0: + return (0.0, cx, cy) + # Picco sub-pixel dentro la finestra (gestisce plateau e fit 3x3) + px_f, py_f = self._subpixel_peak( + acc, int(max_loc[0]), int(max_loc[1]), plateau_radius=margin, + ) + return (float(max_val), float(x_lo + px_f), float(y_lo + py_f)) # Golden-section search su [-search_radius, +search_radius]: # converge in log tempo a precisione ~0.1°, ~8 valutazioni vs 5 @@ -1064,7 +1020,25 @@ class LineShapeMatcher: x1 = x2; s1 = s2; cx1 = cx2; cy1 = cy2 x2 = a_lo + _GOLDEN * (a_hi - a_lo) s2, cx2, cy2 = _score_at_angle(x2) - return best + + ang_best, s_best, cx_best, cy_best = best + if spread_fine is None: + return best + # Score finale alla posa raffinata sullo spread COARSE: stessa + # semantica dello score pre-refine (tolleranza spread_radius), + # cosi' min_score/verify mantengono il significato di prima. + fx, fy, fb = _feats_at_angle(ang_best) + if len(fx) < 8: + return best + xs = np.round(fx - center[0]).astype(np.int32) + int(round(cx_best)) + ys = np.round(fy - center[1]).astype(np.int32) + int(round(cy_best)) + ok = (xs >= 0) & (xs < W) & (ys >= 0) & (ys < H) + if not ok.any(): + return (ang_best, 0.0, cx_best, cy_best) + bits = spread0[ys[ok], xs[ok]].astype(np.int32) + hit = (bits & np.left_shift(1, fb[ok].astype(np.int32))) != 0 + score_coarse = float(hit.sum()) / len(fx) + return (ang_best, score_coarse, cx_best, cy_best) def _get_view_template( self, view_idx: int, @@ -1089,26 +1063,13 @@ class LineShapeMatcher: """ if self.template_gray is None: return 1.0 - h, w = self.template_gray.shape - scale = variant.scale - sw = max(16, int(round(w * scale))) - sh = max(16, int(round(h * scale))) - gray_s = cv2.resize(self.template_gray, (sw, sh), interpolation=cv2.INTER_LINEAR) mask_src = ( self._train_mask if self._train_mask is not None else np.full_like(self.template_gray, 255) ) - mask_s = cv2.resize(mask_src, (sw, sh), interpolation=cv2.INTER_NEAREST) - diag = int(np.ceil(np.hypot(sh, sw))) + 6 - py = (diag - sh) // 2; px = (diag - sw) // 2 - gray_p = cv2.copyMakeBorder( - gray_s, py, diag - sh - py, px, diag - sw - px, cv2.BORDER_REPLICATE, + gray_p, mask_p, center, diag = self._prepare_padded_template( + self.template_gray, mask_src, variant.scale, ) - mask_p = cv2.copyMakeBorder( - mask_s, py, diag - sh - py, px, diag - sw - px, - cv2.BORDER_CONSTANT, value=0, - ) - center = (diag / 2.0, diag / 2.0) M = cv2.getRotationMatrix2D(center, angle_deg, 1.0) gray_r = cv2.warpAffine(gray_p, M, (diag, diag), flags=cv2.INTER_LINEAR, @@ -1125,8 +1086,8 @@ class LineShapeMatcher: ix = int(round(cx)); iy = int(round(cy)) hits = 0 for i in range(n_feat): - xs = ix + int(fx[i] - center[0]) - ys = iy + int(fy[i] - center[1]) + xs = ix + int(round(fx[i] - center[0])) + ys = iy + int(round(fy[i] - center[1])) if 0 <= xs < W and 0 <= ys < H: bit = spread_dtype(1 << int(fb[i])) if spread0[ys, xs] & bit: @@ -1140,26 +1101,13 @@ class LineShapeMatcher: """Soft-margin gradient similarity (Halcon Metric='use_polarity').""" if self.template_gray is None: return 0.0 - h, w = self.template_gray.shape - scale = variant.scale - sw = max(16, int(round(w * scale))) - sh = max(16, int(round(h * scale))) - gray_s = cv2.resize(self.template_gray, (sw, sh), interpolation=cv2.INTER_LINEAR) mask_src = ( self._train_mask if self._train_mask is not None else np.full_like(self.template_gray, 255) ) - mask_s = cv2.resize(mask_src, (sw, sh), interpolation=cv2.INTER_NEAREST) - diag = int(np.ceil(np.hypot(sh, sw))) + 6 - py = (diag - sh) // 2; px = (diag - sw) // 2 - gray_p = cv2.copyMakeBorder( - gray_s, py, diag - sh - py, px, diag - sw - px, cv2.BORDER_REPLICATE, + gray_p, mask_p, center, diag = self._prepare_padded_template( + self.template_gray, mask_src, variant.scale, ) - mask_p = cv2.copyMakeBorder( - mask_s, py, diag - sh - py, px, diag - sw - px, - cv2.BORDER_CONSTANT, value=0, - ) - center = (diag / 2.0, diag / 2.0) M = cv2.getRotationMatrix2D(center, angle_deg, 1.0) gray_r = cv2.warpAffine(gray_p, M, (diag, diag), flags=cv2.INTER_LINEAR, @@ -1179,8 +1127,8 @@ class LineShapeMatcher: ix = int(round(cx)); iy = int(round(cy)) sims = []; weights = [] for i in range(len(fx)): - xs = ix + int(fx[i] - center[0]) - ys = iy + int(fy[i] - center[1]) + xs = ix + int(round(fx[i] - center[0])) + ys = iy + int(round(fy[i] - center[1])) if not (0 <= xs < W and 0 <= ys < H): continue tx = float(gx_t[int(fy[i]), int(fx[i])]) @@ -1201,35 +1149,34 @@ class LineShapeMatcher: def _subpixel_refine_lm( self, scene_gray: np.ndarray, variant: _Variant, cx: float, cy: float, angle_deg: float, - n_iters: int = 2, - ) -> tuple[float, float]: - """Sub-pixel refinement iterativo via gradient-field least-squares. + n_iters: int = 4, + scene_grad: tuple[np.ndarray, np.ndarray] | None = None, + ) -> tuple[float, float, float]: + """Refinement least-squares congiunto di posizione E angolo. - Halcon-equivalent SubPixel='least_squares_high'. Precisione attesa - 0.05 px (vs 0.5 px del fit quadratic 2D). + Halcon-equivalent SubPixel='least_squares_high'. Per ogni feature + template cerca il picco sub-pixel del gradiente scena lungo la + normale dell'edge (snap ±2 px, fit parabolico su 5 campioni), poi + risolve ai minimi quadrati pesati il sistema 3x3 in (dx, dy, dθ): + + n_i · (d + dθ·u_i) = t_i, u_i = (r_y,i, -r_x,i) + + dove r_i = offset feature dal centro, n_i = normale edge template, + t_i = offset del picco lungo n_i, u_i = derivata della rotazione + nella convenzione cv2.getRotationMatrix2D (R = [[c,s],[-s,c]]). + Tra le iterazioni offset e normali vengono ruotati analiticamente + (no re-warp del template). Precisione attesa <0.1 px / <0.1°. + + scene_grad: (gx, gy) Sobel della scena precomputati (evita un + Sobel full-frame per ogni match). Ritorna (cx, cy, angle_deg). """ - if self.template_gray is None: - return cx, cy - h, w = self.template_gray.shape - scale = variant.scale - sw = max(16, int(round(w * scale))) - sh = max(16, int(round(h * scale))) - gray_s = cv2.resize(self.template_gray, (sw, sh), interpolation=cv2.INTER_LINEAR) - mask_src = ( - self._train_mask if self._train_mask is not None - else np.full_like(self.template_gray, 255) + t, train_mask = self._get_view_template(getattr(variant, "view_idx", 0)) + if t is None: + return cx, cy, angle_deg + mask_src = train_mask if train_mask is not None else np.full_like(t, 255) + gray_p, mask_p, center, diag = self._prepare_padded_template( + t, mask_src, variant.scale, ) - mask_s = cv2.resize(mask_src, (sw, sh), interpolation=cv2.INTER_NEAREST) - diag = int(np.ceil(np.hypot(sh, sw))) + 6 - py = (diag - sh) // 2; px = (diag - sw) // 2 - gray_p = cv2.copyMakeBorder( - gray_s, py, diag - sh - py, px, diag - sw - px, cv2.BORDER_REPLICATE, - ) - mask_p = cv2.copyMakeBorder( - mask_s, py, diag - sh - py, px, diag - sw - px, - cv2.BORDER_CONSTANT, value=0, - ) - center = (diag / 2.0, diag / 2.0) M = cv2.getRotationMatrix2D(center, angle_deg, 1.0) gray_r = cv2.warpAffine(gray_p, M, (diag, diag), flags=cv2.INTER_LINEAR, @@ -1241,51 +1188,98 @@ class LineShapeMatcher: mag_t = cv2.magnitude(gx_t, gy_t) _, bins_t = self._gradient(gray_r) fx, fy, _ = self._extract_features(mag_t, bins_t, mask_r) - if len(fx) < 4: - return cx, cy - n = len(fx) - ddx_t = (fx - center[0]).astype(np.float32) - ddy_t = (fy - center[1]).astype(np.float32) - gx_tf = np.array([gx_t[int(fy[i]), int(fx[i])] for i in range(n)], dtype=np.float32) - gy_tf = np.array([gy_t[int(fy[i]), int(fx[i])] for i in range(n)], dtype=np.float32) - mag_tf = np.hypot(gx_tf, gy_tf) + 1e-6 - nx_t = gx_tf / mag_tf - ny_t = gy_tf / mag_tf - gx_s = cv2.Sobel(scene_gray, cv2.CV_32F, 1, 0, ksize=3) - gy_s = cv2.Sobel(scene_gray, cv2.CV_32F, 0, 1, ksize=3) + if len(fx) < 8: + return cx, cy, angle_deg + rx = (fx - center[0]).astype(np.float64) + ry = (fy - center[1]).astype(np.float64) + gxf = gx_t[fy, fx].astype(np.float64) + gyf = gy_t[fy, fx].astype(np.float64) + nm = np.hypot(gxf, gyf) + 1e-9 + nx = gxf / nm + ny = gyf / nm + + if scene_grad is not None: + gx_s, gy_s = scene_grad + else: + gx_s = cv2.Sobel(scene_gray, cv2.CV_32F, 1, 0, ksize=3) + gy_s = cv2.Sobel(scene_gray, cv2.CV_32F, 0, 1, ksize=3) H, W = scene_gray.shape - cur_cx, cur_cy = float(cx), float(cy) - for _ in range(n_iters): - xs = cur_cx + ddx_t - ys = cur_cy + ddy_t - xs_c = np.clip(xs, 0, W - 1.001) - ys_c = np.clip(ys, 0, H - 1.001) + + def _bilin(g: np.ndarray, xs: np.ndarray, ys: np.ndarray) -> np.ndarray: + xs_c = np.clip(xs, 0.0, W - 1.001) + ys_c = np.clip(ys, 0.0, H - 1.001) x0 = xs_c.astype(np.int32); y0 = ys_c.astype(np.int32) ax = xs_c - x0; ay = ys_c - y0 - def _bilin(g): - v00 = g[y0, x0]; v10 = g[y0, x0 + 1] - v01 = g[y0 + 1, x0]; v11 = g[y0 + 1, x0 + 1] - return ((1 - ax) * (1 - ay) * v00 - + ax * (1 - ay) * v10 - + (1 - ax) * ay * v01 - + ax * ay * v11) - sx_v = _bilin(gx_s) - sy_v = _bilin(gy_s) - mag_s = np.hypot(sx_v, sy_v) + 1e-6 - nx_s = sx_v / mag_s - ny_s = sy_v / mag_s - w = np.minimum(mag_s, 255.0).astype(np.float32) - err_x = (nx_s - nx_t) * w - err_y = (ny_s - ny_t) * w - step_x = -float(err_x.sum()) / (w.sum() + 1e-6) - step_y = -float(err_y.sum()) / (w.sum() + 1e-6) - step_x = max(-1.0, min(1.0, step_x)) - step_y = max(-1.0, min(1.0, step_y)) - cur_cx += step_x - cur_cy += step_y - if abs(step_x) < 0.02 and abs(step_y) < 0.02: + return ((1 - ax) * (1 - ay) * g[y0, x0] + + ax * (1 - ay) * g[y0, x0 + 1] + + (1 - ax) * ay * g[y0 + 1, x0] + + ax * ay * g[y0 + 1, x0 + 1]) + + t_offsets = np.array([-2.0, -1.0, 0.0, 1.0, 2.0]) + n_feat = len(rx) + idx = np.arange(n_feat) + cur_cx, cur_cy, cur_ang = float(cx), float(cy), float(angle_deg) + for _ in range(n_iters): + px = cur_cx + rx + py = cur_cy + ry + # |grad| scena campionato a 5 offset lungo la normale di ogni + # feature; il picco sub-pixel lungo la normale e' la distanza + # firmata t_i dall'edge scena piu' vicino. + mags = np.empty((5, n_feat)) + sxs = np.empty((5, n_feat)) + sys_ = np.empty((5, n_feat)) + for k, t_off in enumerate(t_offsets): + sx_v = _bilin(gx_s, px + t_off * nx, py + t_off * ny) + sy_v = _bilin(gy_s, px + t_off * nx, py + t_off * ny) + sxs[k] = sx_v; sys_[k] = sy_v + mags[k] = np.hypot(sx_v, sy_v) + k_best = np.argmax(mags, axis=0) + m_pk = mags[k_best, idx] + t_i = t_offsets[k_best] + # Fit parabolico sui picchi interni (k in 1..3) + interior = (k_best >= 1) & (k_best <= 3) + if interior.any(): + ki = k_best[interior]; ii = idx[interior] + m_m = mags[ki - 1, ii]; m_0 = mags[ki, ii]; m_p = mags[ki + 1, ii] + denom = (m_m - 2.0 * m_0 + m_p) + off = np.where(np.abs(denom) > 1e-9, + 0.5 * (m_m - m_p) / (denom - 1e-12), 0.0) + t_i = t_i.astype(np.float64) + t_i[interior] += np.clip(off, -0.5, 0.5) + # Peso: |grad| al picco * allineamento direzione (mod π se no + # polarity). Feature senza edge (sotto weak_grad) escluse; + # picco sul bordo finestra = snap inaffidabile → dimezzato. + sx_pk = sxs[k_best, idx]; sy_pk = sys_[k_best, idx] + cos_al = (nx * sx_pk + ny * sy_pk) / (m_pk + 1e-9) + align = np.maximum(0.0, cos_al) if self.use_polarity else np.abs(cos_al) + wgt = np.minimum(m_pk, 255.0) * align * align + wgt[m_pk < self.weak_grad] = 0.0 + wgt[~interior] *= 0.5 + if float(wgt.sum()) < 1e-6: break - return cur_cx, cur_cy + # LSQ pesato 3x3: A_i = [n_x, n_y, n_x·r_y - n_y·r_x] + a3 = nx * ry - ny * rx + A = np.stack([nx, ny, a3], axis=1) + Aw = A * wgt[:, None] + AtA = Aw.T @ A + Atb = Aw.T @ t_i.astype(np.float64) + try: + sol = np.linalg.solve(AtA + 1e-6 * np.eye(3), Atb) + except np.linalg.LinAlgError: + break + ddx = float(np.clip(sol[0], -1.5, 1.5)) + ddy = float(np.clip(sol[1], -1.5, 1.5)) + dth = float(np.clip(sol[2], -math.radians(1.5), math.radians(1.5))) + cur_cx += ddx + cur_cy += ddy + cur_ang += math.degrees(dth) + # Ruota offset e normali di dθ (convenzione R = [[c,s],[-s,c]]) + c = math.cos(dth); s = math.sin(dth) + rx, ry = c * rx + s * ry, -s * rx + c * ry + nx, ny = c * nx + s * ny, -s * nx + c * ny + if abs(ddx) < 0.01 and abs(ddy) < 0.01 and abs(dth) < 1.7e-4: + break + return cur_cx, cur_cy, cur_ang def _verify_ncc( self, scene_gray: np.ndarray, cx: float, cy: float, @@ -1370,15 +1364,21 @@ class LineShapeMatcher: coarse_stride: int = 1, scale_penalty: float = 0.0, search_roi: tuple[int, int, int, int] | None = None, - pyramid_propagate: bool = False, # off di default: meno duplicati - propagate_topk: int = 4, - refine_pose_joint: bool = False, + # ON di default: full-res valutato solo in finestre locali attorno + # ai picchi top-level (costo ∝ candidati, non varianti × W × H). + # I duplicati che avevano fatto disattivare questa modalita' sono + # gestiti dalla NMS IoU poligonale post-refine. + pyramid_propagate: bool = True, + propagate_topk: int = 8, + refine_pose_joint: bool = False, # deprecato: alias di refine_angle greediness: float = 0.0, batch_top: bool = False, nms_iou_threshold: float = 0.3, min_recall: float = 0.0, use_soft_score: bool = False, - subpixel_lm: bool = False, + # ON di default: least-squares finale (posizione + angolo) sui + # gradienti scena, precisione attesa <0.1 px / <0.1°. + subpixel_lm: bool = True, debug: bool = False, profile: bool = False, ) -> list[Match]: @@ -1462,7 +1462,7 @@ class LineShapeMatcher: cached = self._scene_cache_get(cache_key) if cache_key else None if cached is not None: grays, spread_top, bit_active_top, density_top, spread0, \ - bit_active_full, density_full, top = cached + bit_active_full, density_full, top, spread_fine = cached else: grays = [gray0] for _ in range(self.pyramid_levels - 1): @@ -1478,6 +1478,7 @@ class LineShapeMatcher: spread0 = None bit_active_full = None density_full = None + spread_fine = None _checkpoint("spread_top") if nms_radius is None: nms_radius = max(8, min(self.template_size) // 2) @@ -1488,6 +1489,18 @@ class LineShapeMatcher: # coarse_angle_factor (skip 1 ogni 2): con step fine non e' utile. # Risultato osservato: precisione "veloce" 10° dava risultati # migliori di "preciso" 2° proprio perche evitava il pruning. + # Il path windowed (pyramid_propagate) assume che il picco + # top-level localizzi la posizione entro il margine finestra. + # Su template ALLUNGATI (es. lama 40x280, o ROI parziale lungo un + # asse) lo score top-level ha un plateau lungo l'asse e il picco + # puo' essere lontano decine di px dal centro vero → le finestre + # tagliano fuori la posa giusta e il match muore in verify NCC. + # In quel caso si usa il full-scan esatto (costo maggiore ma + # nessuna perdita di recall). + if pyramid_propagate and self.template_size != (0, 0): + tw_t, th_t = self.template_size + if max(tw_t, th_t) / max(1, min(tw_t, th_t)) > 2.0: + pyramid_propagate = False eff_step = self._effective_angle_step() top_factor = self.top_score_factor cf_eff = max(1, coarse_angle_factor) @@ -1598,16 +1611,25 @@ class LineShapeMatcher: return vi, -1.0 best = float(score.max()) if pyramid_propagate and best > 0: - flat = score.ravel() - k = min(propagate_topk, flat.size) - idx = np.argpartition(-flat, k - 1)[:k] + # Picchi = MASSIMI LOCALI sopra soglia, non top-K pixel: + # su template allungati lo score top-level ha plateau + # estesi e i top-K pixel si concentrano tutti sulle 2-3 + # istanze piu' forti, perdendo per sempre le altre. + # Soglia permissiva (0.5x): un picco scartato qui = + # istanza persa, un picco in piu' = solo una finestra + # extra di costo marginale (dedup via mark). + thr = top_thresh * 0.5 + dil = cv2.dilate(score, np.ones((5, 5), np.uint8)) + ys_l, xs_l = np.nonzero((score >= dil) & (score >= thr)) peaks: list[tuple[int, int, float]] = [] - for i in idx: - s = float(flat[i]) - if s < top_thresh * 0.7: - continue - yt, xt = int(i // score.shape[1]), int(i % score.shape[1]) - peaks.append((xt, yt, s)) + if len(ys_l): + vals = score[ys_l, xs_l] + k = min(max(propagate_topk, 2 * max_matches), len(vals)) + sel = np.argpartition(-vals, k - 1)[:k] + peaks = [ + (int(xs_l[i]), int(ys_l[i]), float(vals[i])) + for i in sel + ] peaks_by_vi[vi] = peaks return vi, best @@ -1664,6 +1686,13 @@ class LineShapeMatcher: expanded.add(vi_n) # Usa lo score del coarse come stima per il sort successivo score_by_vi[vi_n] = max(score_by_vi.get(vi_n, 0.0), s_top) + # Propaga i picchi top-level del coarse anche ai vicini: + # l'oggetto e' nella stessa posizione (angolo ±step), quindi + # anche i vicini possono usare il path windowed invece del + # full-scan dell'intera scena (che dominava il costo full-res). + if (pyramid_propagate and vi_n != vi_c + and peaks_by_vi.get(vi_c)): + peaks_by_vi.setdefault(vi_n, []).extend(peaks_by_vi[vi_c]) kept_variants: list[tuple[int, float]] = [ (vi, score_by_vi[vi]) for vi in expanded ] @@ -1690,54 +1719,63 @@ class LineShapeMatcher: if (spread0 & (spread0.dtype.type(1) << b)).any()) ) density_full = _jit_popcount(spread0) + # Bitmap fine (raggio 1) per il refine: sulla bitmap dilatata + # lo score satura e il refine angolare/posizionale non vede + # alcun gradiente (vedi _refine_angle). + spread_fine = self._spread_bitmap(gray0, radius=1) # Salva cache scena complete if cache_key is not None: self._scene_cache_put(cache_key, ( grays, spread_top, bit_active_top, density_top, - spread0, bit_active_full, density_full, top, + spread0, bit_active_full, density_full, top, spread_fine, )) for sc in unique_scales: bg_cache_full[sc] = _bg_for_scale(density_full, sc, 1) # Margine in full-res attorno ad ogni peak top: copre incertezza - # downsampling (sf_top px) + spread_radius + slack per NMS. - propagate_margin = sf_top + self.spread_radius + max(8, nms_radius // 2) + # downsampling (2·sf_top px) + plateau radius del subpixel (10) + + # slack. NON serve includere nms_radius: la NMS lavora sui candidati + # estratti, non richiede score validi oltre il plateau del picco. + propagate_margin = 2 * sf_top + max(10, self.spread_radius) + 6 H_full, W_full = spread0.shape def _full_score(vi: int) -> tuple[int, np.ndarray]: var = self.variants[vi] lvl0 = var.levels[0] - if not pyramid_propagate or vi not in peaks_by_vi or not peaks_by_vi[vi]: + peaks = peaks_by_vi.get(vi) if pyramid_propagate else None + margin = propagate_margin + if not peaks: # Path legacy: scansiona intera scena return vi, _jit_score_bitmap_rescored( spread0, lvl0.dx, lvl0.dy, lvl0.bin, bit_active_full, bg_cache_full[var.scale], ) - # Path piramide propagata: valuta solo crop locali attorno - # alle posizioni dei picchi top-level (riproiettati a full-res). + # Path piramide propagata: valuta solo finestre locali attorno + # ai picchi top-level (riproiettati a full-res). Il kernel + # windowed campiona lo spread dell'INTERA scena: chiamare il + # kernel su un crop trattava le feature fuori-crop come miss + # (template raggio > finestra → score ~0 ovunque, 0 match). score_full = np.zeros((H_full, W_full), dtype=np.float32) mark = np.zeros((H_full, W_full), dtype=bool) bg = bg_cache_full[var.scale] - for xt, yt, _s in peaks_by_vi[vi]: + for xt, yt, _s in peaks: cx0 = xt * sf_top cy0 = yt * sf_top - x_lo = max(0, cx0 - propagate_margin) - x_hi = min(W_full, cx0 + propagate_margin + 1) - y_lo = max(0, cy0 - propagate_margin) - y_hi = min(H_full, cy0 + propagate_margin + 1) + x_lo = max(0, cx0 - margin) + x_hi = min(W_full, cx0 + margin + 1) + y_lo = max(0, cy0 - margin) + y_hi = min(H_full, cy0 + margin + 1) if x_hi <= x_lo or y_hi <= y_lo: continue if mark[y_lo:y_hi, x_lo:x_hi].all(): continue - # Crop spread + bg, valuta kernel sul crop - spread_crop = np.ascontiguousarray(spread0[y_lo:y_hi, x_lo:x_hi]) - bg_crop = np.ascontiguousarray(bg[y_lo:y_hi, x_lo:x_hi]) - score_crop = _jit_score_bitmap_rescored( - spread_crop, lvl0.dx, lvl0.dy, lvl0.bin, - bit_active_full, bg_crop, + score_win = _jit_score_bitmap_rescored_window( + spread0, lvl0.dx, lvl0.dy, lvl0.bin, + bit_active_full, bg, + y_lo, x_lo, y_hi - y_lo, x_hi - x_lo, ) score_full[y_lo:y_hi, x_lo:x_hi] = np.maximum( - score_full[y_lo:y_hi, x_lo:x_hi], score_crop, + score_full[y_lo:y_hi, x_lo:x_hi], score_win, ) mark[y_lo:y_hi, x_lo:x_hi] = True return vi, score_full @@ -1811,6 +1849,14 @@ class LineShapeMatcher: # Subpixel + refine + verify solo sui candidati pre-NMS (max pre_cap) kept: list[Match] = [] tw, th = self.template_size + # Sobel scena precomputato una volta per il refine LM (prima era + # un Sobel full-frame per OGNI match). + scene_grad = None + if subpixel_lm and self.template_gray is not None and preliminary_int: + scene_grad = ( + cv2.Sobel(gray0, cv2.CV_32F, 1, 0, ksize=3), + cv2.Sobel(gray0, cv2.CV_32F, 0, 1, ksize=3), + ) for score, xi, yi, vi in preliminary_int: if subpixel and vi in score_maps: cx_f, cy_f = self._subpixel_peak( @@ -1821,12 +1867,10 @@ class LineShapeMatcher: var = self.variants[vi] ang_f = var.angle_deg score_f = score - if refine_pose_joint and self.template_gray is not None: - ang_f, score_f, cx_f, cy_f = self._refine_pose_joint( - spread0, self.template_gray, cx_f, cy_f, - var.angle_deg, var.scale, mask_full, - ) - elif refine_angle and self.template_gray is not None: + # refine_pose_joint (Nelder-Mead) rimosso: valutava lo score a + # posizioni intere su bitmap satura (funzione a gradini piatta, + # il simplex terminava subito). Ora e' alias del refine standard. + if (refine_angle or refine_pose_joint) and self.template_gray is not None: ang_f, score_f, cx_f, cy_f = self._refine_angle( spread0, bit_active_full, self.template_gray, cx_f, cy_f, var.angle_deg, var.scale, mask_full, @@ -1835,14 +1879,15 @@ class LineShapeMatcher: # del bin angolare della variante grezza. search_radius=self._effective_angle_step(), original_score=score, + spread_fine=spread_fine, ) - # Halcon SubPixel='least_squares_high': refinement iterativo - # gradient-field per precisione 0.05 px (vs 0.5 quadratic 2D). + # Halcon SubPixel='least_squares_high': least-squares finale + # (posizione + angolo) sui gradienti scena, <0.1 px / <0.1°. if subpixel_lm and self.template_gray is not None: - cx_lm, cy_lm = self._subpixel_refine_lm( - gray0, var, cx_f, cy_f, ang_f, + cx_lm, cy_lm, ang_lm = self._subpixel_refine_lm( + gray0, var, cx_f, cy_f, ang_f, scene_grad=scene_grad, ) - cx_f, cy_f = float(cx_lm), float(cy_lm) + cx_f, cy_f, ang_f = float(cx_lm), float(cy_lm), float(ang_lm) # NCC verify (Halcon-style): se ncc_skip_above < 1.0 salta # il calcolo per shape-score gia alti. Default 1.01 = NCC sempre, # piu sicuro contro falsi positivi (lo shape-score satura facile).