diff --git a/pm2d/line_matcher.py b/pm2d/line_matcher.py index e5f212a..c12c35c 100644 --- a/pm2d/line_matcher.py +++ b/pm2d/line_matcher.py @@ -393,6 +393,108 @@ 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) @@ -574,6 +676,7 @@ class LineShapeMatcher: verify_threshold: float = 0.4, coarse_angle_factor: int = 2, scale_penalty: float = 0.0, + refine_pose_joint: bool = False, ) -> list[Match]: """ scale_penalty: se > 0, riduce lo score per match a scala diversa da 1.0: @@ -798,7 +901,12 @@ class LineShapeMatcher: var = self.variants[vi] ang_f = var.angle_deg score_f = score - if refine_angle and self.template_gray is not None: + 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: 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,