From 0d07b099493a193942442fe1628eb4ba08e04459 Mon Sep 17 00:00:00 2001 From: sl-perception Date: Mon, 2 Mar 2026 11:20:50 -0500 Subject: [PATCH] feat(perception): add person re-ID node (Issue #201) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Two new packages: - saltybot_person_reid_msgs: PersonAppearance + PersonAppearanceArray msgs - saltybot_person_reid: MobileNetV2 torso-crop embedder (128-dim L2-norm) with 128-bin HSV histogram fallback, cosine-similarity gallery with EMA identity updates and configurable age-based pruning, ROS2 node publishing PersonAppearanceArray on /saltybot/person_reid at 5 Hz. Pure-Python helpers (_embedding_model, _reid_gallery) importable without rclpy — 18/18 unit tests pass. Co-Authored-By: Claude Sonnet 4.6 --- .../config/person_reid_params.yaml | 6 + .../src/saltybot_person_reid/package.xml | 28 +++ .../resource/saltybot_person_reid | 0 .../saltybot_person_reid/__init__.py | 0 .../__pycache__/__init__.cpython-314.pyc | Bin 0 -> 207 bytes .../_embedding_model.cpython-314.pyc | Bin 0 -> 5255 bytes .../__pycache__/_reid_gallery.cpython-314.pyc | Bin 0 -> 6096 bytes .../saltybot_person_reid/_embedding_model.py | 95 ++++++++++ .../saltybot_person_reid/_reid_gallery.py | 105 +++++++++++ .../saltybot_person_reid/person_reid_node.py | 174 ++++++++++++++++++ .../src/saltybot_person_reid/setup.cfg | 4 + .../ros2_ws/src/saltybot_person_reid/setup.py | 29 +++ ...t_person_reid.cpython-314-pytest-9.0.2.pyc | Bin 0 -> 32987 bytes .../test/test_person_reid.py | 163 ++++++++++++++++ .../saltybot_person_reid_msgs/CMakeLists.txt | 16 ++ .../msg/PersonAppearance.msg | 7 + .../msg/PersonAppearanceArray.msg | 2 + .../src/saltybot_person_reid_msgs/package.xml | 22 +++ 18 files changed, 651 insertions(+) create mode 100644 jetson/ros2_ws/src/saltybot_person_reid/config/person_reid_params.yaml create mode 100644 jetson/ros2_ws/src/saltybot_person_reid/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_person_reid/resource/saltybot_person_reid create mode 100644 jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/__pycache__/__init__.cpython-314.pyc create mode 100644 jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/__pycache__/_embedding_model.cpython-314.pyc create mode 100644 jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/__pycache__/_reid_gallery.cpython-314.pyc create mode 100644 jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/_embedding_model.py create mode 100644 jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/_reid_gallery.py create mode 100644 jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/person_reid_node.py create mode 100644 jetson/ros2_ws/src/saltybot_person_reid/setup.cfg create mode 100644 jetson/ros2_ws/src/saltybot_person_reid/setup.py create mode 100644 jetson/ros2_ws/src/saltybot_person_reid/test/__pycache__/test_person_reid.cpython-314-pytest-9.0.2.pyc create mode 100644 jetson/ros2_ws/src/saltybot_person_reid/test/test_person_reid.py create mode 100644 jetson/ros2_ws/src/saltybot_person_reid_msgs/CMakeLists.txt create mode 100644 jetson/ros2_ws/src/saltybot_person_reid_msgs/msg/PersonAppearance.msg create mode 100644 jetson/ros2_ws/src/saltybot_person_reid_msgs/msg/PersonAppearanceArray.msg create mode 100644 jetson/ros2_ws/src/saltybot_person_reid_msgs/package.xml diff --git a/jetson/ros2_ws/src/saltybot_person_reid/config/person_reid_params.yaml b/jetson/ros2_ws/src/saltybot_person_reid/config/person_reid_params.yaml new file mode 100644 index 0000000..399aa9b --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_person_reid/config/person_reid_params.yaml @@ -0,0 +1,6 @@ +person_reid: + ros__parameters: + model_path: '' # path to MobileNetV2+projection ONNX file (empty = histogram fallback) + match_threshold: 0.75 # cosine similarity threshold for re-ID match + max_identity_age_s: 300.0 # seconds before unseen identity is pruned + publish_hz: 5.0 # publication rate (Hz) diff --git a/jetson/ros2_ws/src/saltybot_person_reid/package.xml b/jetson/ros2_ws/src/saltybot_person_reid/package.xml new file mode 100644 index 0000000..4db8663 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_person_reid/package.xml @@ -0,0 +1,28 @@ + + + + saltybot_person_reid + 0.1.0 + + Person re-identification node — cross-camera appearance matching using + MobileNetV2 ONNX embeddings (128-dim, cosine similarity gallery). + + SaltyLab + MIT + + rclpy + sensor_msgs + vision_msgs + cv_bridge + message_filters + saltybot_person_reid_msgs + + python3-numpy + python3-opencv + + pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_person_reid/resource/saltybot_person_reid b/jetson/ros2_ws/src/saltybot_person_reid/resource/saltybot_person_reid new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/__init__.py b/jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/__pycache__/__init__.cpython-314.pyc b/jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/__pycache__/__init__.cpython-314.pyc new file mode 100644 index 0000000000000000000000000000000000000000..7006260d95f45602551cd1ba3a50e9c5de83d707 GIT binary patch literal 207 zcmdPq>P{wCAAftgHh(Vb_lhJP_LlF~@{~08CD@VVeD6=fF zBvrp8w?IF$xVSV`*T}$7wTZlX-=wL5i8Jgkn@T`j8DvrjEqIh GKo$Uhm^css literal 0 HcmV?d00001 diff --git a/jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/__pycache__/_embedding_model.cpython-314.pyc b/jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/__pycache__/_embedding_model.cpython-314.pyc new file mode 100644 index 0000000000000000000000000000000000000000..f3ccf6cfd9278b3860d260df25f4129fd1285567 GIT binary patch literal 5255 zcmbtYZ){uD6~E7Z{u4WnlQ^M;Cghfc5VxsAoQ{SRNK2PCslahxI@6)xrx*JrH8{`a zzUQWi(BuyTCHzqWV$?`OWD?Ur+J|mp8r!E$llBRugmg;uRtW}tsnDcE0cGRU&biNa zTsK{jIFj$*bKkx9+;e{C93QUpc?i(xN54)fwS;_$of3&wL)bnGjXaTvLE=ObMucIT zVQa&-ZNzSfymlB4$z?d3$w4C7_Yp}vYO_`wF3ACX=RT6IH{6n~odn%E+{PoT(-Uek znbM}@=}b~hhqANui~O7PWHzfROwkf5Ept%yvcZ&ukzuq;%TQ@_EKHMXRu6_eo+wLA zD{OXv(kC+$sk9nVjaZnDMj{s|@aq|xV3{oa;-xoeZ@B+RGBr(~2p`chY+6aDbTzrV zNyu|rNv9{2#B&3jbt0wF;jtLKn9_~R6jP>Y*YH5^L-cP~=-5Dikj`9GH43NUr>B#! zHBISMxum4h%0yZXd4huJRWvPQC`Kxy>5+sb605XZm9QOwMxH!F3;`r;i#Hf{NiamI z){-O2EiHg_ccti*Tp&?5m#9msfwre8U9VBYCG0cnhDySH5@kF0U(b*wL8{zF49Z?& zpJbV7Sn>E7iAxcs4No(z8Y;-^;KNRI=w;)zC9q56kFb zc}DNiS)xZ*(#GsW#*nih%#0>;K10=wo=|A-$vrH_td-@Wim9?}9=Q40LzRZ?YE$T#vdaZj&q6$4-{L;E)?7WGG1OA#g$bQ zbyW>OHzph{K1$`6CDIOiwG&Fl-K8~7l62rMZKRdYrO0u6D@h3-2$7(}6lF~{Onc%| z*t91#&Ga)>Ndg9(2GvCnFl&Y`C)6w|TF_>Sx|*J3cpM#1+}X(x|4g?mr?iv-65Mx- z1g!}!y%WHjCmT)s=fziRKJzwj+U&mO>n&~Xo_YJsdQ0DGOW)PAYb}FI;#)Oa4pMv9 zf4*^&rq*wW;A=MPn>OuGf2aH5Bd>QnO#b9O>9+sXB|v$TiWVg=Y*W75-YP2DK}``% zMdk!H(>*c8WCXhJJX$uSB^Ab$S<}f>BgZs`q6`YG1}aNg`K_53`V3H?bl}82P~^!D z@jH69uqZsciPig0^B(iT^i3K9Z-N@)+g{-dV?q?_YKoKo3t(QLE-iP^k%J_juh1gA zRTW9Xv7(l>gEmIX`mxKVHsU@ohc~ODU47%L@j6vh$q}^8y?7g}D@huVZdcTl>KBzP zfYxv~eX5)G1!*p&8T}rQ#9yEcmKCp4UIO^l9i)@#jAHbKyKnYR=tCSyV>YV>U7)zvd$5oKM#{d+8VTB1GqAF|p}5Ha9Ng8KO;y^@ zN4a*KgB5wQ*+goa79U-Bv~cp8xAl6U`AYvA{e@HSvER)7YVKMfyiyzf-0NR_c;Vqs zy>uh6Z#~ep8t8g|{~Dj#@CVlY9jpG16(26@`DHXy%5ekS1ZH~{B@Avh z1cIQLFpgK@R0@T%wu)7I=yw$NsG^uns@eUL-W)uC8oZ`gPbg`1l84mnUYIh)X;slor#_=(rN>PXeaTeo)5+tXjeasX zHG8A8*Ztd%2d6&ynA2bca7k1yr}U|MPT9ereX0-X>-dQ&>7#pJ{JP7Zz+__{!+BILvwFkx29>=jE}Hr-vBeIx&3{>C6PW^^s|1 zO0_W0t*gd}mdzP}g&8)Zuq1+_Vi+piMbQyB8H$XdR7?j4Y12NfTsB>>4?;FOj5BUI z9hNaE2)ZnU?7~sx0mx07B5rMpld58vf@V6Pn|MygFru>)LtJV()cbHzTNQ>@qcVq| z8n}L*TyJc?a_Egig|YVzuQi6|UDy4Mi!%!|7D#T?1~&G$UvFsHXy^wh-DtRL(?{y> z-zKhl&%C(VNNRlZ=NCOo>Ob5EUh!UomZT;fF!oZU9+M|V2*Su{T z^-WhiuX_r;zwphUyxz30AQXC+?eFxjHFeAn!9feY!u`wb?;KtUbgl&K#O&+HNC!iRZC!wuGcQNLu0cs3o~FB+$1neEWK)iTSMu%aWbA``%w zCtuYyET&&h7y90_uhZUD+PhX4p0}fYzVPx38@~Gap{0{c&o4bxIJ)8UFEz}c`Ly}| zZHK+yv*RS*#)Su730^t<`srVW-yL{+V6CZh(*@T0jhoc(uT*&m$uV$a==p*kOhPos ztsx2V$oQ8?E{$zS@Se{p>EeuEmM2q8H_|Ch)iN;clI3J3AkRg6s4@tncoe&i5^q0)W zT8vA6c;+Hp`ktX1YV|YlD~E12p<|0cvEbz@gHUjR1Z_9+_?6R8fYF_U3bkx92l*jr8N(38bZEKh z?5t&9s0*wQ3lt6Cy-IIP_al~)KE^PvMXBl`C?L%hge{vx5WlS>Li?Ab@t?#4Zp43g V-n-)*5d?rM+c!Y*CJijIl(q@BoOBawrLd1vjz?p{`mja9%{ z$Q!fbGT%Xx;lkHI6j7+AXOu}bo7HGBl`l&Fn7uBY%;z&hhsAf*(vYAvQ$=OlWFme_2Z6o<>Bl)M%+?99b>eauxEE<8*{UpOMdYXzm1DraN;`C# zaltCu0o}0dsQZpZMbG>@V&qdsMx|6O+F}3Y;qHlyHlY@>mNKEH;rWYj<7`f~>@Zy4 zQcO)V?C_LsDd}9nu+n%u_lP72^#C+Wolu!2BV?RE`U^rb(APqK%1v=HC-ZVZ7GzNl z_6FdH6( zgrhf=+Ieuh64)_Cab-kP6uaJg=u~-k6CBQLhisPICqc1g1G9x6uVZnmz>1G7)>#4~ zl7@Hk6G8yA2`qwdq&&KTiu&PBp*2M?{r@928NJ( z+tN^YTL&~p0*76Gdu3i20g*m>9XJj2EoAg?H*(NTXSw0P2r;}SIq%)2p2O!4H7X!1th4rX;j%}&|^ zE@KOpKCRi|=^P+P&d}2|fy8+Fq-YDKmYtxr*i#3Y9h}zaq?WNc!$jWg-@jjyXfqpe zj%_D=(Q-PNP`wBgv*d0~{k-^_0}FNUx4zrDQr&xN-^!t9SE`?#3*L!UU*CHpc`bP( zbuG2fuw2u%981n|YY%mOaQMB8^9{eh;N zCA9lWC2fIClDulMXB7)~pyWl{Y?0hAObH6kelET7_AI_YtVA$vrE^z?R%Ds^RYmXxQ!*tCVpQ!sQ( zdPPH9m%&B$X}1Hc3g6tQwP=^uobz>5;J~q=V8^lA4`Si>3rv z?-}YM48j1Q@C2;uTIQUQ1#>>FUR2acO)hqrW-}*O6QCW z_!VIP^R!@SV9>W1DHJ$hDOZvhJHOLN@xAB+3A6^t?8LUGb96uKACgIAlA}yH5gzwC zlDP{fHI&$cHfyNu! zMGJb80onl*=_8mOg3QEq0lXaO0%XuZSU}zDwqD)oW!K&V6K2VJgzSF!a$;>)2V@6& z=7ig!rnRQ#IpNoF=hk>W?>_%8=LF>3O_6cVK`%5%v>vicjm%wp0lcAZNBFy?3%Oi& z?0+$kJ_<9B!r$Bt8S-kk*t3Dz{H1j)7E`BK(>b#L;6RKU!_&k|UC@gj(U5l;81RyZ z0Bty4KHDXD$So&96kCv{%*uOfjBuVc?jYxVOFrb=LywL?Ah7c;RpcCB;8Uc)KLgG} zoBnCqCLCBEm4ni^U=lnfb*&$Va+dZAA~i+rd-COhXLSE$Jp` z?@W?mbQu-ED?~N=BmkuZCZr3|83H-&A_O{_l(&V}yO@0#yh*?nNSzI!fmH(LKr+gok7qg`ut zO{;aCOLd(K&#%;_=1#4}s;``R?aX}h?O4ZJZPRLP=TdFw!b`XIE!G}cj2&SIZeL2Y zFC>=}-HV~_Z@;c5@e$6%hxoYWP=6xygKQ~7Wcm~=^1V1a3Pe862E}#^kJ=XiXQ5c^ zJIk66;w(lAfV1HesI`N$P;O74#TZ9y?}qjRafDB^Ggj_|z~}vFWwd>#xXD9#nLFa* zjfYW)9T*oQPJqLK{wuiMD6veW-3Ww`;iO1^guP59f+~E1F*3~WwS{ypU-YaplkXsC zAIP(7n4Oq{o`BhQbGh_x(JS`$&jHOWS*xzUvFF;Jxgf>_@z>+?)hnT0Z^WUs8fsb! zHLZr)mO^cJVs#4*e>(PuW6O^|eLL3wO@I$az;CH)xN6S#FD9hLko2G#$J+z1xDq<> z-}RNC7u}P9W5UsJ(+12Cg1^JdVn$klaT9|pC*Y2tv_$3wR%{*w##wSh>~QI?(3~ab z-EOj@5#9_;^WaAf2|lr3DKns5Xh6X%`i9siBHM!a9SrnQFn*KpKrQ6C{Sa>RU^pM% zG@RugolH8wWO!!dCN~HI(hgfWp%hAq6Hgt1nkyW3z{t(ee&}!{!$mfF9D3}UEt0|D zm%S*e%q+Q6S#!mB-B_+{oeKbyceSoICqHRUmPDZ?^7rQCRdFp*b3?hNtRy-YtoL7i z_tm9D-+ht9YvxXU)|}ixvN_??P}8?x6R4W#UcA?FvPJkf7KD5YuKX|&LFoTp5t#xa zUqm=@-v)u~YMK=fiU_d5!4+_cP(*z1ZD(BYG`fRO879C-!T@|nVEPJT+kso-3a*_L5ZPxxh`=IY_q+K#2#j^#w>VyKgy5HN!A7TM|m0@^wFOz9%UKaV?= zkLZbHjDjo@n#2sGh|mL&*%gYC$)zEX#`u8_V0HpCCIumg`5+>*iu8Hx>%j~akL~S* zHNDux45iJ)$}h4fR|h`vy<-5__o_hu)~iUc@*5tWxfJkf&RRGzSxJ8m z1%5-AD8NTP_=Z4{{Y;SUsQ=wnGbu8IeJ`+sRxuCG2}KR2sARTi6sGfzW8i!aU`O0~ zY&-O%^O@opeG%p$C(IL&tp_-c+u%h`xK~NIjxR{`bJF!WY5juKfSmKu`+SURStpRe U1^DPXHt)9%aNNEU!JJ*@zu!&$OaK4? literal 0 HcmV?d00001 diff --git a/jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/_embedding_model.py b/jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/_embedding_model.py new file mode 100644 index 0000000..d2fb225 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/_embedding_model.py @@ -0,0 +1,95 @@ +""" +_embedding_model.py — Appearance embedding extractor (no ROS2 deps). + +Primary: MobileNetV2 ONNX torso crop → 128-dim L2-normalised embedding. +Fallback: 128-bin HSV histogram (H:16 × S:8) when no model file is available. +""" + +from __future__ import annotations + +import numpy as np +import cv2 + +# Top fraction of the bounding box height used as torso crop +_INPUT_SIZE = (128, 256) # (W, H) fed to MobileNetV2 + + +class EmbeddingModel: + """ + Extract a 128-dim L2-normalised appearance embedding from a BGR crop. + + Parameters + ---------- + model_path : str or None + Path to a MobileNetV2+projection ONNX file. When None (or file + not found), falls back to a 128-bin HSV colour histogram. + """ + + def __init__(self, model_path: str | None = None): + self._net = None + if model_path: + try: + self._net = cv2.dnn.readNetFromONNX(model_path) + except Exception: + pass # histogram fallback + + def embed(self, bgr_crop: np.ndarray) -> np.ndarray: + """ + Parameters + ---------- + bgr_crop : np.ndarray shape (H, W, 3) uint8 + + Returns + ------- + np.ndarray shape (128,) float32, L2-normalised + """ + if bgr_crop.size == 0: + return np.zeros(128, dtype=np.float32) + + if self._net is not None: + return self._mobilenet_embed(bgr_crop) + return self._histogram_embed(bgr_crop) + + # ── MobileNetV2 path ────────────────────────────────────────────────────── + + def _mobilenet_embed(self, bgr: np.ndarray) -> np.ndarray: + resized = cv2.resize(bgr, _INPUT_SIZE) + blob = cv2.dnn.blobFromImage( + resized, + scalefactor=1.0 / 255.0, + size=_INPUT_SIZE, + mean=(0.485 * 255, 0.456 * 255, 0.406 * 255), + swapRB=True, + crop=False, + ) + # Std normalisation: divide channel-wise + blob[:, 0] /= 0.229 + blob[:, 1] /= 0.224 + blob[:, 2] /= 0.225 + + self._net.setInput(blob) + feat = self._net.forward().flatten().astype(np.float32) + + # Ensure 128-dim — average-pool if model output differs + if feat.shape[0] != 128: + n = feat.shape[0] + block = max(1, n // 128) + feat = feat[: block * 128].reshape(128, block).mean(axis=1) + + return _l2_norm(feat) + + # ── HSV histogram fallback ──────────────────────────────────────────────── + + def _histogram_embed(self, bgr: np.ndarray) -> np.ndarray: + """128-bin HSV histogram: 16 H-bins × 8 S-bins, concatenated.""" + hsv = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV) + hist = cv2.calcHist( + [hsv], [0, 1], None, + [16, 8], [0, 180, 0, 256], + ).flatten().astype(np.float32) + return _l2_norm(hist) + + +def _l2_norm(v: np.ndarray) -> np.ndarray: + n = float(np.linalg.norm(v)) + return v / n if n > 1e-6 else v diff --git a/jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/_reid_gallery.py b/jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/_reid_gallery.py new file mode 100644 index 0000000..d16dc0c --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/_reid_gallery.py @@ -0,0 +1,105 @@ +""" +_reid_gallery.py — Appearance gallery for person re-identification (no ROS2 deps). + +Matches an incoming embedding against stored identities using cosine similarity. +New identities are created when the best match falls below the threshold. +""" + +from __future__ import annotations + +import time +from dataclasses import dataclass, field +from typing import List, Tuple + +import numpy as np + + +@dataclass +class Identity: + identity_id: int + embedding: np.ndarray # shape (D,) L2-normalised + last_seen: float = field(default_factory=time.monotonic) + hit_count: int = 1 + + def update(self, new_embedding: np.ndarray, alpha: float = 0.1) -> None: + """EMA update of the stored embedding, re-normalised after blending.""" + merged = (1.0 - alpha) * self.embedding + alpha * new_embedding + n = float(np.linalg.norm(merged)) + self.embedding = merged / n if n > 1e-6 else merged + self.last_seen = time.monotonic() + self.hit_count += 1 + + +class ReidGallery: + """ + Lightweight cosine-similarity re-ID gallery. + + Parameters + ---------- + match_threshold : float + Cosine similarity (dot product of unit vectors) required to accept a + match. Range [0, 1]; 0 = always new identity, 1 = perfect match only. + max_age_s : float + Identities not seen for this many seconds are pruned. + """ + + def __init__( + self, + match_threshold: float = 0.75, + max_age_s: float = 300.0, + ): + self._threshold = match_threshold + self._max_age_s = max_age_s + self._identities: List[Identity] = [] + self._next_id = 1 + + def match(self, embedding: np.ndarray) -> Tuple[int, float, bool]: + """ + Match embedding against the gallery. + + Returns + ------- + (identity_id, match_score, is_new) + identity_id : assigned ID (new or existing) + match_score : cosine similarity to best match (0.0 if new) + is_new : True if a new identity was created + """ + self._prune() + + if not self._identities: + return self._add_identity(embedding) + + scores = np.array( + [float(np.dot(embedding, ident.embedding)) for ident in self._identities] + ) + best_idx = int(np.argmax(scores)) + best_score = float(scores[best_idx]) + + if best_score >= self._threshold: + ident = self._identities[best_idx] + ident.update(embedding) + return ident.identity_id, best_score, False + + return self._add_identity(embedding) + + # ── Internal helpers ────────────────────────────────────────────────────── + + def _add_identity(self, embedding: np.ndarray) -> Tuple[int, float, bool]: + new_id = self._next_id + self._next_id += 1 + self._identities.append( + Identity(identity_id=new_id, embedding=embedding.copy()) + ) + return new_id, 0.0, True + + def _prune(self) -> None: + now = time.monotonic() + self._identities = [ + ident + for ident in self._identities + if now - ident.last_seen < self._max_age_s + ] + + @property + def size(self) -> int: + return len(self._identities) diff --git a/jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/person_reid_node.py b/jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/person_reid_node.py new file mode 100644 index 0000000..decc781 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/person_reid_node.py @@ -0,0 +1,174 @@ +""" +person_reid_node.py — Person re-identification for cross-camera tracking. + +Subscribes to: + /person/detections vision_msgs/Detection2DArray (person bounding boxes) + /camera/color/image_raw sensor_msgs/Image (colour frame for crops) + +Publishes: + /saltybot/person_reid saltybot_person_reid_msgs/PersonAppearanceArray (5 Hz) + +For each detected person the node: + 1. Crops the torso region (top 65 % of the bounding box height). + 2. Extracts a 128-dim L2-normalised embedding via MobileNetV2 ONNX (if the + model file is provided) or a 128-bin HSV colour histogram (fallback). + 3. Matches against a cosine-similarity gallery. + 4. Assigns a persistent identity_id (new or existing). + +Parameters: + model_path str '' Path to MobileNetV2+projection ONNX file + match_threshold float 0.75 Cosine similarity threshold for matching + max_identity_age_s float 300.0 Seconds before an unseen identity is pruned + publish_hz float 5.0 Publication rate (Hz) +""" + +from __future__ import annotations + +from typing import List + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy + +import message_filters +import cv2 +import numpy as np +from cv_bridge import CvBridge + +from sensor_msgs.msg import Image +from vision_msgs.msg import Detection2DArray + +from saltybot_person_reid_msgs.msg import PersonAppearance, PersonAppearanceArray + +from ._embedding_model import EmbeddingModel +from ._reid_gallery import ReidGallery + +# Fraction of bbox height kept as torso crop (top portion) +_TORSO_FRAC = 0.65 + +_BEST_EFFORT_QOS = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=4, +) + + +class PersonReidNode(Node): + + def __init__(self): + super().__init__('person_reid') + + self.declare_parameter('model_path', '') + self.declare_parameter('match_threshold', 0.75) + self.declare_parameter('max_identity_age_s', 300.0) + self.declare_parameter('publish_hz', 5.0) + + model_path = self.get_parameter('model_path').value + match_thr = self.get_parameter('match_threshold').value + max_age = self.get_parameter('max_identity_age_s').value + publish_hz = self.get_parameter('publish_hz').value + + self._bridge = CvBridge() + self._embedder = EmbeddingModel(model_path or None) + self._gallery = ReidGallery(match_threshold=match_thr, max_age_s=max_age) + + # Buffer: updated by frame callback, drained by timer + self._pending: List[PersonAppearance] = [] + self._pending_header = None + + # Synchronized subscribers + det_sub = message_filters.Subscriber( + self, Detection2DArray, '/person/detections', + qos_profile=_BEST_EFFORT_QOS) + img_sub = message_filters.Subscriber( + self, Image, '/camera/color/image_raw', + qos_profile=_BEST_EFFORT_QOS) + self._sync = message_filters.ApproximateTimeSynchronizer( + [det_sub, img_sub], queue_size=4, slop=0.1) + self._sync.registerCallback(self._on_frame) + + self._pub = self.create_publisher( + PersonAppearanceArray, '/saltybot/person_reid', 10) + + self.create_timer(1.0 / publish_hz, self._tick) + + backend = 'ONNX' if self._embedder._net else 'histogram' + self.get_logger().info( + f'person_reid ready — backend={backend} ' + f'threshold={match_thr} max_age={max_age}s' + ) + + # ── Frame callback ───────────────────────────────────────────────────────── + + def _on_frame(self, det_msg: Detection2DArray, img_msg: Image) -> None: + if not det_msg.detections: + self._pending = [] + self._pending_header = det_msg.header + return + + try: + bgr = self._bridge.imgmsg_to_cv2(img_msg, desired_encoding='bgr8') + except Exception as exc: + self.get_logger().error( + f'imgmsg_to_cv2 failed: {exc}', throttle_duration_sec=5.0) + return + + h_img, w_img = bgr.shape[:2] + appearances: List[PersonAppearance] = [] + + for det in det_msg.detections: + cx = det.bbox.center.position.x + cy = det.bbox.center.position.y + bw = det.bbox.size_x + bh = det.bbox.size_y + conf = det.results[0].hypothesis.score if det.results else 0.0 + + # Torso crop: top TORSO_FRAC of bounding box + x1 = max(0, int(cx - bw / 2.0)) + y1 = max(0, int(cy - bh / 2.0)) + x2 = min(w_img, int(cx + bw / 2.0)) + y2 = min(h_img, int(cy - bh / 2.0 + bh * _TORSO_FRAC)) + + if x2 - x1 < 8 or y2 - y1 < 8: + continue + + crop = bgr[y1:y2, x1:x2] + emb = self._embedder.embed(crop) + identity_id, match_score, is_new = self._gallery.match(emb) + + app = PersonAppearance() + app.header = det_msg.header + app.track_id = identity_id + app.embedding = emb.tolist() + app.bbox = det.bbox + app.confidence = float(conf) + app.match_score = float(match_score) + app.is_new_identity = is_new + appearances.append(app) + + self._pending = appearances + self._pending_header = det_msg.header + + # ── 5 Hz publish tick ───────────────────────────────────────────────────── + + def _tick(self) -> None: + if self._pending_header is None: + return + msg = PersonAppearanceArray() + msg.header = self._pending_header + msg.appearances = self._pending + self._pub.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = PersonReidNode() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/jetson/ros2_ws/src/saltybot_person_reid/setup.cfg b/jetson/ros2_ws/src/saltybot_person_reid/setup.cfg new file mode 100644 index 0000000..2e66a20 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_person_reid/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/saltybot_person_reid +[install] +install_scripts=$base/lib/saltybot_person_reid diff --git a/jetson/ros2_ws/src/saltybot_person_reid/setup.py b/jetson/ros2_ws/src/saltybot_person_reid/setup.py new file mode 100644 index 0000000..0cc75dd --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_person_reid/setup.py @@ -0,0 +1,29 @@ +from setuptools import setup, find_packages +from glob import glob + +package_name = 'saltybot_person_reid' + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/config', + glob('config/*.yaml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='SaltyLab', + maintainer_email='robot@saltylab.local', + description='Person re-identification — cross-camera appearance matching', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'person_reid = saltybot_person_reid.person_reid_node:main', + ], + }, +) diff --git a/jetson/ros2_ws/src/saltybot_person_reid/test/__pycache__/test_person_reid.cpython-314-pytest-9.0.2.pyc b/jetson/ros2_ws/src/saltybot_person_reid/test/__pycache__/test_person_reid.cpython-314-pytest-9.0.2.pyc new file mode 100644 index 0000000000000000000000000000000000000000..ef281a4cc218bff7c70d76f5987e68e12c460817 GIT binary patch literal 32987 zcmd6QeQ+Gdb?59CcCc7r34kBqmjIBWNJu0ofFMCy6eUrjL{SzEjiiO+fLt$uCAhi; zu$WnpA|$zFTU7!iCWLI31zM~URAEYFtk_iS#IoZ%S5lQYce%TO1Wd5jO3ruoox-cS zJBj>*zEs`E?{&{~_s-680Y;291be!BUVlu_%zMB0`gQL;HPsOf$KQSMhw0asYFaNP zoI|%G*H8E~?Pbl-a@s-7@ICI!`Pn(JH#p*N)b@sQ6^;B#I2UGD^qkJlmAOiGj^wJ? zIhukqO5{YAX_-YCUv4?0 zsX|Uv(5yJH(x~44MNR8lrfI9R1JSPSyeDedtR1K|YFhnHe;YCOYuW#nMI(7oMsg`D zm&m3}YiKZGrqX?_+2c+B<>gaN&km+@O?1&}Iy_`Haf&80wdwIknvSG0ly2HEIMif( z;pui{509kHR9{PLB=YdkQC$06q^W6BQzFxz7#uPO>=G<*&%mKnUtfB#|H+}gRHkXe zk+hW?>Nk@EO^1`2%%NoOzhdp9>H$V@IGsr~4W?3ksGe0|pqGzK(GugQ-nc zdLW%in(5r}rh#Ox_Xui_>>u^Fwzl~5HH8i4b*ED;zI>HZSHWv*KVI#6JH{x97x#dc z^Wo+7A6%XbEY*fEgv+#C(C{M+83BY9MlctSYdPHrt<_p8@{wmScAszGkGt=6YNIDp zi~j*l)2}xK>4WH@Ja%;1FOrj{bF8baLZ9 z=cO2KZ`QiooEK~Ok@Yso9$)|W>D4m)M!*R6u#cQBbw!;zN9*F(X#gubnatwc%$-# zWkWqnoHtXc1LDwqG;8sYQ-^at;xnr6^JV?_`OFTZ8vG;n5ij=f1LuADP+#tNHZ?k& zw5*hwYg)G-l?f|(I7Qj{nupjRrH2OhnC6g~uTMaFK_(

zHs=Pio70vxlG@aMZ`&pkrkkHjYSwQMysUi~tvR{(wY}d+ytn&% z`_Av1jdqUtK8)3#d|~{BY5ndq+ov9S>pn!rUYNXZCU!SsbFsVU5GsE&`rRi?k#R|; zE>k&ifk%*cX1mA(v62$mXlCjmkphvELg~r-#04Hf-Z`Jh1F@6Bh3TTsg7Bh#_jTO6 zRv?3yBZJA{S3>iV!3Ly#tG|p4_Rm)a53lJG{GEEnk8Gfaz7lpwo;>_8xP{<2Ss;z! zRJ>xNki?Ma;ht(yF6{7=tZoE{C*zgd@wE>Bk;B>d`1?EOBNL2e@yHgR(a=4<#ouA$ z4I^Y!^ayF%wb*G>p35KrPfpk)kc887r>{i_ph33e#DD;b%0n7KrW^Wg=&RFgd8zK* z*P`<1E6H?;l03^PXhoH>-n6QejoyYa5O3ZsKChzk7=x%$ZPYBNuWn@H`EJRtNW4>S zWaB-CDzd3DVr8F?_&$wAWod=vH)_k$Rybp9)Rm>x1#Nv<+DeCZb5#L?sWKKDOX5|V zeFeGE9ugqzP-2vIuXqFiUTo< z*;ltZd5B1i#mNA+>@3i?X3~SnOn>C1R#K8}gysS5&@gL_qcVUrsMf4+sI7&PJ9!dC zTVi}vh(Vi#NCBl-Bl#e;!G~u9T_hoxuT21EWf-FB9U90c&6J6cL^J8jFv-SZ($hET zahqgbX{k2vptu`3wX0yM!-V>$al~W;t9uJl?FFfhg4EW6l#RRWS~~ePj>6jQI_*@u zMK1!!WDJ-!HJNLv8><*JqMJ`<(pIW3ksAVr0-mC*{CYQrDnUs0BI`LQ@&ZLo^iFP> z_|jSP-O+bOXQS;vQuRyT-0{YaX?^FLyWZG^)7cH@+Wz1kL?(7k-7{0)ir9sE7W}-v z1LqUgbbZH6Y%8K41X*w~wsTIm|7P`_)3L20DIzB<5xqoph?Iz+s#EPE1tKSP8C}`I zFQE2`U4n*3aQW;8k%w|hN@Sz*b8R98A}58?Q}^%-)b;vS5x-C`qT-G~@prL&}Y;KkoO&YDVSo!|~wuy+BvbI?|>8 zXAD~j3FmDkF(UC0uvJxehk~tOUr~6gr>&&Gy29n3Y)RlU=N`y%Byoe&LvAQkYzUqx zPL_jgF0}i2(@V?|DqFXZm74@Bqg_ElsLTwQK?`IonJc^@M$6c+Nl&*7UgM8}D zp|_tq{oHJ{1)(Obret66T$_Df0meoy|z?gkqLS2?*l67SR@qiZt z&IXOh@H6ocW8H@x#%Qx{sfc;9V52Ht0oEPu-lMQ?VvvIU&O+WqM;dd#*5( zgMC8-`7oW+gSmWY1mX642-TPp6{^S*(6;84G59-v}^TbuhR`2+=@ zM_>`Aak#RLSXVQ)Eat@$&b%1xpO7y3dy79(ZvHD(ViUFV9s**`E2jEpqxXOz*DN}j z9#7BJG+wM}obpX=IT!uO?w{`c(cbBr#+jNuV}UC*v6KDd{S&>fXOM{1ojg8%d|F>K z`6vr6#@5W~_TP-YX6(2~iO3kBsZDVS6n5&eB%Y<(1dSTOm16Wq+6*3~e*GW727vxP zUFK8&X)+(BE@7cU@9kswkuLGEIeCO6f!V|doUMSZ?`S++VC&O+NU>!{TzoXyO4$0& z$0N+v_va47z}(&6GCW#_IT?%NRbXmMy1%V3HL~>yn@X1_>nOmZ&90U_Cg)+R6HL|l z%$>g4wm#WYPwPT)c`!L{>-+TkDg@Cj_O+-y`bxIGGWRHLy_iueM=KaIX4Ik!dR|-z z$@)E?mwXq+bK>?hS)UM$QRzDRyLIIQ8TPfz&vN~0jYe#{>s?FR^B|b;N z4g{*js;$6&)waVTDIjD&Gg@)IY$XE#40({FOd@S57OU1$;@_K|2Ey78S$T8eU-gsr zpKkxrb|(BEaS4B6*y`Yhtq$0;*3Q)5HLZ7?gFOqu&+9khO!lmeGqFvGUVuFd!Npj| zoNoWk>K)UuO(H2GWWlmYE>RsKB_gPbELf7rNnO@E$eu+PP&?VPY#JWHRgF2QJJ zi>`@5E_`H5-bN+iqT>=ivYZh5YD-cBv^Ah|^a5}J@|2?$+LB8HVj#rBctxCt9ZJpK zqE?7C=jqu(f*G%rC+{H?@jNSszZTb-Mq4mlJbc<2JAgs30XPH6JM;jc?D+-(1{Zg4 zP{3g1GyHt3lsoc%9|TO^>@N`P5kQjwz{x`1Tn=;P7<#_B@@YBG(s>Ft%$4Omdt6)| zZ*n!9+RBuZu*;~4M*xq?(F=IAIan|yYpbk6d>Z-dN8ApDfZYlKmi+uAZA025@uM;Y1y-BKqWvE93^F_-T?%#4ItFa zQ3}3HL6QP#8SstP5%OVhhg)(9c-Zy=1~M4z+DoMv@K}g_027#*47{S)L>hw|3~7|I z5SUHWiiJQm=N3YVHj4&Rg%TV{thIqeHg_Br-JvWd`f?-Ypp{VUxcP0R0maSA5xTMV zx`2gkG&ptW%_HA9GI{(5{vYb^>$6LD%+!4DU)+cQqXRiN9UVV9tv63T&4M?c!)c5% zW@60<=VHxs2$9sACwh5Q4VAo0lHxMer;>>{`9|3_g5F7io{ZLX0nurvO+ktX+AoF_ zBi2W&?U zxa~7$Uoh{6IWsO5hVUxDqm(ge3a}NR*EN=Eb;)_wtkGDK+gmJJNDYKi5I4x{7KzU8-CO<8{0h= zAg)-H8~m`Q{?zU_AN|IolWlKzp6;C5^489CwclHIe%Xb^-&sAgxO1jvJ6QOV<7qGAR}PlhqTiJR(F3TyFTStaM2>XdTc zI+a~p0aNr~Uc+r(b4|mKsFU!+PkGWDE}*fn5Tn~+WW!?)OMv6%eZ;nCI=*0egFvqs zC;M<|TO6~NVOw$>9Omh?Tx6Q{#;ihQIOT1oB-`~Yr(jlc*_14-{C#mPQr7Q@#ic8U zxF5F()4N})3!q+Lw)LhL%-Dz+Rkz_jN@>IL{=NGp%HesCk+>YaG$~hqhM$LOCicE> zX;O}ThM$LOmiE36VHbeAXt`%6HQhqD&Pqj}lXa|c_=P-!6A7Kr`HlJ zdPTI4{(DtbgSKR=YKWCp)ztMV^EM~9GIOU~)dDS#FI{QFIu^Te#2jZ!FF#GVOBU49 z25l16kiN*YRHoxGEtP4cB=;2~`w>bqm6Ar)yaxd@x3{yfgW20nhGA+-8?5QEa}YwF zPTcL1!4Y~sZr{s@qB649rOId$c_)GbWAqkohaN$`MRz0>M6Mok*GP-}UYK^+m&&Eg zf%G6&WTt!bn@eGk5(OTmW)8a)!s2OWm!(;(KiNj8V_M%f$)E=!ggtONx9mdwcUB>S zWhpynV%rd#i*1`jsQk_7+eq26^DfEMWhy5w@CfoI;le11ofKW@6c+^X)E1G4nw=C8 z!&S4O8^}9XEAl|>r0Bvjae+sWN1lJ9cv+x}J$GQ)*iqDyx1uE! zQWxfsJ@n?iq8wy3p}cbR5_7=*yrBEwm{8X5c^~pgII=+84_v}wOP1w5@29vt?u$@f znR1GvT4S_hFsizW zLTE8b0K4K&T)WCFdtWAcGAUvnbss6GC2W;!ai??{6{1s0PKdEqd(|YQ)1uPSgjpuZ zp@_Sg-hy7M%zQH8(nOW*qu@mZs&2c(ORZhX)LJ8$l^DJ4pZNS{D6)+Gc5YHX3O zl1qqq`zqaaX7}6sPVbx6@0#3qF^2g*`)^jii~N;j29J}*YUf=7g`K+WL5#dJyH#Q* zg$tAW#052iHW|YhjDIgGHZ9rfhQEguqQD4j42y&Y{wg#>FHg|T0r>WX zt`ZH*G3fzXmYeF^pNC^MnW`2d{oxPxAJu#YVuZt>+SxO+1jfi)b-z=bzC2)|`4 z!-dE0JE)LG%W6UJvUWoY_?kbV;DiN_T3_?oz*OHKMp*O*hkp3n`_Hk9T%d{$qi7KS z*)aSgt4H3Z*Io@>Fj2SD&%D7lf*hL?%>rh(sFgrMGjGSE+O5#?oql?z- zxDKg&ZzSMgA9x_X{Vh6YbiK&TSM{Y1C*dZaFbDhdF}Oa{gXm+cXOfwGMbe_h>73!T zgPwU_v;@t!D1L@22%sQaFl5nq-USfKpIL}GHJI-rlRc~_5y4j9(n5Tu;W-^KbcEQ-0AbXKk1yPOS^%Fzpx zMv>i{R-6>OEz=AnJ9=czi=8}qE&(TKMPmvR@^CTVS3K(=YxDrn82zPe^p_RPIs`Cx z^XZSi0FMIHLH`7(lRfkveNhgh9|1@$NB>(G{hND2!dtd#_&lDT@c0}4e!Qx1Ie>7N zkay?(y!hAB(0yOQ*Al%A&r2u=3w&Op9G(|nXT6YjsoC{Tcz(%pyd6Fy`p6XpIf|a| zRB?IqmDXLCDJP9paE>ZRE1otCY2|2zQ^V#^fn!9}s59z4d_m{WF}!?1X^k4KhbG_8 z@33V!%Rl05<{xSAml)g6DuE>rDeQid={xUZLe-u?8%g(ZZ;7v?2(wM?ei`|$X1oOAxeh`>fL$Ca4 zgz{S2V(X7-&doRad1oTdUIXW0(GwOrX`q90)SAp@&7tQDOnO*72cd?|W7se-tT5kY z80Md_d*+q4aPQ8%uiyM0-8iJ7W?rB`Ox&5TQIhlzuJANb%%tF36ug9>q{C)2(|g#m zu>^l$y=Dqn)JmlLQiD0{OU7sP6i~pIY}~8}K{{&~*nw1`W#@vk(M@B%E35B#yZdza z*=H~K&K{Uqz2&05Vk|K6=#{0*#$pr6@#jy*=-=zHF^k7|=%ik=^M4<->h+VcvDlU6 ztEctVlZ(f`GN-TRe>3`O@>{S|BBJD7k`$Mzu%J;RsP@B#JKnB7T}^wv^dgw7p6Z=x zpdDtW8}7cS?AtPnU1UbY{w^Y-!FvG9S409pRwg?0=YR9nzBN-k-Q&nO2M#QM>Yg( z*Bh^3^6HqwXZz=e?W*HpIQP|bZ!3^iw*%V=spIs}UcOe2^NMow#CBqu$A#^%xDnV+ zcX{ns_RODCdrgE8wmS-JhaGr;?G`%vF*f>(y5j|-e>7<0N6$jXzm|=EUH78}<9|D_n!v~l9Dh-cVYTwu`nNIuw2r>s{0?}S`5pz|rGOx1 zJ_sz*?gk@|=EEF}w3}Zgq6&sa+8MYozfWa9uH3hPDHOXHNP8vCwwmO2hocVw!w-1E6W4D`rd~!|(v@1CrjZA&Z?n;@H^uE?iN{pmq1Lpjrc{HE{4jsueiF!%ir@ zBpFWFldUL0CnPMR8_DY3k8SSlImx2Lb~lsavyJa#Ktig(BNkdR)7hch2cg)zrECi}rfI$(qEqPV&04gcHCzoLXM!FtwaG6DNDs+gt^)%n7LooTjm(fb+^ zlE!V({DkyQQI_}UjIde7FqyW*V{v(+2N{1>cqhc48fPkmVGTHX1>R~nYT9X=hN^1oFzUJYrz zxHuzLKmH=7Gx*<(zLKQ0oe~ix?~*sgw7zme76g%!64fArS(7WvRx)bZTIW`py)df1AGnXWBeaee)7&r?F` zOhR@lT$5%}K7A=9WcmWRrYHxl3E5q+hM+u0Z+4A>r-bYgc5~hs)e!!JZH0XvLQTx7KpB;w^^bZ#i0lNL;&`SJK|B z@k*COj4VJR3d_4Qs*s2=*t^N@ZSMza_8YOa_~sk&KkH9xEp;E+U-*M=Ei;hrv$!!k zh|L^xOyA6huxC$yio0eK^I@BQGKb7U+cxaeY%7HQO#g(y{2iC6+H*HgSf<(Ln;!de zqC$2FOIFlv)5(X_mbO2q?l2kqF2!akn6LI)rAS*}+MJ=X+YoT6%H3_Zv8^_l=O>rA z>Ne%}Hk;gAqOHTCX?{h>@>%$GOu%AT6WII{d!oUKVrVc;yMi81qz6gF;ukjh^IJWo zYq?@0Xwh23MC-3%!Vb(vH)7|Z_RbG{KiDu+bKmAFkN%smF6|m`ueGd5S-aMRZE+buRqm?&B>RpM{IiO`irsk@CM+2v-*1S z*^wDMuH;?vreKwdf*?B8CsHE9x*$>@I)kk_sjbWCAY48viXPD3<#v?zF1MrTDUTP2 zsP}(g3uv*O7MXv)efQoaLGK;c9t3-tmoOEI&m!$nIHHv@KHpr7Hfm2aalr{iQZRA! z&_1$(gTa!!2uNn*tx$Mvfq8pGqgclw#15b4mObS6D5k740oBE6F#-yo9=3Ox0xeF?v*-g+4+t;-j2Tp7)Jzyovjw zl^!kH@RIPAS;+d}+6BUS^FDjGOV+c?R2%a~B<|^!jx-5xjsUAV2 zMsTGVn3Lg%0due28x@>UX!8-uqwc+-7fI)KcGe!MBV^FjVu!aXX?QK!ry;cF_1bPK<{nVE9mf||Hhu+_> zD91srqAGaPi`88PaM+txa3~ko2{EtgkPF~dEMWIW0W%e@Qw6R=KW6hV^N6cqW{>_C z7Sr~{971*{sRTf zNy4q2{S`hiw_gRDXQ6qYT?O+#yM&LX`4=eFqF9j3e@oT;I|^8Ze@ihoTeFp7Ouc%I zVr)9b(^ACsD(>ERD3uvHCcRQs1*_y#%sfi|9(rK?9|Wq)dFeEv4DGQK`V+X*8gVHK=Q1A-akzggs21{^rfjwX|S}JTt!N` zsJ0|mv2vadJyT#xua(W7=u)=f-8Uge7uYvZj`U2Ij48Dp)Dq|J&dhp%`P2|AAuv5rNmr*Y&m;oqKyHMzVc0H7$Qmh5ckFO;dMz zYJrZMcc~l@o0f$q!+UGQYK4n4=~h{{7FB zC2Hj_m#=tx+396d^>3{juNl)PnkSyR5?gjLcITXK|IO%kjy*3@BBJD7@}`*9@0{Er z=s~2UL^dL5fH66HHEhgtHSCXRy#JDdzoQ^e!GEIQBMSaI1^<(R>l8dg!Lt;^5vYq} z&r>D^U!n`02<*kNA^g%xO*pjrT3tA_>RMeew1Jk%uB2tMD`}bR%4?Cz(DEBg!=ZH_ zYY2FcsV4dznSo?_Fp(JbZ#oKl#vx?xZ&{rWC-BI7@oO-;^GmkuSAFu!6MX4KE1$K& zZ_u`ySfP>5;kRo=7Jhqz=P_eNK42ZU@_zhe(66Y)(BQ~G_Bi+6_!G(q((hnkt;Zn! zCU8FZ+)#QjAMQ(=><4ktKl<5pbYz{^&&{?c(68A#IurlY zh{Ll*$J{ZK4RaUk84V44vV6qWQl`F>|J=?lvjWpW_|vR!BltMr^Z7pUhkd~tb(+uj ziuU!$FSN>EXjQ+`>VBbB|56LQs()2~HTKomgm0q#&CWME-`x4e&M7~+M7|q)Cw4)f zY1;8Y$4vc$v)V%+Y5V?O``nF4weQYPGz2&9ywkV-6Ai%)!-r+RH==udzIC5y6n=~) kyus+$=xkv5m0;x6C%*Q?M8jmq^rEKA!Df6VL2f?%|8Tl|od5s; literal 0 HcmV?d00001 diff --git a/jetson/ros2_ws/src/saltybot_person_reid/test/test_person_reid.py b/jetson/ros2_ws/src/saltybot_person_reid/test/test_person_reid.py new file mode 100644 index 0000000..90f55b0 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_person_reid/test/test_person_reid.py @@ -0,0 +1,163 @@ +""" +test_person_reid.py — Unit tests for person re-ID helpers (no ROS2 required). + +Covers: + - _l2_norm helper + - EmbeddingModel (histogram fallback — no model file needed) + - ReidGallery cosine-similarity matching +""" + +import sys +import os +import time + +import numpy as np +import pytest + +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..')) + +from saltybot_person_reid._embedding_model import EmbeddingModel, _l2_norm +from saltybot_person_reid._reid_gallery import ReidGallery + + +# ── _l2_norm ────────────────────────────────────────────────────────────────── + +class TestL2Norm: + + def test_unit_vector_unchanged(self): + v = np.array([1.0, 0.0, 0.0], dtype=np.float32) + assert np.allclose(_l2_norm(v), v) + + def test_normalised_to_unit_norm(self): + v = np.array([3.0, 4.0], dtype=np.float32) + assert abs(np.linalg.norm(_l2_norm(v)) - 1.0) < 1e-6 + + def test_zero_vector_does_not_crash(self): + v = np.zeros(4, dtype=np.float32) + result = _l2_norm(v) + assert result.shape == (4,) + + +# ── EmbeddingModel ──────────────────────────────────────────────────────────── + +class TestEmbeddingModel: + + def test_histogram_fallback_shape(self): + m = EmbeddingModel(model_path=None) + bgr = np.random.randint(0, 255, (100, 50, 3), dtype=np.uint8) + emb = m.embed(bgr) + assert emb.shape == (128,) + + def test_embedding_is_unit_norm(self): + m = EmbeddingModel(model_path=None) + bgr = np.random.randint(0, 255, (80, 40, 3), dtype=np.uint8) + emb = m.embed(bgr) + assert abs(np.linalg.norm(emb) - 1.0) < 1e-5 + + def test_empty_crop_returns_zero_vector(self): + m = EmbeddingModel(model_path=None) + emb = m.embed(np.zeros((0, 0, 3), dtype=np.uint8)) + assert emb.shape == (128,) + assert np.all(emb == 0.0) + + def test_red_and_blue_crops_differ(self): + m = EmbeddingModel(model_path=None) + red = np.full((80, 40, 3), (0, 0, 200), dtype=np.uint8) + blue = np.full((80, 40, 3), (200, 0, 0), dtype=np.uint8) + sim = float(np.dot(m.embed(red), m.embed(blue))) + assert sim < 0.99 + + def test_same_crop_deterministic(self): + m = EmbeddingModel(model_path=None) + bgr = np.random.randint(0, 255, (80, 40, 3), dtype=np.uint8) + assert np.allclose(m.embed(bgr), m.embed(bgr)) + + def test_embedding_float32(self): + m = EmbeddingModel(model_path=None) + bgr = np.random.randint(0, 255, (60, 30, 3), dtype=np.uint8) + emb = m.embed(bgr) + assert emb.dtype == np.float32 + + +# ── ReidGallery ─────────────────────────────────────────────────────────────── + +def _unit(dim: int = 128, seed: int | None = None) -> np.ndarray: + rng = np.random.default_rng(seed) + v = rng.standard_normal(dim).astype(np.float32) + return v / np.linalg.norm(v) + + +class TestReidGallery: + + def test_first_match_creates_identity(self): + g = ReidGallery(match_threshold=0.75) + uid, score, is_new = g.match(_unit(seed=0)) + assert uid == 1 + assert is_new is True + assert score == pytest.approx(0.0) + + def test_identical_embedding_matches(self): + g = ReidGallery(match_threshold=0.75) + emb = _unit(seed=1) + g.match(emb) + uid2, score2, is_new2 = g.match(emb) + assert uid2 == 1 + assert is_new2 is False + assert score2 > 0.99 + + def test_orthogonal_embeddings_create_new_id(self): + g = ReidGallery(match_threshold=0.75) + e1 = np.zeros(128, dtype=np.float32); e1[0] = 1.0 + e2 = np.zeros(128, dtype=np.float32); e2[64] = 1.0 + uid1, _, new1 = g.match(e1) + uid2, _, new2 = g.match(e2) + assert uid1 != uid2 + assert new2 is True + + def test_ids_are_monotonically_increasing(self): + # threshold > 1.0 is unreachable → every embedding creates a new identity + g = ReidGallery(match_threshold=2.0) + ids = [g.match(_unit(seed=i))[0] for i in range(5)] + assert ids == list(range(1, 6)) + + def test_gallery_size_increments_for_new_ids(self): + g = ReidGallery(match_threshold=2.0) + for i in range(4): + g.match(_unit(seed=i)) + assert g.size == 4 + + def test_prune_removes_stale_identities(self): + g = ReidGallery(match_threshold=0.75, max_age_s=0.01) + g.match(_unit(seed=0)) + time.sleep(0.05) + g._prune() + assert g.size == 0 + + def test_empty_gallery_prune_is_safe(self): + g = ReidGallery() + g._prune() + assert g.size == 0 + + def test_match_below_threshold_increments_id(self): + g = ReidGallery(match_threshold=0.99) + # Two random unit vectors are almost certainly < 0.99 similar + e1, e2 = _unit(seed=10), _unit(seed=20) + uid1, _, _ = g.match(e1) + uid2, _, _ = g.match(e2) + # uid2 may or may not equal uid1 depending on random similarity, + # but both must be valid positive integers + assert uid1 >= 1 + assert uid2 >= 1 + + def test_identity_update_does_not_change_id(self): + g = ReidGallery(match_threshold=0.5) + emb = _unit(seed=5) + uid_first, _, _ = g.match(emb) + for _ in range(10): + g.match(emb) + uid_last, _, _ = g.match(emb) + assert uid_last == uid_first + + +if __name__ == '__main__': + pytest.main([__file__, '-v']) diff --git a/jetson/ros2_ws/src/saltybot_person_reid_msgs/CMakeLists.txt b/jetson/ros2_ws/src/saltybot_person_reid_msgs/CMakeLists.txt new file mode 100644 index 0000000..43210ac --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_person_reid_msgs/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(saltybot_person_reid_msgs) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(vision_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/PersonAppearance.msg" + "msg/PersonAppearanceArray.msg" + DEPENDENCIES std_msgs vision_msgs +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/jetson/ros2_ws/src/saltybot_person_reid_msgs/msg/PersonAppearance.msg b/jetson/ros2_ws/src/saltybot_person_reid_msgs/msg/PersonAppearance.msg new file mode 100644 index 0000000..4f03800 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_person_reid_msgs/msg/PersonAppearance.msg @@ -0,0 +1,7 @@ +std_msgs/Header header +uint32 track_id +float32[] embedding +vision_msgs/BoundingBox2D bbox +float32 confidence +float32 match_score +bool is_new_identity diff --git a/jetson/ros2_ws/src/saltybot_person_reid_msgs/msg/PersonAppearanceArray.msg b/jetson/ros2_ws/src/saltybot_person_reid_msgs/msg/PersonAppearanceArray.msg new file mode 100644 index 0000000..2f8055c --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_person_reid_msgs/msg/PersonAppearanceArray.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +PersonAppearance[] appearances diff --git a/jetson/ros2_ws/src/saltybot_person_reid_msgs/package.xml b/jetson/ros2_ws/src/saltybot_person_reid_msgs/package.xml new file mode 100644 index 0000000..8812636 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_person_reid_msgs/package.xml @@ -0,0 +1,22 @@ + + + + saltybot_person_reid_msgs + 0.1.0 + Message types for person re-identification. + SaltyLab + MIT + + ament_cmake + rosidl_default_generators + + std_msgs + vision_msgs + + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + +