simittag 0.1.0__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- marker/__init__.py +0 -0
- marker/generate.py +122 -0
- marker/svg.py +159 -0
- simittag/__init__.py +1 -0
- simittag/board.py +159 -0
- simittag/calibrate.py +129 -0
- simittag/cli.py +97 -0
- simittag/codec.py +215 -0
- simittag/detect.py +864 -0
- simittag/gf256.py +247 -0
- simittag/payload.py +170 -0
- simittag/pose.py +139 -0
- simittag/spec.py +216 -0
- simittag-0.1.0.dist-info/METADATA +263 -0
- simittag-0.1.0.dist-info/RECORD +18 -0
- simittag-0.1.0.dist-info/WHEEL +4 -0
- simittag-0.1.0.dist-info/entry_points.txt +2 -0
- simittag-0.1.0.dist-info/licenses/LICENSE +26 -0
marker/__init__.py
ADDED
|
File without changes
|
marker/generate.py
ADDED
|
@@ -0,0 +1,122 @@
|
|
|
1
|
+
"""
|
|
2
|
+
Render a Simittag marker to a numpy array / PNG.
|
|
3
|
+
|
|
4
|
+
Geometry and cell ordering come from simittag.spec / simittag.codec, so the image
|
|
5
|
+
is guaranteed consistent with what the detector expects.
|
|
6
|
+
|
|
7
|
+
Convention (shared with detector): pixel (x=col, y=row); marker centered;
|
|
8
|
+
dx=x-cx, dy=y-cy; theta = atan2(dy, dx) mod 2*pi; sector = floor(theta/step);
|
|
9
|
+
radius normalized so the outer ring's outer edge = 1.0.
|
|
10
|
+
"""
|
|
11
|
+
from __future__ import annotations
|
|
12
|
+
|
|
13
|
+
import argparse
|
|
14
|
+
import numpy as np
|
|
15
|
+
|
|
16
|
+
from simittag.spec import MarkerSpec, DEFAULT, VARIANTS
|
|
17
|
+
from simittag import codec, payload as _payload
|
|
18
|
+
|
|
19
|
+
|
|
20
|
+
def render(payload: bytes, spec: MarkerSpec = DEFAULT, size: int = 1024,
|
|
21
|
+
supersample: int = 4, margin: float = 0.12,
|
|
22
|
+
tile_rows: int = 64, inverted: bool = False) -> np.ndarray:
|
|
23
|
+
"""
|
|
24
|
+
Return a (size, size) uint8 grayscale image (255=white, 0=black).
|
|
25
|
+
margin: white quiet-zone fraction added around the marker (radius 1.0).
|
|
26
|
+
inverted: white marker foreground on a black background.
|
|
27
|
+
"""
|
|
28
|
+
if size <= 0:
|
|
29
|
+
raise ValueError("size must be positive")
|
|
30
|
+
if supersample <= 0:
|
|
31
|
+
raise ValueError("supersample must be positive")
|
|
32
|
+
if not (0.0 <= margin < 1.0):
|
|
33
|
+
raise ValueError("margin must be in [0, 1)")
|
|
34
|
+
if tile_rows <= 0:
|
|
35
|
+
raise ValueError("tile_rows must be positive")
|
|
36
|
+
|
|
37
|
+
grid = codec.encode(payload, spec)
|
|
38
|
+
S = size * supersample
|
|
39
|
+
# radius 1.0 maps to (1 - margin) of the half-extent, leaving a quiet border
|
|
40
|
+
cx = cy = (S - 1) / 2.0
|
|
41
|
+
R_px = (S / 2.0) * (1.0 - margin)
|
|
42
|
+
|
|
43
|
+
# Work in horizontal tiles. The original full-frame implementation kept
|
|
44
|
+
# several SxS float64 arrays alive together and peaked around 1.4 GB RSS at
|
|
45
|
+
# the default 1024x / 4x supersampling. A binary supersample is only SxS
|
|
46
|
+
# bytes; temporary coordinate arrays are bounded by tile_rows x S.
|
|
47
|
+
img = np.full((S, S), 255, dtype=np.uint8)
|
|
48
|
+
dx = (np.arange(S, dtype=np.float64)[None, :] - cx) / R_px
|
|
49
|
+
step = 2 * np.pi / spec.SECTOR_COUNT
|
|
50
|
+
ring_w = (spec.R_DATA_OUT - spec.R_DATA_IN) / spec.RING_COUNT
|
|
51
|
+
|
|
52
|
+
for y0 in range(0, S, tile_rows):
|
|
53
|
+
y1 = min(S, y0 + tile_rows)
|
|
54
|
+
dy = (np.arange(y0, y1, dtype=np.float64)[:, None] - cy) / R_px
|
|
55
|
+
r = np.sqrt(dx * dx + dy * dy)
|
|
56
|
+
theta = np.mod(np.arctan2(dy, dx), 2 * np.pi)
|
|
57
|
+
tile = img[y0:y1]
|
|
58
|
+
|
|
59
|
+
# outer ring and bullseye
|
|
60
|
+
tile[(r >= spec.R_RING_IN) & (r <= 1.0)] = 0
|
|
61
|
+
tile[r <= spec.R_BULLSEYE] = 0
|
|
62
|
+
|
|
63
|
+
# data cells
|
|
64
|
+
data_mask = (r >= spec.R_DATA_IN) & (r < spec.R_DATA_OUT)
|
|
65
|
+
ring_idx = np.clip(((r - spec.R_DATA_IN) / ring_w).astype(int),
|
|
66
|
+
0, spec.RING_COUNT - 1)
|
|
67
|
+
sec_idx = np.clip((theta / step).astype(int), 0, spec.SECTOR_COUNT - 1)
|
|
68
|
+
cell_val = grid[ring_idx, sec_idx] # 1 => black
|
|
69
|
+
tile[data_mask & (cell_val == 1)] = 0
|
|
70
|
+
|
|
71
|
+
# downsample (anti-alias)
|
|
72
|
+
result = img.reshape(size, supersample, size, supersample).mean(axis=(1, 3)).astype(np.uint8)
|
|
73
|
+
return 255 - result if inverted else result
|
|
74
|
+
|
|
75
|
+
|
|
76
|
+
def save_png(arr: np.ndarray, path: str):
|
|
77
|
+
# OpenCV is already a detector dependency; using it here avoids making
|
|
78
|
+
# Pillow an undeclared generation-only dependency.
|
|
79
|
+
import cv2
|
|
80
|
+
if not cv2.imwrite(path, arr):
|
|
81
|
+
raise OSError(f"could not write PNG: {path}")
|
|
82
|
+
|
|
83
|
+
|
|
84
|
+
def _parse_payload(s: str, n: int) -> bytes:
|
|
85
|
+
if s.startswith("0x"):
|
|
86
|
+
v = int(s, 16)
|
|
87
|
+
return v.to_bytes(n, "big")
|
|
88
|
+
b = s.encode()
|
|
89
|
+
if len(b) > n:
|
|
90
|
+
raise ValueError(f"payload string too long ({len(b)} > {n} bytes)")
|
|
91
|
+
return b.ljust(n, b"\x00")
|
|
92
|
+
|
|
93
|
+
|
|
94
|
+
if __name__ == "__main__":
|
|
95
|
+
ap = argparse.ArgumentParser(description="Render a Simittag marker")
|
|
96
|
+
ap.add_argument("--variant", default="M", choices=list(VARIANTS),
|
|
97
|
+
help="T=tracking 3x16, M=balanced 4x24, D=data 5x36")
|
|
98
|
+
ap.add_argument("--id", type=lambda s: int(s, 0), default=None,
|
|
99
|
+
help="ID-mode integer (e.g. 0x1234); fills the payload")
|
|
100
|
+
ap.add_argument("--payload", default=None,
|
|
101
|
+
help="raw payload: hex (0x...) or text (fills payload bytes)")
|
|
102
|
+
ap.add_argument("--out", default="marker.png")
|
|
103
|
+
ap.add_argument("--size", type=int, default=1024)
|
|
104
|
+
ap.add_argument("--inverted", action="store_true",
|
|
105
|
+
help="render white foreground on a black background")
|
|
106
|
+
args = ap.parse_args()
|
|
107
|
+
|
|
108
|
+
sp = VARIANTS[args.variant]
|
|
109
|
+
if args.payload is not None:
|
|
110
|
+
payload = _parse_payload(args.payload, sp.payload_bytes)
|
|
111
|
+
desc = f"payload={payload.hex()}"
|
|
112
|
+
else:
|
|
113
|
+
# default to an ID so every variant has a sensible no-arg render
|
|
114
|
+
idval = args.id if args.id is not None else 0x1234
|
|
115
|
+
payload = _payload.encode_id(idval, sp)
|
|
116
|
+
desc = f"id=0x{idval:x}"
|
|
117
|
+
arr = render(payload, sp, size=args.size, inverted=args.inverted)
|
|
118
|
+
save_png(arr, args.out)
|
|
119
|
+
# confirm it decodes from its own grid (sanity, not validation)
|
|
120
|
+
back, _ = codec.decode(codec.encode(payload, sp), sp)
|
|
121
|
+
print(f"wrote {args.out} ({args.size}x{args.size}) variant={sp.NAME} {desc} "
|
|
122
|
+
f"self-decode={'OK' if back == payload else 'FAIL'}")
|
marker/svg.py
ADDED
|
@@ -0,0 +1,159 @@
|
|
|
1
|
+
"""
|
|
2
|
+
Vector (SVG) renderer for Simittag markers, for clean printing at an exact physical
|
|
3
|
+
size. Geometry matches marker/generate.py (the raster renderer) cell-for-cell, which
|
|
4
|
+
in turn matches the detector's sampling convention (simittag.spec / simittag.codec):
|
|
5
|
+
|
|
6
|
+
angle: theta = atan2(y, x) in screen coords (y DOWN), sector s spans [s*step,(s+1)*step)
|
|
7
|
+
radius: normalized so the outer ring's outer edge = 1.0
|
|
8
|
+
ring r spans [R_DATA_IN + r*ring_w, R_DATA_IN + (r+1)*ring_w), grid[r,s]==1 => black
|
|
9
|
+
|
|
10
|
+
Painter's order (no even-odd needed): white bg -> black outer disk -> white disk at
|
|
11
|
+
R_RING_IN (carves the ring) -> black bullseye -> black data cells.
|
|
12
|
+
"""
|
|
13
|
+
from __future__ import annotations
|
|
14
|
+
import math
|
|
15
|
+
|
|
16
|
+
from simittag.spec import MarkerSpec, DEFAULT
|
|
17
|
+
from simittag import codec
|
|
18
|
+
|
|
19
|
+
|
|
20
|
+
def _pt(cx, cy, rho, ang, R):
|
|
21
|
+
return (cx + R * rho * math.cos(ang), cy + R * rho * math.sin(ang))
|
|
22
|
+
|
|
23
|
+
|
|
24
|
+
def _sector_path(cx, cy, ri, ro, a0, a1, R):
|
|
25
|
+
"""Annular-sector path (one data cell), radii normalized, angles in radians."""
|
|
26
|
+
x0o, y0o = _pt(cx, cy, ro, a0, R)
|
|
27
|
+
x1o, y1o = _pt(cx, cy, ro, a1, R)
|
|
28
|
+
x1i, y1i = _pt(cx, cy, ri, a1, R)
|
|
29
|
+
x0i, y0i = _pt(cx, cy, ri, a0, R)
|
|
30
|
+
Ro, Ri = ro * R, ri * R
|
|
31
|
+
return (f"M{x0o:.3f},{y0o:.3f} A{Ro:.3f},{Ro:.3f} 0 0 1 {x1o:.3f},{y1o:.3f} "
|
|
32
|
+
f"L{x1i:.3f},{y1i:.3f} A{Ri:.3f},{Ri:.3f} 0 0 0 {x0i:.3f},{y0i:.3f} Z")
|
|
33
|
+
|
|
34
|
+
|
|
35
|
+
def marker_svg_body(grid, spec: MarkerSpec, cx, cy, R):
|
|
36
|
+
"""SVG element string for one marker centered at (cx,cy) with outer radius R (units)."""
|
|
37
|
+
step = 2 * math.pi / spec.SECTOR_COUNT
|
|
38
|
+
ring_w = (spec.R_DATA_OUT - spec.R_DATA_IN) / spec.RING_COUNT
|
|
39
|
+
out = []
|
|
40
|
+
# black outer disk, then white disk to leave the outer ring annulus
|
|
41
|
+
out.append(f'<circle cx="{cx}" cy="{cy}" r="{R:.3f}" fill="#000"/>')
|
|
42
|
+
out.append(f'<circle cx="{cx}" cy="{cy}" r="{R*spec.R_RING_IN:.3f}" fill="#fff"/>')
|
|
43
|
+
# bullseye
|
|
44
|
+
out.append(f'<circle cx="{cx}" cy="{cy}" r="{R*spec.R_BULLSEYE:.3f}" fill="#000"/>')
|
|
45
|
+
# data cells
|
|
46
|
+
paths = []
|
|
47
|
+
for ring in range(spec.RING_COUNT):
|
|
48
|
+
ri = spec.R_DATA_IN + ring * ring_w
|
|
49
|
+
ro = ri + ring_w
|
|
50
|
+
for s in range(spec.SECTOR_COUNT):
|
|
51
|
+
if grid[ring, s] == 1:
|
|
52
|
+
paths.append(_sector_path(cx, cy, ri, ro, s * step, (s + 1) * step, R))
|
|
53
|
+
if paths:
|
|
54
|
+
out.append(f'<path d="{" ".join(paths)}" fill="#000"/>')
|
|
55
|
+
return "\n".join(out)
|
|
56
|
+
|
|
57
|
+
|
|
58
|
+
def marker_svg(payload: bytes, spec: MarkerSpec = DEFAULT, size_mm: float = 40.0,
|
|
59
|
+
margin: float = 0.12, label: str = None) -> str:
|
|
60
|
+
"""
|
|
61
|
+
Standalone SVG (sized in mm) for one marker. `size_mm` is the marker diameter
|
|
62
|
+
(the outer ring's outer edge); a white quiet zone of `margin` is added around it.
|
|
63
|
+
Optional `label` text is drawn under the quiet zone.
|
|
64
|
+
"""
|
|
65
|
+
grid = codec.encode(payload, spec)
|
|
66
|
+
R = size_mm / 2.0
|
|
67
|
+
pad = R * margin
|
|
68
|
+
W = size_mm + 2 * pad
|
|
69
|
+
cx = cy = W / 2.0
|
|
70
|
+
label_h = max(3.0, size_mm * 0.10) if label else 0.0
|
|
71
|
+
H = W + label_h
|
|
72
|
+
parts = [
|
|
73
|
+
f'<svg xmlns="http://www.w3.org/2000/svg" width="{W:.2f}mm" height="{H:.2f}mm" '
|
|
74
|
+
f'viewBox="0 0 {W:.3f} {H:.3f}">',
|
|
75
|
+
f'<rect x="0" y="0" width="{W:.3f}" height="{H:.3f}" fill="#fff"/>',
|
|
76
|
+
marker_svg_body(grid, spec, cx, cy, R),
|
|
77
|
+
]
|
|
78
|
+
if label:
|
|
79
|
+
fs = label_h * 0.62
|
|
80
|
+
parts.append(
|
|
81
|
+
f'<text x="{cx:.3f}" y="{W + label_h*0.72:.3f}" font-family="monospace" '
|
|
82
|
+
f'font-size="{fs:.2f}" text-anchor="middle" fill="#000">{_esc(label)}</text>')
|
|
83
|
+
parts.append("</svg>")
|
|
84
|
+
return "\n".join(parts)
|
|
85
|
+
|
|
86
|
+
|
|
87
|
+
def _esc(s):
|
|
88
|
+
return (str(s).replace("&", "&").replace("<", "<").replace(">", ">"))
|
|
89
|
+
|
|
90
|
+
|
|
91
|
+
# ---- printable sheet ----
|
|
92
|
+
PAGES_MM = {"A4": (210.0, 297.0), "Letter": (215.9, 279.4)}
|
|
93
|
+
|
|
94
|
+
|
|
95
|
+
def sheet_svg(items, page="A4", size_mm=40.0, margin_mm=12.0, gap_mm=8.0,
|
|
96
|
+
cut_marks=True):
|
|
97
|
+
"""
|
|
98
|
+
Lay markers out on a printable page (SVG sized in mm). `items` = list of
|
|
99
|
+
(payload_bytes, spec, label). Auto-flows into a grid; returns one SVG string.
|
|
100
|
+
Print at 100% scale (no fit-to-page) for the physical size to be exact.
|
|
101
|
+
"""
|
|
102
|
+
pw, ph = PAGES_MM.get(page, PAGES_MM["A4"])
|
|
103
|
+
cell = size_mm * 1.12 # marker + its 12% quiet zone
|
|
104
|
+
label_h = max(3.0, size_mm * 0.10)
|
|
105
|
+
cell_h = cell + label_h + gap_mm
|
|
106
|
+
cell_w = cell + gap_mm
|
|
107
|
+
cols = max(1, int((pw - 2 * margin_mm + gap_mm) // cell_w))
|
|
108
|
+
rows = max(1, int((ph - 2 * margin_mm + gap_mm) // cell_h))
|
|
109
|
+
per_page = cols * rows
|
|
110
|
+
|
|
111
|
+
parts = [
|
|
112
|
+
f'<svg xmlns="http://www.w3.org/2000/svg" width="{pw}mm" height="{ph}mm" '
|
|
113
|
+
f'viewBox="0 0 {pw} {ph}">',
|
|
114
|
+
f'<rect x="0" y="0" width="{pw}" height="{ph}" fill="#fff"/>',
|
|
115
|
+
]
|
|
116
|
+
# only the first page (browsers print one SVG page; multi-page is a future nicety)
|
|
117
|
+
for i, (payload, spec, label) in enumerate(items[:per_page]):
|
|
118
|
+
r, c = divmod(i, cols)
|
|
119
|
+
x0 = margin_mm + c * cell_w
|
|
120
|
+
y0 = margin_mm + r * cell_h
|
|
121
|
+
grid = codec.encode(payload, spec)
|
|
122
|
+
R = size_mm / 2.0
|
|
123
|
+
cx = x0 + cell / 2.0
|
|
124
|
+
cy = y0 + cell / 2.0
|
|
125
|
+
if cut_marks:
|
|
126
|
+
parts.append(_cut_marks(x0, y0, cell, cell))
|
|
127
|
+
parts.append(f'<g>{marker_svg_body(grid, spec, cx, cy, R)}</g>')
|
|
128
|
+
fs = label_h * 0.6
|
|
129
|
+
parts.append(
|
|
130
|
+
f'<text x="{cx:.3f}" y="{y0 + cell + label_h*0.7:.3f}" '
|
|
131
|
+
f'font-family="monospace" font-size="{fs:.2f}" text-anchor="middle" '
|
|
132
|
+
f'fill="#000">{_esc(label)}</text>')
|
|
133
|
+
extra = max(0, len(items) - per_page)
|
|
134
|
+
if extra:
|
|
135
|
+
parts.append(
|
|
136
|
+
f'<text x="{pw/2:.1f}" y="{ph-4:.1f}" font-family="monospace" '
|
|
137
|
+
f'font-size="3.5" text-anchor="middle" fill="#888">'
|
|
138
|
+
f'+{extra} more (reduce size or count to fit one page)</text>')
|
|
139
|
+
parts.append("</svg>")
|
|
140
|
+
return "\n".join(parts)
|
|
141
|
+
|
|
142
|
+
|
|
143
|
+
def _cut_marks(x, y, w, h, m=2.0):
|
|
144
|
+
L = []
|
|
145
|
+
for (px, py, dx, dy) in [(x, y, 1, 0), (x, y, 0, 1),
|
|
146
|
+
(x + w, y, -1, 0), (x + w, y, 0, 1),
|
|
147
|
+
(x, y + h, 1, 0), (x, y + h, 0, -1),
|
|
148
|
+
(x + w, y + h, -1, 0), (x + w, y + h, 0, -1)]:
|
|
149
|
+
L.append(f'<line x1="{px:.2f}" y1="{py:.2f}" x2="{px+dx*m:.2f}" '
|
|
150
|
+
f'y2="{py+dy*m:.2f}" stroke="#bbb" stroke-width="0.2"/>')
|
|
151
|
+
return "".join(L)
|
|
152
|
+
|
|
153
|
+
|
|
154
|
+
if __name__ == "__main__":
|
|
155
|
+
from simittag import payload as _p
|
|
156
|
+
svg = marker_svg(_p.encode_id(0x1234, DEFAULT), DEFAULT, size_mm=40,
|
|
157
|
+
label="SIMITTAG-M 0x1234")
|
|
158
|
+
open("/tmp/marker.svg", "w").write(svg)
|
|
159
|
+
print("wrote /tmp/marker.svg", len(svg), "bytes")
|
simittag/__init__.py
ADDED
|
@@ -0,0 +1 @@
|
|
|
1
|
+
__version__ = "0.1.0"
|
simittag/board.py
ADDED
|
@@ -0,0 +1,159 @@
|
|
|
1
|
+
"""
|
|
2
|
+
Calibration board definitions.
|
|
3
|
+
|
|
4
|
+
A board is a set of markers at known planar positions (mm), used to solve for
|
|
5
|
+
camera intrinsics (see simittag.calibrate). Board coordinates: origin at the
|
|
6
|
+
center of tag 0 (top-left), x right, y down — matching image conventions.
|
|
7
|
+
|
|
8
|
+
Boards are self-describing: each printed sheet carries one D-variant tag whose
|
|
9
|
+
RAW payload encodes the board parameters, so the calibrator can configure
|
|
10
|
+
itself from the sheet alone. The studio (web/) also writes a JSON sidecar with
|
|
11
|
+
explicit per-tag positions; when available it is the preferred source of truth
|
|
12
|
+
(it also locates the descriptor tag itself, adding one more point per view).
|
|
13
|
+
|
|
14
|
+
Descriptor RAW payload (8 bytes, version 1):
|
|
15
|
+
|
|
16
|
+
[0] version (1)
|
|
17
|
+
[1] family (1 = grid, 2 = multiscale)
|
|
18
|
+
[2:4] pitch in 0.1 mm, big-endian (grid: cell pitch; multiscale: frame step)
|
|
19
|
+
[4:6] tag diameter in 0.1 mm, big-endian
|
|
20
|
+
[6] rows (grid: rows; multiscale: side tags per column)
|
|
21
|
+
[7] cols (grid: cols; multiscale: tags per top/bottom row)
|
|
22
|
+
|
|
23
|
+
Layout algorithms per family are frozen here and mirrored in web/js/calib.js;
|
|
24
|
+
a descriptor fully determines every tag position.
|
|
25
|
+
"""
|
|
26
|
+
from __future__ import annotations
|
|
27
|
+
import json
|
|
28
|
+
from dataclasses import dataclass, field
|
|
29
|
+
|
|
30
|
+
DESCRIPTOR_VERSION = 1
|
|
31
|
+
FAMILY_GRID = 1
|
|
32
|
+
FAMILY_MULTISCALE = 2
|
|
33
|
+
FAMILY_NAMES = {FAMILY_GRID: "grid", FAMILY_MULTISCALE: "multiscale"}
|
|
34
|
+
|
|
35
|
+
# multiscale layout constants (frozen, v1): the center anchor is an M tag with
|
|
36
|
+
# this id, ANCHOR_RATIO times the perimeter tag diameter; side tags are spaced
|
|
37
|
+
# SIDE_STEP_RATIO times the top/bottom step.
|
|
38
|
+
MULTISCALE_ANCHOR_ID = 500
|
|
39
|
+
ANCHOR_RATIO = 3.5
|
|
40
|
+
SIDE_STEP_RATIO = 1.6
|
|
41
|
+
|
|
42
|
+
|
|
43
|
+
@dataclass
|
|
44
|
+
class BoardTag:
|
|
45
|
+
"""One marker at a known board position (center, mm)."""
|
|
46
|
+
variant: str # "T" | "M" | "D"
|
|
47
|
+
mode: str # "ID" | "RAW"
|
|
48
|
+
value: object # int for ID, bytes for RAW
|
|
49
|
+
x_mm: float
|
|
50
|
+
y_mm: float
|
|
51
|
+
diameter_mm: float
|
|
52
|
+
|
|
53
|
+
|
|
54
|
+
@dataclass
|
|
55
|
+
class Board:
|
|
56
|
+
family: int
|
|
57
|
+
pitch_mm: float
|
|
58
|
+
diameter_mm: float
|
|
59
|
+
rows: int
|
|
60
|
+
cols: int
|
|
61
|
+
tags: list = field(default_factory=list) # [BoardTag]
|
|
62
|
+
|
|
63
|
+
def point_for(self, variant, mode, value):
|
|
64
|
+
"""Board (x, y) for a detection, or None if it is not on this board."""
|
|
65
|
+
for t in self.tags:
|
|
66
|
+
if t.variant == variant and t.mode == mode and t.value == value:
|
|
67
|
+
return (t.x_mm, t.y_mm)
|
|
68
|
+
return None
|
|
69
|
+
|
|
70
|
+
@property
|
|
71
|
+
def descriptor_raw(self) -> bytes:
|
|
72
|
+
return pack_descriptor(self.family, self.pitch_mm, self.diameter_mm,
|
|
73
|
+
self.rows, self.cols)
|
|
74
|
+
|
|
75
|
+
|
|
76
|
+
def pack_descriptor(family, pitch_mm, diameter_mm, rows, cols) -> bytes:
|
|
77
|
+
p, d = round(pitch_mm * 10), round(diameter_mm * 10)
|
|
78
|
+
if not (0 < p < 65536 and 0 < d < 65536 and 0 < rows < 256 and 0 < cols < 256):
|
|
79
|
+
raise ValueError("board parameters out of descriptor range")
|
|
80
|
+
return bytes([DESCRIPTOR_VERSION, family, p >> 8, p & 0xFF,
|
|
81
|
+
d >> 8, d & 0xFF, rows, cols])
|
|
82
|
+
|
|
83
|
+
|
|
84
|
+
def unpack_descriptor(raw: bytes) -> dict:
|
|
85
|
+
if len(raw) < 8 or raw[0] != DESCRIPTOR_VERSION:
|
|
86
|
+
raise ValueError(f"not a v{DESCRIPTOR_VERSION} board descriptor")
|
|
87
|
+
family = raw[1]
|
|
88
|
+
if family not in FAMILY_NAMES:
|
|
89
|
+
raise ValueError(f"unknown board family {family}")
|
|
90
|
+
return {"family": family,
|
|
91
|
+
"pitch_mm": ((raw[2] << 8) | raw[3]) / 10.0,
|
|
92
|
+
"diameter_mm": ((raw[4] << 8) | raw[5]) / 10.0,
|
|
93
|
+
"rows": raw[6], "cols": raw[7]}
|
|
94
|
+
|
|
95
|
+
|
|
96
|
+
def grid_board(pitch_mm, diameter_mm, rows, cols) -> Board:
|
|
97
|
+
"""Uniform rows x cols grid of ID tags, row-major ids from 0.
|
|
98
|
+
Variant T while ids fit in one byte, M beyond."""
|
|
99
|
+
variant = "T" if rows * cols <= 256 else "M"
|
|
100
|
+
b = Board(FAMILY_GRID, pitch_mm, diameter_mm, rows, cols)
|
|
101
|
+
for i in range(rows * cols):
|
|
102
|
+
b.tags.append(BoardTag(variant, "ID", i,
|
|
103
|
+
(i % cols) * pitch_mm, (i // cols) * pitch_mm,
|
|
104
|
+
diameter_mm))
|
|
105
|
+
return b
|
|
106
|
+
|
|
107
|
+
|
|
108
|
+
def multiscale_board(step_mm, diameter_mm, side_rows, top_cols) -> Board:
|
|
109
|
+
"""Perimeter frame of small T tags + one large M anchor in the center.
|
|
110
|
+
Top and bottom rows have `top_cols` tags at `step_mm` pitch; each side has
|
|
111
|
+
`side_rows` tags between them at SIDE_STEP_RATIO * step_mm pitch."""
|
|
112
|
+
b = Board(FAMILY_MULTISCALE, step_mm, diameter_mm, side_rows, top_cols)
|
|
113
|
+
w = (top_cols - 1) * step_mm
|
|
114
|
+
sstep = step_mm * SIDE_STEP_RATIO
|
|
115
|
+
h = (side_rows + 1) * sstep
|
|
116
|
+
i = 0
|
|
117
|
+
for c in range(top_cols): # top + bottom rows
|
|
118
|
+
b.tags.append(BoardTag("T", "ID", i, c * step_mm, 0.0, diameter_mm)); i += 1
|
|
119
|
+
b.tags.append(BoardTag("T", "ID", i, c * step_mm, h, diameter_mm)); i += 1
|
|
120
|
+
for r in range(1, side_rows + 1): # left + right columns
|
|
121
|
+
b.tags.append(BoardTag("T", "ID", i, 0.0, r * sstep, diameter_mm)); i += 1
|
|
122
|
+
b.tags.append(BoardTag("T", "ID", i, w, r * sstep, diameter_mm)); i += 1
|
|
123
|
+
b.tags.append(BoardTag("M", "ID", MULTISCALE_ANCHOR_ID,
|
|
124
|
+
w / 2.0, h / 2.0, diameter_mm * ANCHOR_RATIO))
|
|
125
|
+
return b
|
|
126
|
+
|
|
127
|
+
|
|
128
|
+
def board_from_descriptor(raw: bytes) -> Board:
|
|
129
|
+
d = unpack_descriptor(raw)
|
|
130
|
+
if d["family"] == FAMILY_GRID:
|
|
131
|
+
return grid_board(d["pitch_mm"], d["diameter_mm"], d["rows"], d["cols"])
|
|
132
|
+
return multiscale_board(d["pitch_mm"], d["diameter_mm"], d["rows"], d["cols"])
|
|
133
|
+
|
|
134
|
+
|
|
135
|
+
def load_board(path) -> Board:
|
|
136
|
+
"""Load a studio JSON sidecar (explicit tag positions, preferred)."""
|
|
137
|
+
j = json.load(open(path))
|
|
138
|
+
if j.get("simittag_board") != 1:
|
|
139
|
+
raise ValueError(f"{path} is not a simittag board file")
|
|
140
|
+
family = {v: k for k, v in FAMILY_NAMES.items()}[j["family"]]
|
|
141
|
+
b = Board(family, j["pitch_mm"], j["diameter_mm"], j["rows"], j["cols"])
|
|
142
|
+
for t in j["tags"]:
|
|
143
|
+
value = t["value"]
|
|
144
|
+
if t["mode"] == "RAW":
|
|
145
|
+
value = bytes.fromhex(value)
|
|
146
|
+
b.tags.append(BoardTag(t["variant"], t["mode"], value,
|
|
147
|
+
t["x_mm"], t["y_mm"], t["diameter_mm"]))
|
|
148
|
+
return b
|
|
149
|
+
|
|
150
|
+
|
|
151
|
+
def find_board(detections) -> Board | None:
|
|
152
|
+
"""Reconstruct the board from a descriptor tag among detections, if any."""
|
|
153
|
+
for r in detections:
|
|
154
|
+
if r["variant"] == "D" and r["mode"] == "RAW":
|
|
155
|
+
try:
|
|
156
|
+
return board_from_descriptor(bytes(r["value"]))
|
|
157
|
+
except ValueError:
|
|
158
|
+
continue
|
|
159
|
+
return None
|
simittag/calibrate.py
ADDED
|
@@ -0,0 +1,129 @@
|
|
|
1
|
+
"""
|
|
2
|
+
Camera calibration from simittag boards (see simittag.board).
|
|
3
|
+
|
|
4
|
+
The intrinsics contract mirrors AprilTag's apriltag_detection_info_t: fx, fy,
|
|
5
|
+
cx, cy in pixels (aprilrobotics/apriltag, apriltag_pose.h) — plus the OpenCV
|
|
6
|
+
distortion vector, which AprilTag leaves to the caller. detect.detect() takes
|
|
7
|
+
the same parameters as `K=` / `dist=`; feeding it a saved CameraIntrinsics
|
|
8
|
+
replaces the default 60-degree-FOV guess with measured values, which is what
|
|
9
|
+
turns the reported poses from approximate into metric.
|
|
10
|
+
|
|
11
|
+
Usage:
|
|
12
|
+
intr = calibrate_images(["a.png", "b.png", ...]) # board from sheet
|
|
13
|
+
intr.save("intrinsics.json")
|
|
14
|
+
...
|
|
15
|
+
intr = CameraIntrinsics.load("intrinsics.json")
|
|
16
|
+
detect.detect(gray, K=intr.K, dist=intr.dist_array)
|
|
17
|
+
"""
|
|
18
|
+
from __future__ import annotations
|
|
19
|
+
import json
|
|
20
|
+
from dataclasses import dataclass, field
|
|
21
|
+
|
|
22
|
+
import numpy as np
|
|
23
|
+
import cv2
|
|
24
|
+
|
|
25
|
+
from . import detect as _detect
|
|
26
|
+
from . import board as _board
|
|
27
|
+
|
|
28
|
+
MIN_POINTS_PER_VIEW = 6
|
|
29
|
+
MIN_VIEWS = 4
|
|
30
|
+
|
|
31
|
+
|
|
32
|
+
@dataclass
|
|
33
|
+
class CameraIntrinsics:
|
|
34
|
+
"""Pinhole intrinsics, AprilTag field convention (pixels) + distortion."""
|
|
35
|
+
fx: float
|
|
36
|
+
fy: float
|
|
37
|
+
cx: float
|
|
38
|
+
cy: float
|
|
39
|
+
dist: list = field(default_factory=list) # OpenCV k1 k2 p1 p2 k3
|
|
40
|
+
width: int = 0
|
|
41
|
+
height: int = 0
|
|
42
|
+
rms_px: float = 0.0 # calibration reprojection RMS
|
|
43
|
+
views: int = 0
|
|
44
|
+
|
|
45
|
+
@property
|
|
46
|
+
def K(self) -> np.ndarray:
|
|
47
|
+
return np.array([[self.fx, 0, self.cx],
|
|
48
|
+
[0, self.fy, self.cy],
|
|
49
|
+
[0, 0, 1]], dtype=np.float64)
|
|
50
|
+
|
|
51
|
+
@property
|
|
52
|
+
def dist_array(self) -> np.ndarray:
|
|
53
|
+
return np.array(self.dist, dtype=np.float64)
|
|
54
|
+
|
|
55
|
+
def save(self, path):
|
|
56
|
+
json.dump({"simittag_intrinsics": 1, **self.__dict__},
|
|
57
|
+
open(path, "w"), indent=2)
|
|
58
|
+
|
|
59
|
+
@classmethod
|
|
60
|
+
def load(cls, path) -> "CameraIntrinsics":
|
|
61
|
+
j = json.load(open(path))
|
|
62
|
+
if j.pop("simittag_intrinsics", None) != 1:
|
|
63
|
+
raise ValueError(f"{path} is not a simittag intrinsics file")
|
|
64
|
+
return cls(**j)
|
|
65
|
+
|
|
66
|
+
|
|
67
|
+
def _view_points(detections, board):
|
|
68
|
+
"""Match one image's detections against the board -> (obj Nx3, img Nx2)."""
|
|
69
|
+
obj, img = [], []
|
|
70
|
+
for r in detections:
|
|
71
|
+
value = r["value"]
|
|
72
|
+
if r["mode"] == "RAW":
|
|
73
|
+
value = bytes(value)
|
|
74
|
+
pt = board.point_for(r["variant"], r["mode"], value)
|
|
75
|
+
if pt is not None:
|
|
76
|
+
obj.append([pt[0], pt[1], 0.0])
|
|
77
|
+
img.append(r["center"])
|
|
78
|
+
return (np.array(obj, dtype=np.float32),
|
|
79
|
+
np.array(img, dtype=np.float32))
|
|
80
|
+
|
|
81
|
+
|
|
82
|
+
def calibrate(images, board=None, versions=None) -> CameraIntrinsics:
|
|
83
|
+
"""
|
|
84
|
+
Solve intrinsics (Zhang's method via cv2.calibrateCamera) from grayscale
|
|
85
|
+
images of a simittag calibration board. If `board` is None it is
|
|
86
|
+
reconstructed from the descriptor tag found on the sheet.
|
|
87
|
+
"""
|
|
88
|
+
obj_pts, img_pts = [], []
|
|
89
|
+
size = None
|
|
90
|
+
for gray in images:
|
|
91
|
+
if size is None:
|
|
92
|
+
size = (gray.shape[1], gray.shape[0])
|
|
93
|
+
elif size != (gray.shape[1], gray.shape[0]):
|
|
94
|
+
raise ValueError("all calibration images must share one resolution")
|
|
95
|
+
dets = _detect.detect(gray, versions=versions)
|
|
96
|
+
if board is None:
|
|
97
|
+
board = _board.find_board(dets)
|
|
98
|
+
if board is None:
|
|
99
|
+
continue
|
|
100
|
+
obj, img = _view_points(dets, board)
|
|
101
|
+
if len(obj) >= MIN_POINTS_PER_VIEW:
|
|
102
|
+
obj_pts.append(obj)
|
|
103
|
+
img_pts.append(img)
|
|
104
|
+
if board is None:
|
|
105
|
+
raise ValueError("no board descriptor found — pass board= or use a "
|
|
106
|
+
"sheet with a descriptor tag")
|
|
107
|
+
if len(obj_pts) < MIN_VIEWS:
|
|
108
|
+
raise ValueError(f"only {len(obj_pts)} usable view(s) "
|
|
109
|
+
f"(>= {MIN_POINTS_PER_VIEW} board tags each); "
|
|
110
|
+
f"need at least {MIN_VIEWS}")
|
|
111
|
+
rms, K, dist, _rvecs, _tvecs = cv2.calibrateCamera(
|
|
112
|
+
obj_pts, img_pts, size, None, None)
|
|
113
|
+
return CameraIntrinsics(
|
|
114
|
+
fx=float(K[0, 0]), fy=float(K[1, 1]),
|
|
115
|
+
cx=float(K[0, 2]), cy=float(K[1, 2]),
|
|
116
|
+
dist=[float(v) for v in dist.ravel()],
|
|
117
|
+
width=size[0], height=size[1],
|
|
118
|
+
rms_px=float(rms), views=len(obj_pts))
|
|
119
|
+
|
|
120
|
+
|
|
121
|
+
def calibrate_images(paths, board=None, versions=None) -> CameraIntrinsics:
|
|
122
|
+
"""calibrate() over image files."""
|
|
123
|
+
images = []
|
|
124
|
+
for p in paths:
|
|
125
|
+
gray = cv2.imread(str(p), cv2.IMREAD_GRAYSCALE)
|
|
126
|
+
if gray is None:
|
|
127
|
+
raise ValueError(f"cannot read {p}")
|
|
128
|
+
images.append(gray)
|
|
129
|
+
return calibrate(images, board=board, versions=versions)
|
simittag/cli.py
ADDED
|
@@ -0,0 +1,97 @@
|
|
|
1
|
+
#!/usr/bin/env python3
|
|
2
|
+
"""
|
|
3
|
+
Simittag command line: encode a payload to a marker, decode an image,
|
|
4
|
+
or solve camera intrinsics from calibration-sheet photos.
|
|
5
|
+
|
|
6
|
+
simittag encode --id 12345 --out marker.png
|
|
7
|
+
simittag encode --raw "hi" --out marker.png
|
|
8
|
+
simittag decode image.png
|
|
9
|
+
simittag calibrate img1.png img2.png ... --out intrinsics.json
|
|
10
|
+
simittag decode image.png --intrinsics intrinsics.json
|
|
11
|
+
"""
|
|
12
|
+
from __future__ import annotations
|
|
13
|
+
import argparse
|
|
14
|
+
import cv2
|
|
15
|
+
|
|
16
|
+
from simittag.spec import VARIANTS
|
|
17
|
+
from simittag import payload, detect
|
|
18
|
+
from simittag import board as board_mod
|
|
19
|
+
from simittag.calibrate import CameraIntrinsics, calibrate_images
|
|
20
|
+
from marker.generate import render as render_marker, save_png
|
|
21
|
+
|
|
22
|
+
|
|
23
|
+
def cmd_encode(a):
|
|
24
|
+
sp = VARIANTS[a.variant]
|
|
25
|
+
if a.id is not None:
|
|
26
|
+
pl = payload.encode_id(a.id, sp)
|
|
27
|
+
elif a.raw is not None:
|
|
28
|
+
data = bytes.fromhex(a.raw[2:]) if a.raw.startswith("0x") else a.raw.encode()
|
|
29
|
+
pl = payload.encode_raw(data, sp)
|
|
30
|
+
else:
|
|
31
|
+
raise SystemExit("give --id or --raw")
|
|
32
|
+
arr = render_marker(pl, sp, size=a.size, inverted=a.inverted)
|
|
33
|
+
save_png(arr, a.out)
|
|
34
|
+
print(f"wrote {a.out} payload={pl.hex()} decode={payload.decode(pl, sp)}")
|
|
35
|
+
|
|
36
|
+
|
|
37
|
+
def cmd_decode(a):
|
|
38
|
+
gray = cv2.imread(a.image, cv2.IMREAD_GRAYSCALE)
|
|
39
|
+
if gray is None:
|
|
40
|
+
raise SystemExit(f"cannot read {a.image}")
|
|
41
|
+
versions = None if a.variant == "auto" else a.variant
|
|
42
|
+
K = dist = None
|
|
43
|
+
if a.intrinsics:
|
|
44
|
+
intr = CameraIntrinsics.load(a.intrinsics)
|
|
45
|
+
K, dist = intr.K, intr.dist_array
|
|
46
|
+
res = detect.detect(gray, versions=versions, K=K, dist=dist)
|
|
47
|
+
if not res:
|
|
48
|
+
print("no marker decoded")
|
|
49
|
+
return
|
|
50
|
+
for r in res:
|
|
51
|
+
polarity = "white-on-black" if r["inverted"] else "black-on-white"
|
|
52
|
+
print(f" {r['variant']} {r['mode']}={r['value']} "
|
|
53
|
+
f"center=({r['center'][0]:.0f},{r['center'][1]:.0f}) "
|
|
54
|
+
f"tilt={r['tilt_deg']:.1f}deg {polarity}")
|
|
55
|
+
|
|
56
|
+
|
|
57
|
+
def cmd_calibrate(a):
|
|
58
|
+
board = board_mod.load_board(a.board) if a.board else None
|
|
59
|
+
intr = calibrate_images(a.images, board=board)
|
|
60
|
+
intr.save(a.out)
|
|
61
|
+
print(f"wrote {a.out}")
|
|
62
|
+
print(f" fx={intr.fx:.1f} fy={intr.fy:.1f} cx={intr.cx:.1f} cy={intr.cy:.1f}")
|
|
63
|
+
print(f" dist={['%.4f' % v for v in intr.dist]}")
|
|
64
|
+
print(f" {intr.views} views, reprojection rms {intr.rms_px:.3f} px")
|
|
65
|
+
|
|
66
|
+
|
|
67
|
+
def main():
|
|
68
|
+
ap = argparse.ArgumentParser(prog="simittag")
|
|
69
|
+
sub = ap.add_subparsers(dest="cmd", required=True)
|
|
70
|
+
e = sub.add_parser("encode")
|
|
71
|
+
e.add_argument("--variant", choices=VARIANTS, default="M")
|
|
72
|
+
source = e.add_mutually_exclusive_group(required=True)
|
|
73
|
+
source.add_argument("--id", type=lambda s: int(s, 0))
|
|
74
|
+
source.add_argument("--raw")
|
|
75
|
+
e.add_argument("--out", default="marker.png")
|
|
76
|
+
e.add_argument("--size", type=int, default=1024)
|
|
77
|
+
e.add_argument("--inverted", action="store_true")
|
|
78
|
+
e.set_defaults(fn=cmd_encode)
|
|
79
|
+
d = sub.add_parser("decode")
|
|
80
|
+
d.add_argument("image")
|
|
81
|
+
d.add_argument("--variant", choices=("auto", *VARIANTS), default="auto")
|
|
82
|
+
d.add_argument("--intrinsics", help="intrinsics.json from `calibrate` "
|
|
83
|
+
"(replaces the default 60-degree-FOV guess)")
|
|
84
|
+
d.set_defaults(fn=cmd_decode)
|
|
85
|
+
c = sub.add_parser("calibrate",
|
|
86
|
+
help="solve camera intrinsics from calibration-sheet photos")
|
|
87
|
+
c.add_argument("images", nargs="+")
|
|
88
|
+
c.add_argument("--board", help="board JSON from the studio (default: "
|
|
89
|
+
"reconstruct from the sheet's descriptor tag)")
|
|
90
|
+
c.add_argument("--out", default="intrinsics.json")
|
|
91
|
+
c.set_defaults(fn=cmd_calibrate)
|
|
92
|
+
args = ap.parse_args()
|
|
93
|
+
args.fn(args)
|
|
94
|
+
|
|
95
|
+
|
|
96
|
+
if __name__ == "__main__":
|
|
97
|
+
main()
|