diff --git a/android/cartfollow-devlog.md b/android/cartfollow-devlog.md new file mode 100644 index 000000000..95c5d9fea --- /dev/null +++ b/android/cartfollow-devlog.md @@ -0,0 +1,235 @@ +# Human Cart Simulator 开发进度记录 + +> 所属项目：自主跟随购物车原型 +> 代码位置：`dev/OpenBot/android/robot/src/main/java/org/openbot/cartfollow/` +> 开发分支：`feature/human-cart-simulator`（Phase 1）/ `feature/distance-control`（Phase 2 起） +> 最后更新：2026-07-06 + +--- + +## 1. 模块总览 + +Human Cart Simulator 是购物车跟随功能的上位机核心模块，在 OpenBot App 中新增一个功能页面（"Cart Simulator"），实现基于手机摄像头人物检测的跟随控制闭环。 + +### 文件清单 + +| 文件 | 行数 | 作用 | +|------|------|------| +| `HumanCartSimulatorFragment.java` | 491 | 主 UI Fragment：摄像头预览 + 检测框绘制 + 确认/重拍/取消 + 倒计时 + 调试信息显示 | +| `ControlGenerator.java` | 92 | 控制算法：从指定目标生成 `Control(left,right)`，支持 `generateFromTarget()` | +| `FollowState.java` | 14 | 状态机枚举：`IDLE / CAPTURE_TARGET / LOCKED_PENDING_CONFIRM / CONFIRMED_ARMED / REACQUIRE_TARGET / READY_TO_FOLLOW / FOLLOW / LOST / SEARCH / STOP` | +| `FollowStateMachine.java` | 277 | 完整状态机：管理两阶段目标初始化、重识别、倒计时、跟随、丢失、搜索、停止 | +| `TargetMemory.java` | 143 | 目标记忆：confirmedBbox、面积、上下身 HSV 颜色直方图、动态位置 | +| `TargetMatcher.java` | 79 | 目标匹配：position + size + color + confidence 融合评分（ReID 接口预留） | +| `ReIDFeatureExtractor.java` | 5 | ReID 接口占位，尚未接入真实 embedding 推理 | +| `HumanCommandInterpreter.java` | 37 | 中文指令解释器：将 Control + 状态翻译为人可读的调试指令 | +| `fragment_human_cart_simulator.xml` | 218 | 布局文件：OverlayView + 指令文本 + 快照确认面板 + 倒计时 + 调试信息 + 底部面板 | + +### 集成点（在 OpenBot App 中的入口） + +| 文件 | 修改内容 | +|------|----------| +| `FeatureList.java` | 新增 `CART_SIMULATOR` 类别，显示在主菜单 | +| `MainFragment.java` | 添加 Cart Simulator 的导航路由 | +| `nav_graph.xml` | 注册 `cartSimFragment` 导航目标 | +| `strings.xml` | 新增 `cart_simulator` / `cart_sim_start` / `cart_sim_idle` 字符串 | + +--- + +## 2. 当前实现状态 + +### 2.1 已完成（commit `dd6aa95` + `409d85f` + `4da208a`） + +| 功能 | 状态 | 说明 | +|------|------|------| +| 人物检测（MobileNet-SSD） | 已完成 | 复用 OpenBot 现有 `Detector`，筛选 `classType="person"` | +| 两阶段目标初始化 | 已完成 | `CAPTURE_TARGET → LOCKED_PENDING_CONFIRM → CONFIRMED_ARMED`，采集时记录 confirmedBbox、面积、上下身颜色直方图，截图供用户确认 | +| 用户确认 / 重拍 / 取消 | 已完成 | 确认面板含快照预览与三按钮，状态切换正确 | +| 目标记忆 `TargetMemory` | 已完成 | 保存 confirmedBbox、confirmedArea、上下身 HSV 直方图、动态 lastBbox/lastCenter/lastArea/lastSeenTime | +| 目标匹配 `TargetMatcher` | 已完成 | position(0.40) + size(0.20) + color(0.30) + confidence(0.10) 融合评分，阈值 0.5；ReID 接口预留未接入 | +| 确认后重识别启动 | 已完成 | `CONFIRMED_ARMED → REACQUIRE_TARGET`，连续 `REACQUIRE_MATCH_N=8` 帧匹配后进入倒计时 | +| 倒计时启动 | 已完成 | `READY_TO_FOLLOW` 倒计时 3 秒后进入 FOLLOW | +| 完整状态机 `FollowStateMachine` | 已完成 | `IDLE → CAPTURE → CONFIRM → REACQUIRE → COUNTDOWN → FOLLOW → LOST → SEARCH → STOP` 全链路 | +| `LOST → SEARCH → STOP` 执行逻辑 | 已完成 | 连续 `FOLLOW_LOST_M=10` 帧未匹配进入 LOST；LOST 持续 `LOST_TO_SEARCH_MS=800ms` 进入 SEARCH；SEARCH 超时 `SEARCH_TIMEOUT_MS=5000ms` 进入 STOP；期间重新匹配则回 FOLLOW | +| Control 生成（基于指定目标） | 已完成 | `generateFromTarget()` 根据匹配目标而非最大框生成 `Control(left,right)` | +| 转向方向修正 | 已完成 | `FLIP_TURN=true`，commit `409d85f` 修正 | +| 距离控制（太近停车） | 已完成 | `TOO_CLOSE_H_RATIO=0.75`，目标框超过画面高度 75% 时 forward=0。**注：当前为硬编码 setpoint，Phase 2 将改为初始化标定** | +| UI 模式切换 | 已完成 | 开关控制检测启停，启动后锁定模型选择 | +| 置信度调节 | 已完成 | +/- 按钮以 5% 步进调节，范围 5%-95% | +| 模型选择 | 已完成 | 支持本地和 URL 模型，修复了 URL 模型的错误提示 | +| 检测框可视化 | 已完成 | 绿色目标框 / 黄色候选框 / 白色普通行人框 / 红色匹配失败框 | +| 快照确认面板 | 已完成 | LOCKED_PENDING_CONFIRM 时显示候选目标截图 + 确认/重拍/取消 | +| 倒计时显示 | 已完成 | READY_TO_FOLLOW 时显示剩余秒数 | +| 调试信息面板 | 已完成 | 显示 state / forward / turn / left / right / persons / fps | +| 中文指令提示 | 已完成 | 显示"请向前"等中文建议指令（调试用途） | +| 导航集成 | 已完成 | 已注册到主菜单 "Cart Simulator" 入口 | + +### 2.2 核心控制算法（当前状态，Phase 2 将重构） + +``` +输入：人物检测结果列表 + 画面尺寸 + 传感器角度 +输出：Control(left, right) + +算法流程： + 1. 过滤出置信度 ≥ MIN_CONFIDENCE 的 person 检测结果 + 2. 在 FOLLOW/LOST/SEARCH 中由 TargetMatcher 选出最佳匹配目标（非最大框） + 3. 计算： + xError = target_centerX / imgWidth - 0.5 // 横向偏差 [-0.5, +0.5] + heightRatio = target_boxHeight / imgHeight // 目标占比 + distError = TARGET_H_RATIO - heightRatio // 距离偏差（硬编码 setpoint） + 4. turn = K_TURN × xError × (FLIP_TURN ? -1 : 1) + forward = clamp(K_DIST × distError, 0, MAX_FORWARD) + if heightRatio > TOO_CLOSE_H_RATIO: forward = 0 + 5. left = forward - turn, right = forward + turn +``` + +当前可调参数（`ControlGenerator`）： + +| 参数 | 默认值 | 含义 | +|------|--------|------| +| `K_TURN` | 1.5 | 转向灵敏度 | +| `K_DIST` | 1.0 | 距离跟随灵敏度 | +| `TARGET_H_RATIO` | 0.5 | 目标理想高度占比（硬编码，Phase 2 将改为初始化标定） | +| `TOO_CLOSE_H_RATIO` | 0.75 | 太近阈值（硬编码，Phase 2 将改为 setpoint 比例） | +| `MAX_FORWARD` | 0.6 | 最大前进速度 | +| `MIN_CONFIDENCE` | 0.5 | 最小检测置信度 | +| `FLIP_TURN` | true | 转向方向翻转 | + +当前状态机参数（`FollowStateMachine`）： + +| 参数 | 默认值 | 含义 | +|------|--------|------| +| `CAPTURE_FRAMES` | 15 | 采集帧数阈值 | +| `REACQUIRE_MATCH_N` | 8 | 重识别连续匹配帧数 | +| `FOLLOW_LOST_M` | 10 | FOLLOW 连续未匹配进入 LOST 的帧数 | +| `LOST_TO_SEARCH_MS` | 800 | LOST 进入 SEARCH 的延时 | +| `SEARCH_TIMEOUT_MS` | 5000 | SEARCH 超时进入 STOP | +| `COUNTDOWN_MS` | 3000 | 倒计时时长 | + +--- + +## 3. 尚未实现（待开发） + +### 3.1 关键缺失 + +| 功能 | 优先级 | 说明 | +|------|--------|------| +| **初始化距离标定 + 图像伺服** | 高 | 当前 `TARGET_H_RATIO=0.5` 为硬编码，需在采集时记录 `desired_bbox_height_ratio / area_ratio / bottom_ratio`，后续以 setpoint 比例判断距离状态。见 Phase 2 计划书 | +| **DistanceState 输出** | 高 | 输出 `TOO_FAR / OK / TOO_CLOSE / UNKNOWN`，替代当前线性 distError，UNKNOWN 时停车 | +| **距离调试显示** | 高 | Simulator 需显示 `height_scale / area_scale / bottom_shift / distance_state / distance_confidence` | +| **`vehicle.setControl()` 集成** | 中（阶段6） | 当前 Control 仅显示在 UI 上，未实际发送给底盘。硬件联调阶段在 `processFrame()` 中调用 `vehicle.setControl()` | +| **目标重锁定增强** | 中 | 当前 LOST/SEARCH 恢复复用 TargetMatcher，未加入 ReID 强确认 | +| **参数持久化** | 低 | 当前调参仅内存生效，重启恢复默认 | +| **参数 UI 面板** | 低 | K_TURN / MAX_FORWARD 等参数需通过代码修改，没有 UI 界面 | + +### 3.2 状态机（已完整实现） + +``` +IDLE ──(启动开关)──→ CAPTURE_TARGET ──(采集N帧)──→ LOCKED_PENDING_CONFIRM + │ + 确认 / 重拍 / 取消 + │ +CONFIRMED_ARMED ──(检测到人)──→ REACQUIRE_TARGET ──(连续N帧匹配)──→ READY_TO_FOLLOW + │ + 倒计时3秒 + ↓ + FOLLOW + │ + 连续M帧未匹配 + ↓ + LOST ──(800ms)──→ SEARCH ──(5s超时)──→ STOP + │ │ + └──(重新匹配)──────┴──→ FOLLOW + +STOP ──(用户重新开始)──→ CAPTURE_TARGET +``` + +### 3.3 已知代码问题 + +| 问题 | 说明 | +|------|------| +| 目标选择策略仍依赖位置+颜色 | TargetMatcher 未接入 ReID，多人长时间交叉下可能误锁。阶段3 处理 | +| 距离控制硬编码 setpoint | `TARGET_H_RATIO=0.5` 不随用户身高/采集距离自适应。Phase 2 解决 | +| forward 限幅非对称 | `forward = max(0, min(forward, MAX_FORWARD))` — 只允许前进，不允许后退。安全优先，合理 | +| sensorOrientation 处理 | 横竖屏切换时 target 位置计算需实测验证；bottom_shift 在旋转下方向待验证 | + +--- + +## 4. 与下位机的接口 + +| 方向 | 协议 | 说明 | +|------|------|------| +| 上位机→下位机 | `c,` | 由 `Vehicle.sendControl()` 发送，范围 [-255,255] | +| 心跳 | `h` | 由 `Vehicle` 自动管理 | + +**当前状态：Cart Simulator 未调用 `vehicle.setControl()`，因此底盘不会运动。** 首版联调前需要先接通这个链路。 + +--- + +## 5. 后续开发计划 + +> 阶段划分对齐 `design/自主跟随购物车上位机软件开发计划.md` 与 `design/上位机软件开发 Phase 2——修正跟随距离控制计划书.md` + +### Phase 1：状态机与目标初始化闭环（已完成） + +- [x] 完整状态机 `IDLE → CAPTURE → CONFIRM → REACQUIRE → COUNTDOWN → FOLLOW → LOST → SEARCH → STOP` +- [x] 两阶段目标初始化 + 用户确认 + 重识别启动 +- [x] TargetMemory / TargetMatcher / ControlGenerator 接入 +- [x] Human Cart Simulator 实时提示与调试显示 +- [ ] 多人干扰不切换目标（部分保证，待 ReID 增强） + +### Phase 2：修正跟随距离控制（进行中） + +- [ ] 新增 `DistanceState` 枚举（`TOO_FAR / OK / TOO_CLOSE / UNKNOWN`） +- [ ] 新增 `ImageSetpointDistanceEstimator`，输出 height_scale / area_scale / bottom_shift / state / confidence +- [ ] `TargetMemory` 采集时记录 `desired_bbox_height_ratio / area_ratio / bottom_ratio` +- [ ] 重构 `ControlGenerator`，forward 由 DistanceState 决定，移除硬编码 setpoint +- [ ] `FollowStateMachine.FrameResult` 透传 distanceEstimate +- [ ] Human Cart Simulator 显示 dist_state / hScale / aScale / bShift / distConf +- [ ] `HumanCommandInterpreter` 纳入 distance state +- [ ] 0.8-1.2 m 目标距离标定验证 + +**不在 Phase 2 范围**：`vehicle.setControl()` 接通（阶段6）、ReID 增强（阶段3）、障碍处理（阶段5） + +### Phase 3：真实检测框数据闭环 + ReID + +- [ ] 从 OpenBot Android 导出真实 person bbox crop +- [ ] PC 端验证 confirmedGallery / reid_score / reid_margin +- [ ] Android 端部署 osnet_x0_25（ONNX Runtime Mobile 主线） +- [ ] 多人/遮挡/重现场景验证 + +### Phase 4：单目深度辅助 + +- [ ] MiDaS / Depth Anything Android 部署调研 +- [ ] 作为距离/障碍风险辅助，不替代安全状态机 + +### Phase 5：局部可通行空间与跟随式避障 + +- [ ] LEFT / CENTER / RIGHT 三方向 free score +- [ ] 候选动作 SLOW_FORWARD / LEFT_ARC / RIGHT_ARC / BLOCKED_WAIT + +### Phase 6：硬件联调 + +- [ ] 接通 `vehicle.setControl()` 到底盘 +- [ ] 真实车速/转向半径/延迟标定 +- [ ] ToF / 超声波安全冗余 + +--- + +## 6. 提交历史 + +| Commit | 日期 | 说明 | +|--------|------|------| +| `dd6aa95` | 2026-07 | Add Human Cart Simulator for shopping cart follow debugging | +| `409d85f` | 2026-07 | Fix turn direction, confidence button height, and model selection | +| `4da208a` | 2026-07-02 | Add two-stage target init, target memory and full follow state machine | + +--- + +## 7. 调试提示 + +- 使用 `/dev/OpenBot/android` 在 Android Studio 中打开工程 +- 主菜单 → "Cart Simulator" 进入本模块 +- 打开 Start 开关开始检测，关闭开关回到 IDLE +- 调试信息面板显示实时 state / forward / turn / persons / fps +- 中文指令文本仅供调试参考，实际不会发送给底盘 diff --git a/android/robot/src/main/java/org/openbot/cartfollow/ControlGenerator.java b/android/robot/src/main/java/org/openbot/cartfollow/ControlGenerator.java new file mode 100644 index 000000000..a93e0b2af --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/ControlGenerator.java @@ -0,0 +1,92 @@ +package org.openbot.cartfollow; + +import android.graphics.RectF; +import java.util.ArrayList; +import java.util.List; +import org.openbot.tflite.Detector.Recognition; +import org.openbot.vehicle.Control; + +public class ControlGenerator { + + public static class Result { + public final Control control; + public final Recognition target; + public final List persons; + public final boolean tooClose; + + public Result(Control control, Recognition target, List persons, boolean tooClose) { + this.control = control; + this.target = target; + this.persons = persons; + this.tooClose = tooClose; + } + } + + public float K_TURN = 1.5f; + public float K_DIST = 1.0f; + public float TARGET_H_RATIO = 0.5f; + public float MAX_FORWARD = 0.6f; + public float MIN_CONFIDENCE = 0.5f; + public float TOO_CLOSE_H_RATIO = 0.75f; + public boolean FLIP_TURN = true; + + public Result generate(List results, int frameW, int frameH, int sensorOrientation) { + List persons = new ArrayList<>(); + if (results != null) { + for (Recognition r : results) { + if (r == null || r.getLocation() == null) continue; + if (r.getConfidence() == null || r.getConfidence() < MIN_CONFIDENCE) continue; + if (!"person".equals(r.getTitle())) continue; + persons.add(r); + } + } + + if (persons.isEmpty() || frameW <= 0 || frameH <= 0) { + return new Result(new Control(0f, 0f), null, persons, false); + } + + Recognition target = null; + float maxArea = -1f; + for (Recognition r : persons) { + RectF loc = r.getLocation(); + float area = loc.width() * loc.height(); + if (area > maxArea) { + maxArea = area; + target = r; + } + } + + return generateFromTarget(target, persons, frameW, frameH, sensorOrientation); + } + + public Result generateFromTarget( + Recognition target, List persons, int frameW, int frameH, int sensorOrientation) { + if (target == null || target.getLocation() == null || frameW <= 0 || frameH <= 0) { + return new Result(new Control(0f, 0f), null, persons == null ? new ArrayList<>() : persons, false); + } + + RectF loc = target.getLocation(); + boolean rotated = sensorOrientation % 180 == 90; + float imgWidth = rotated ? frameH : frameW; + float imgHeight = rotated ? frameW : frameH; + float centerX = rotated ? loc.centerY() : loc.centerX(); + float boxHeight = rotated ? loc.width() : loc.height(); + centerX = Math.max(0f, Math.min(centerX, imgWidth)); + + float xError = centerX / imgWidth - 0.5f; + float heightRatio = boxHeight / imgHeight; + float distError = TARGET_H_RATIO - heightRatio; + boolean tooClose = heightRatio > TOO_CLOSE_H_RATIO; + + float turn = K_TURN * xError; + if (FLIP_TURN) turn = -turn; + + float forward = K_DIST * distError; + forward = Math.max(0f, Math.min(forward, MAX_FORWARD)); + if (tooClose) forward = 0f; + + float left = forward - turn; + float right = forward + turn; + return new Result(new Control(left, right), target, persons, tooClose); + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/FollowState.java b/android/robot/src/main/java/org/openbot/cartfollow/FollowState.java new file mode 100644 index 000000000..aa59c4223 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/FollowState.java @@ -0,0 +1,14 @@ +package org.openbot.cartfollow; + +public enum FollowState { + IDLE, + CAPTURE_TARGET, + LOCKED_PENDING_CONFIRM, + CONFIRMED_ARMED, + REACQUIRE_TARGET, + READY_TO_FOLLOW, + FOLLOW, + LOST, + SEARCH, + STOP +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/FollowStateMachine.java b/android/robot/src/main/java/org/openbot/cartfollow/FollowStateMachine.java new file mode 100644 index 000000000..39c8bc155 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/FollowStateMachine.java @@ -0,0 +1,277 @@ +package org.openbot.cartfollow; + +import android.graphics.Bitmap; +import android.graphics.RectF; +import java.util.ArrayList; +import java.util.List; +import org.openbot.tflite.Detector.Recognition; +import org.openbot.vehicle.Control; + +public class FollowStateMachine { + + public static class FrameResult { + public final FollowState state; + public final Control control; + public final Recognition target; + public final Recognition candidate; + public final List persons; + public final boolean matched; + public final boolean tooClose; + public final Bitmap snapshot; + public final int countdownSec; + + public FrameResult( + FollowState state, + Control control, + Recognition target, + Recognition candidate, + List persons, + boolean matched, + boolean tooClose, + Bitmap snapshot, + int countdownSec) { + this.state = state; + this.control = control; + this.target = target; + this.candidate = candidate; + this.persons = persons; + this.matched = matched; + this.tooClose = tooClose; + this.snapshot = snapshot; + this.countdownSec = countdownSec; + } + } + + public int CAPTURE_FRAMES = 15; + public int REACQUIRE_MATCH_N = 8; + public int FOLLOW_LOST_M = 10; + public long LOST_TO_SEARCH_MS = 800; + public long SEARCH_TIMEOUT_MS = 5000; + public long COUNTDOWN_MS = 3000; + + private final TargetMatcher matcher; + private final ControlGenerator controlGenerator; + private final TargetMemory memory = new TargetMemory(); + + private FollowState state = FollowState.IDLE; + private int captureCount = 0; + private int matchCount = 0; + private int lostCount = 0; + private long stateEnterTime = 0L; + private Bitmap snapshot = null; + + public FollowStateMachine(TargetMatcher matcher, ControlGenerator controlGenerator) { + this.matcher = matcher; + this.controlGenerator = controlGenerator; + } + + public FollowState getState() { + return state; + } + + public TargetMemory getMemory() { + return memory; + } + + public void startCapture() { + if (state == FollowState.IDLE || state == FollowState.STOP) { + memory.clear(); + snapshot = null; + captureCount = 0; + state = FollowState.CAPTURE_TARGET; + } + } + + public void confirm() { + if (state == FollowState.LOCKED_PENDING_CONFIRM) { + state = FollowState.CONFIRMED_ARMED; + matchCount = 0; + } + } + + public void retake() { + if (state == FollowState.LOCKED_PENDING_CONFIRM) { + memory.clear(); + snapshot = null; + captureCount = 0; + state = FollowState.CAPTURE_TARGET; + } + } + + public void cancel() { + memory.clear(); + snapshot = null; + captureCount = 0; + matchCount = 0; + lostCount = 0; + state = FollowState.IDLE; + } + + public FrameResult onFrame( + List persons, Bitmap frame, int frameW, int frameH, int sensorOrientation) { + List safePersons = persons == null ? new ArrayList<>() : persons; + long now = System.currentTimeMillis(); + + switch (state) { + case IDLE: + return new FrameResult( + FollowState.IDLE, new Control(0f, 0f), null, null, safePersons, false, false, null, -1); + + case CAPTURE_TARGET: { + Recognition cand = selectLargest(safePersons); + if (cand == null || cand.getLocation() == null) { + captureCount = 0; + return new FrameResult( + FollowState.CAPTURE_TARGET, new Control(0f, 0f), null, null, safePersons, false, false, null, -1); + } + captureCount++; + if (captureCount >= CAPTURE_FRAMES) { + memory.captureFromBitmap(frame, cand.getLocation()); + snapshot = cropSnapshot(frame, cand.getLocation()); + captureCount = 0; + state = FollowState.LOCKED_PENDING_CONFIRM; + return new FrameResult( + FollowState.LOCKED_PENDING_CONFIRM, new Control(0f, 0f), null, null, safePersons, false, false, snapshot, -1); + } + return new FrameResult( + FollowState.CAPTURE_TARGET, new Control(0f, 0f), null, cand, safePersons, false, false, null, -1); + } + + case LOCKED_PENDING_CONFIRM: + return new FrameResult( + FollowState.LOCKED_PENDING_CONFIRM, new Control(0f, 0f), null, null, safePersons, false, false, snapshot, -1); + + case CONFIRMED_ARMED: + if (!safePersons.isEmpty()) { + state = FollowState.REACQUIRE_TARGET; + matchCount = 0; + } + return new FrameResult( + state, new Control(0f, 0f), null, null, safePersons, false, false, null, -1); + + case REACQUIRE_TARGET: { + TargetMatcher.MatchResult m = matcher.match(safePersons, frame, memory, frameW, frameH); + if (m.matched) matchCount++; + else matchCount = 0; + if (matchCount >= REACQUIRE_MATCH_N) { + state = FollowState.READY_TO_FOLLOW; + stateEnterTime = now; + memory.updateDynamic(m.best); + } + return new FrameResult( + state, new Control(0f, 0f), m.best, null, safePersons, m.matched, false, null, -1); + } + + case READY_TO_FOLLOW: { + int cd = (int) Math.ceil((COUNTDOWN_MS - (now - stateEnterTime)) / 1000.0); + if (now - stateEnterTime >= COUNTDOWN_MS) { + state = FollowState.FOLLOW; + lostCount = 0; + return new FrameResult( + FollowState.FOLLOW, new Control(0f, 0f), null, null, safePersons, false, false, null, -1); + } + TargetMatcher.MatchResult m = matcher.match(safePersons, frame, memory, frameW, frameH); + return new FrameResult( + FollowState.READY_TO_FOLLOW, new Control(0f, 0f), m.best, null, safePersons, m.matched, false, null, Math.max(0, cd)); + } + + case FOLLOW: { + TargetMatcher.MatchResult m = matcher.match(safePersons, frame, memory, frameW, frameH); + if (m.matched) { + memory.updateDynamic(m.best); + lostCount = 0; + ControlGenerator.Result res = + controlGenerator.generateFromTarget(m.best, safePersons, frameW, frameH, sensorOrientation); + return new FrameResult( + FollowState.FOLLOW, res.control, m.best, null, safePersons, true, res.tooClose, null, -1); + } + lostCount++; + if (lostCount >= FOLLOW_LOST_M) { + state = FollowState.LOST; + stateEnterTime = now; + } + return new FrameResult( + state, new Control(0f, 0f), null, null, safePersons, false, false, null, -1); + } + + case LOST: { + TargetMatcher.MatchResult m = matcher.match(safePersons, frame, memory, frameW, frameH); + if (m.matched) { + state = FollowState.FOLLOW; + lostCount = 0; + memory.updateDynamic(m.best); + ControlGenerator.Result res = + controlGenerator.generateFromTarget(m.best, safePersons, frameW, frameH, sensorOrientation); + return new FrameResult( + FollowState.FOLLOW, res.control, m.best, null, safePersons, true, res.tooClose, null, -1); + } + if (now - stateEnterTime >= LOST_TO_SEARCH_MS) { + state = FollowState.SEARCH; + stateEnterTime = now; + } + return new FrameResult( + state, new Control(0f, 0f), null, null, safePersons, false, false, null, -1); + } + + case SEARCH: { + TargetMatcher.MatchResult m = matcher.match(safePersons, frame, memory, frameW, frameH); + if (m.matched) { + state = FollowState.FOLLOW; + lostCount = 0; + memory.updateDynamic(m.best); + ControlGenerator.Result res = + controlGenerator.generateFromTarget(m.best, safePersons, frameW, frameH, sensorOrientation); + return new FrameResult( + FollowState.FOLLOW, res.control, m.best, null, safePersons, true, res.tooClose, null, -1); + } + if (now - stateEnterTime >= SEARCH_TIMEOUT_MS) { + state = FollowState.STOP; + } + return new FrameResult( + state, new Control(0f, 0f), null, null, safePersons, false, false, null, -1); + } + + case STOP: + default: + return new FrameResult( + FollowState.STOP, new Control(0f, 0f), null, null, safePersons, false, false, null, -1); + } + } + + private static Recognition selectLargest(List persons) { + Recognition target = null; + float maxArea = -1f; + for (Recognition r : persons) { + if (r == null || r.getLocation() == null) continue; + RectF loc = r.getLocation(); + float area = loc.width() * loc.height(); + if (area > maxArea) { + maxArea = area; + target = r; + } + } + return target; + } + + private static Bitmap cropSnapshot(Bitmap frame, RectF bbox) { + if (frame == null || bbox == null) return null; + int fw = frame.getWidth(); + int fh = frame.getHeight(); + int l = clamp((int) bbox.left, 0, fw - 1); + int t = clamp((int) bbox.top, 0, fh - 1); + int r = clamp((int) bbox.right, 1, fw); + int b = clamp((int) bbox.bottom, 1, fh); + int w = r - l; + int h = b - t; + if (w <= 0 || h <= 0) return null; + try { + return Bitmap.createBitmap(frame, l, t, w, h); + } catch (Exception e) { + return null; + } + } + + private static int clamp(int v, int lo, int hi) { + return v < lo ? lo : (v > hi ? hi : v); + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/HumanCartSimulatorFragment.java b/android/robot/src/main/java/org/openbot/cartfollow/HumanCartSimulatorFragment.java new file mode 100644 index 000000000..c282e063c --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/HumanCartSimulatorFragment.java @@ -0,0 +1,491 @@ +package org.openbot.cartfollow; + +import android.graphics.Bitmap; +import android.graphics.Canvas; +import android.graphics.Color; +import android.graphics.Matrix; +import android.graphics.Paint; +import android.graphics.RectF; +import android.os.Bundle; +import android.os.Handler; +import android.os.HandlerThread; +import android.os.SystemClock; +import android.view.LayoutInflater; +import android.view.View; +import android.view.ViewGroup; +import android.widget.Toast; +import androidx.annotation.NonNull; +import androidx.annotation.Nullable; +import androidx.camera.core.CameraSelector; +import androidx.camera.core.ImageProxy; +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; +import java.util.Locale; +import org.jetbrains.annotations.NotNull; +import org.openbot.R; +import org.openbot.common.CameraFragment; +import org.openbot.databinding.FragmentHumanCartSimulatorBinding; +import org.openbot.env.ImageUtils; +import org.openbot.tflite.Detector; +import org.openbot.tflite.Model; +import org.openbot.tflite.Network; +import org.openbot.utils.CameraUtils; +import org.openbot.utils.Enums; +import org.openbot.vehicle.Control; +import timber.log.Timber; + +public class HumanCartSimulatorFragment extends CameraFragment { + + private static final int COLOR_TARGET = 0; + private static final int COLOR_CANDIDATE = 1; + private static final int COLOR_NORMAL = 2; + private static final int COLOR_FAIL = 3; + + private FragmentHumanCartSimulatorBinding binding; + private Handler handler; + private HandlerThread handlerThread; + + private boolean computingNetwork = false; + private float minConfidence = 0.5f; + + private Detector detector; + private Matrix frameToCropTransform; + private Bitmap croppedBitmap; + private int sensorOrientation; + private Matrix cropToFrameTransform; + + private Model model; + private Network.Device device = Network.Device.CPU; + private int numThreads = -1; + private final String classType = "person"; + + private long lastProcessingTimeMs = -1; + private long frameNum = 0; + + private final ControlGenerator controlGenerator = new ControlGenerator(); + private final HumanCommandInterpreter interpreter = new HumanCommandInterpreter(); + private final TargetMatcher matcher = new TargetMatcher(); + private final FollowStateMachine stateMachine = + new FollowStateMachine(matcher, controlGenerator); + + private final List drawBoxes = new ArrayList<>(); + private int drawFrameWidth = 0; + private int drawFrameHeight = 0; + private int drawSensorOrientation = 0; + + private final Paint targetBoxPaint = new Paint(); + private final Paint candidateBoxPaint = new Paint(); + private final Paint personBoxPaint = new Paint(); + private final Paint failBoxPaint = new Paint(); + private final Paint boxTextPaint = new Paint(); + + @Override + public void onCreate(@Nullable Bundle savedInstanceState) { + super.onCreate(savedInstanceState); + targetBoxPaint.setColor(Color.GREEN); + targetBoxPaint.setStyle(Paint.Style.STROKE); + targetBoxPaint.setStrokeWidth(8.0f); + candidateBoxPaint.setColor(Color.YELLOW); + candidateBoxPaint.setStyle(Paint.Style.STROKE); + candidateBoxPaint.setStrokeWidth(8.0f); + personBoxPaint.setColor(Color.WHITE); + personBoxPaint.setStyle(Paint.Style.STROKE); + personBoxPaint.setStrokeWidth(6.0f); + failBoxPaint.setColor(Color.RED); + failBoxPaint.setStyle(Paint.Style.STROKE); + failBoxPaint.setStrokeWidth(8.0f); + boxTextPaint.setColor(Color.WHITE); + boxTextPaint.setTextSize(40.0f); + } + + @Override + public View onCreateView( + @NotNull LayoutInflater inflater, ViewGroup container, Bundle savedInstanceState) { + binding = FragmentHumanCartSimulatorBinding.inflate(inflater, container, false); + return inflateFragment(binding, inflater, container); + } + + @Override + public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceState) { + super.onViewCreated(view, savedInstanceState); + + binding.confidenceValue.setText((int) (minConfidence * 100) + "%"); + binding.plusConfidence.setOnClickListener( + v -> { + int confValue = (int) (minConfidence * 100); + if (confValue >= 95) return; + confValue += 5; + minConfidence = confValue / 100f; + binding.confidenceValue.setText(confValue + "%"); + controlGenerator.MIN_CONFIDENCE = minConfidence; + }); + binding.minusConfidence.setOnClickListener( + v -> { + int confValue = (int) (minConfidence * 100); + if (confValue <= 5) return; + confValue -= 5; + minConfidence = confValue / 100f; + binding.confidenceValue.setText(confValue + "%"); + controlGenerator.MIN_CONFIDENCE = minConfidence; + }); + + List models = getModelNames(f -> f.type.equals(Model.TYPE.DETECTOR)); + initModelSpinner(binding.modelSpinner, models, preferencesManager.getObjectNavModel()); + + setAnalyserResolution(Enums.Preview.HD.getValue()); + + binding.trackingOverlay.addCallback(canvas -> drawOverlay(canvas)); + + binding.btnConfirm.setOnClickListener(v -> stateMachine.confirm()); + binding.btnRetake.setOnClickListener(v -> stateMachine.retake()); + binding.btnCancel.setOnClickListener(v -> stateMachine.cancel()); + + binding.startSwitch.setChecked(false); + binding.startSwitch.setOnClickListener( + v -> { + if (binding.startSwitch.isChecked()) { + binding.modelSpinner.setEnabled(false); + stateMachine.startCapture(); + } else { + binding.modelSpinner.setEnabled(true); + stateMachine.cancel(); + resetUiToIdle(); + } + }); + } + + private void resetUiToIdle() { + updateCommandText(getString(R.string.cart_sim_idle)); + updateDebugInfo(FollowState.IDLE, new Control(0f, 0f), 0, 0f); + if (binding != null) { + binding.confirmPanel.setVisibility(View.GONE); + binding.countdownText.setVisibility(View.GONE); + binding.trackingOverlay.postInvalidate(); + } + } + + protected void onInferenceConfigurationChanged() { + computingNetwork = false; + if (croppedBitmap == null) return; + final Network.Device device = getDevice(); + final Model model = getModel(); + final int numThreads = getNumThreads(); + runInBackground(() -> recreateNetwork(model, device, numThreads)); + } + + private void recreateNetwork(Model model, Network.Device device, int numThreads) { + if (model == null) return; + Detector newDetector = null; + try { + newDetector = Detector.create(requireActivity(), model, device, numThreads); + } catch (IllegalArgumentException | IOException e) { + Timber.e(e, "Failed to create network."); + String msg = + model.pathType == Model.PATH_TYPE.URL + ? "该模型未下载，请先在主菜单 Model Management 中下载: " + model.name + : "模型加载失败: " + e.getMessage(); + requireActivity() + .runOnUiThread( + () -> + Toast.makeText(requireContext().getApplicationContext(), msg, Toast.LENGTH_LONG) + .show()); + return; + } + + if (detector != null) { + detector.close(); + } + detector = newDetector; + try { + croppedBitmap = + Bitmap.createBitmap( + detector.getImageSizeX(), detector.getImageSizeY(), Bitmap.Config.ARGB_8888); + frameToCropTransform = + ImageUtils.getTransformationMatrix( + getMaxAnalyseImageSize().getWidth(), + getMaxAnalyseImageSize().getHeight(), + croppedBitmap.getWidth(), + croppedBitmap.getHeight(), + sensorOrientation, + detector.getCropRect(), + detector.getMaintainAspect()); + cropToFrameTransform = new Matrix(); + frameToCropTransform.invert(cropToFrameTransform); + } catch (Exception e) { + Timber.e(e, "Failed to configure detector."); + requireActivity() + .runOnUiThread( + () -> + Toast.makeText( + requireContext().getApplicationContext(), + "模型配置失败: " + e.getMessage(), + Toast.LENGTH_LONG) + .show()); + } + } + + @Override + public synchronized void onResume() { + croppedBitmap = null; + handlerThread = new HandlerThread("inference"); + handlerThread.start(); + handler = new Handler(handlerThread.getLooper()); + super.onResume(); + } + + @Override + public synchronized void onPause() { + handlerThread.quitSafely(); + try { + handlerThread.join(); + handlerThread = null; + handler = null; + } catch (final InterruptedException e) { + e.printStackTrace(); + } + super.onPause(); + } + + protected synchronized void runInBackground(final Runnable r) { + if (handler != null) handler.post(r); + } + + @Override + protected void processUSBData(String data) {} + + @Override + protected void processControllerKeyData(String commandType) {} + + @Override + protected void processFrame(Bitmap bitmap, ImageProxy image) { + if (detector == null) { + updateCropImageInfo(); + if (detector == null) return; + } + + ++frameNum; + if (binding == null || !binding.startSwitch.isChecked()) return; + if (computingNetwork) return; + + computingNetwork = true; + runInBackground( + () -> { + final Canvas canvas = new Canvas(croppedBitmap); + Bitmap workingFrame = bitmap; + if (lensFacing == CameraSelector.LENS_FACING_FRONT) { + Bitmap flipped = CameraUtils.flipBitmapHorizontal(bitmap); + canvas.drawBitmap(flipped, frameToCropTransform, null); + workingFrame = flipped; + } else { + canvas.drawBitmap(bitmap, frameToCropTransform, null); + } + + if (detector != null) { + final long startTime = SystemClock.elapsedRealtime(); + final List results = + detector.recognizeImage(croppedBitmap, classType); + lastProcessingTimeMs = SystemClock.elapsedRealtime() - startTime; + + final List mappedRecognitions = new ArrayList<>(); + for (final Detector.Recognition result : results) { + final RectF location = result.getLocation(); + if (location != null && result.getConfidence() >= minConfidence) { + cropToFrameTransform.mapRect(location); + result.setLocation(location); + mappedRecognitions.add(result); + } + } + + int frameW = getMaxAnalyseImageSize().getWidth(); + int frameH = getMaxAnalyseImageSize().getHeight(); + FollowStateMachine.FrameResult fr = + stateMachine.onFrame( + mappedRecognitions, workingFrame, frameW, frameH, sensorOrientation); + + updateDrawState(fr, frameW, frameH, sensorOrientation); + updateCommandText(commandForState(fr)); + float fps = lastProcessingTimeMs > 0 ? 1000f / lastProcessingTimeMs : 0f; + updateDebugInfo(fr.state, fr.control, fr.persons.size(), fps); + updateUiForState(fr); + binding.trackingOverlay.postInvalidate(); + } + computingNetwork = false; + }); + } + + private void updateCropImageInfo() { + sensorOrientation = 90 - ImageUtils.getScreenOrientation(requireActivity()); + recreateNetwork(getModel(), getDevice(), getNumThreads()); + } + + private synchronized void updateDrawState( + FollowStateMachine.FrameResult fr, int frameW, int frameH, int sensorOrientation) { + drawBoxes.clear(); + for (Detector.Recognition r : fr.persons) { + if (r == null || r.getLocation() == null) continue; + int colorType = COLOR_NORMAL; + if (r == fr.target) { + colorType = fr.matched ? COLOR_TARGET : COLOR_FAIL; + } else if (r == fr.candidate) { + colorType = COLOR_CANDIDATE; + } + drawBoxes.add(new DrawBox(new RectF(r.getLocation()), colorType)); + } + drawFrameWidth = frameW; + drawFrameHeight = frameH; + drawSensorOrientation = sensorOrientation; + } + + private void drawOverlay(Canvas canvas) { + if (drawFrameWidth <= 0 || drawFrameHeight <= 0) return; + final boolean rotated = drawSensorOrientation % 180 == 90; + final float multiplier = + Math.min( + canvas.getHeight() / (float) (rotated ? drawFrameWidth : drawFrameHeight), + canvas.getWidth() / (float) (rotated ? drawFrameHeight : drawFrameWidth)); + Matrix matrix = + ImageUtils.getTransformationMatrix( + drawFrameWidth, + drawFrameHeight, + (int) (multiplier * (rotated ? drawFrameHeight : drawFrameWidth)), + (int) (multiplier * (rotated ? drawFrameWidth : drawFrameHeight)), + drawSensorOrientation, + new RectF(0, 0, 0, 0), + false); + + List snapshot; + synchronized (this) { + snapshot = new ArrayList<>(drawBoxes); + } + for (DrawBox box : snapshot) { + RectF rect = new RectF(box.location); + matrix.mapRect(rect); + Paint paint; + String label = null; + switch (box.colorType) { + case COLOR_TARGET: + paint = targetBoxPaint; + label = "目标"; + break; + case COLOR_CANDIDATE: + paint = candidateBoxPaint; + label = "候选"; + break; + case COLOR_FAIL: + paint = failBoxPaint; + label = "匹配失败"; + break; + default: + paint = personBoxPaint; + break; + } + float cornerSize = Math.min(rect.width(), rect.height()) / 8.0f; + canvas.drawRoundRect(rect, cornerSize, cornerSize, paint); + if (label != null) { + canvas.drawText(label, rect.left + cornerSize, rect.top, boxTextPaint); + } + } + } + + private String commandForState(FollowStateMachine.FrameResult fr) { + switch (fr.state) { + case IDLE: + return "待命，打开 Start 开始采集目标"; + case CAPTURE_TARGET: + return "采集中，请保持站立"; + case LOCKED_PENDING_CONFIRM: + return "请确认是否跟随此人"; + case CONFIRMED_ARMED: + return "已确认，请回到车前"; + case REACQUIRE_TARGET: + return "重识别中…"; + case READY_TO_FOLLOW: + return fr.countdownSec >= 0 ? fr.countdownSec + " 秒后启动" : "准备启动"; + case FOLLOW: + return interpreter.interpret(fr.control, fr.state, fr.tooClose); + case LOST: + return "目标丢失，请停止"; + case SEARCH: + return "原地搜索中…"; + case STOP: + return "已停止"; + default: + return "请停止"; + } + } + + private void updateCommandText(String text) { + if (binding == null) return; + requireActivity().runOnUiThread(() -> binding.commandText.setText(text)); + } + + private void updateUiForState(FollowStateMachine.FrameResult fr) { + if (binding == null) return; + requireActivity() + .runOnUiThread( + () -> { + if (binding == null) return; + boolean showConfirm = fr.state == FollowState.LOCKED_PENDING_CONFIRM; + binding.confirmPanel.setVisibility(showConfirm ? View.VISIBLE : View.GONE); + if (showConfirm && fr.snapshot != null) { + binding.snapshotView.setImageBitmap(fr.snapshot); + } + boolean showCountdown = fr.state == FollowState.READY_TO_FOLLOW; + binding.countdownText.setVisibility(showCountdown ? View.VISIBLE : View.GONE); + if (showCountdown) { + binding.countdownText.setText( + fr.countdownSec >= 0 ? String.valueOf(fr.countdownSec) : ""); + } + }); + } + + private void updateDebugInfo(FollowState state, Control control, int persons, float fps) { + if (binding == null) return; + float forward = (control.getLeft() + control.getRight()) / 2f; + float turn = (control.getRight() - control.getLeft()) / 2f; + String info = + String.format( + Locale.US, + "state=%s\nforward=%.2f\nturn=%.2f\nleft=%.2f\nright=%.2f\npersons=%d\nfps=%.1f", + state.name(), + forward, + turn, + control.getLeft(), + control.getRight(), + persons, + fps); + requireActivity().runOnUiThread(() -> binding.debugInfo.setText(info)); + } + + protected Model getModel() { + return model; + } + + @Override + protected void setModel(Model model) { + if (this.model != model) { + this.model = model; + preferencesManager.setObjectNavModel(model.name); + onInferenceConfigurationChanged(); + } + } + + protected Network.Device getDevice() { + return device; + } + + protected int getNumThreads() { + return numThreads; + } + + private static class DrawBox { + final RectF location; + final int colorType; + + DrawBox(RectF location, int colorType) { + this.location = location; + this.colorType = colorType; + } + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/HumanCommandInterpreter.java b/android/robot/src/main/java/org/openbot/cartfollow/HumanCommandInterpreter.java new file mode 100644 index 000000000..ed8e664c6 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/HumanCommandInterpreter.java @@ -0,0 +1,37 @@ +package org.openbot.cartfollow; + +import org.openbot.vehicle.Control; + +public class HumanCommandInterpreter { + + public static final String CMD_FORWARD = "请向前"; + public static final String CMD_FORWARD_LEFT = "请向前并左转"; + public static final String CMD_FORWARD_RIGHT = "请向前并右转"; + public static final String CMD_TURN_LEFT = "请原地左转"; + public static final String CMD_TURN_RIGHT = "请原地右转"; + public static final String CMD_STOP = "请停止"; + public static final String CMD_LOST = "目标丢失，请停止"; + public static final String CMD_TOO_CLOSE = "目标太近，请停止"; + + public float FORWARD_THRESH = 0.2f; + public float TURN_THRESH = 0.2f; + + public String interpret(Control control, FollowState state, boolean tooClose) { + if (state == FollowState.LOST) return CMD_LOST; + if (tooClose) return CMD_TOO_CLOSE; + + float forward = (control.getLeft() + control.getRight()) / 2f; + float turn = (control.getRight() - control.getLeft()) / 2f; + + boolean movingForward = forward > FORWARD_THRESH; + boolean turningLeft = turn < -TURN_THRESH; + boolean turningRight = turn > TURN_THRESH; + + if (movingForward && !turningLeft && !turningRight) return CMD_FORWARD; + if (movingForward && turningLeft) return CMD_FORWARD_LEFT; + if (movingForward && turningRight) return CMD_FORWARD_RIGHT; + if (turningLeft) return CMD_TURN_LEFT; + if (turningRight) return CMD_TURN_RIGHT; + return CMD_STOP; + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/ReIDFeatureExtractor.java b/android/robot/src/main/java/org/openbot/cartfollow/ReIDFeatureExtractor.java new file mode 100644 index 000000000..9cfdb0a00 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/ReIDFeatureExtractor.java @@ -0,0 +1,5 @@ +package org.openbot.cartfollow; + +public interface ReIDFeatureExtractor { + float[] extract(android.graphics.Bitmap personCrop); +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/TargetMatcher.java b/android/robot/src/main/java/org/openbot/cartfollow/TargetMatcher.java new file mode 100644 index 000000000..cb445c580 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/TargetMatcher.java @@ -0,0 +1,79 @@ +package org.openbot.cartfollow; + +import android.graphics.Bitmap; +import android.graphics.RectF; +import java.util.List; +import org.openbot.tflite.Detector.Recognition; + +public class TargetMatcher { + + public static class MatchResult { + public final Recognition best; + public final float score; + public final boolean matched; + + public MatchResult(Recognition best, float score, boolean matched) { + this.best = best; + this.score = score; + this.matched = matched; + } + } + + public float W_POSITION = 0.40f; + public float W_SIZE = 0.20f; + public float W_COLOR = 0.30f; + public float W_CONFIDENCE = 0.10f; + public float MATCH_THRESHOLD = 0.5f; + + public MatchResult match( + List persons, Bitmap frame, TargetMemory memory, int frameW, int frameH) { + if (persons == null || persons.isEmpty() || memory == null || memory.isEmpty()) { + return new MatchResult(null, 0f, false); + } + RectF refBbox = memory.getLastBbox(); + float refArea = memory.getLastArea(); + float imgDiag = (float) Math.sqrt((double) frameW * frameW + (double) frameH * frameH); + + Recognition best = null; + float bestScore = -1f; + for (Recognition r : persons) { + if (r == null || r.getLocation() == null) continue; + RectF b = r.getLocation(); + float score = scoreOne(b, r.getConfidence(), frame, memory, refBbox, refArea, imgDiag); + if (score > bestScore) { + bestScore = score; + best = r; + } + } + boolean matched = best != null && bestScore >= MATCH_THRESHOLD; + return new MatchResult(best, Math.max(0f, bestScore), matched); + } + + private float scoreOne( + RectF b, + Float confidence, + Bitmap frame, + TargetMemory memory, + RectF refBbox, + float refArea, + float imgDiag) { + float cx = b.centerX(); + float cy = b.centerY(); + float refCx = refBbox.centerX(); + float refCy = refBbox.centerY(); + float dist = (float) Math.sqrt((cx - refCx) * (cx - refCx) + (cy - refCy) * (cy - refCy)); + float positionScore = 1f - Math.min(1f, dist / Math.max(1f, imgDiag * 0.3f)); + + float area = b.width() * b.height(); + float sizeScore = 1f - Math.min(1f, Math.abs(area - refArea) / Math.max(1f, refArea)); + + float colorScore = memory.colorScore(frame, b); + + float confScore = confidence == null ? 0f : Math.min(1f, confidence); + + return W_POSITION * positionScore + + W_SIZE * sizeScore + + W_COLOR * colorScore + + W_CONFIDENCE * confScore; + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/TargetMemory.java b/android/robot/src/main/java/org/openbot/cartfollow/TargetMemory.java new file mode 100644 index 000000000..8d35c8b66 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/TargetMemory.java @@ -0,0 +1,143 @@ +package org.openbot.cartfollow; + +import android.graphics.Bitmap; +import android.graphics.Color; +import android.graphics.RectF; +import org.openbot.tflite.Detector.Recognition; + +public class TargetMemory { + private static final int H_BINS = 8; + private static final int S_BINS = 4; + private static final int HIST_SIZE = H_BINS * S_BINS; + + private RectF confirmedBbox; + private float confirmedArea; + private float confirmedAspectRatio; + private float[] upperColorHist; + private float[] lowerColorHist; + + private RectF lastBbox; + private float lastCenterX; + private float lastCenterY; + private float lastArea; + private long lastSeenTimeMs; + + public void captureFromBitmap(Bitmap bitmap, RectF bbox) { + confirmedBbox = new RectF(bbox); + confirmedArea = bbox.width() * bbox.height(); + confirmedAspectRatio = bbox.width() / Math.max(1f, bbox.height()); + upperColorHist = computeHsvHist(bitmap, upperHalf(bbox)); + lowerColorHist = computeHsvHist(bitmap, lowerHalf(bbox)); + lastBbox = new RectF(bbox); + lastCenterX = bbox.centerX(); + lastCenterY = bbox.centerY(); + lastArea = confirmedArea; + lastSeenTimeMs = System.currentTimeMillis(); + } + + public void updateDynamic(Recognition r) { + if (r == null || r.getLocation() == null) return; + RectF b = r.getLocation(); + lastBbox = new RectF(b); + lastCenterX = b.centerX(); + lastCenterY = b.centerY(); + lastArea = b.width() * b.height(); + lastSeenTimeMs = System.currentTimeMillis(); + } + + public void clear() { + confirmedBbox = null; + confirmedArea = 0f; + confirmedAspectRatio = 0f; + upperColorHist = null; + lowerColorHist = null; + lastBbox = null; + lastCenterX = 0f; + lastCenterY = 0f; + lastArea = 0f; + lastSeenTimeMs = 0L; + } + + public boolean isEmpty() { + return confirmedBbox == null; + } + + public RectF getLastBbox() { + return lastBbox != null ? new RectF(lastBbox) : (confirmedBbox != null ? new RectF(confirmedBbox) : null); + } + + public float getLastArea() { + return lastArea > 0 ? lastArea : confirmedArea; + } + + public float[] getUpperColorHist() { + return upperColorHist; + } + + public float[] getLowerColorHist() { + return lowerColorHist; + } + + public float getConfirmedArea() { + return confirmedArea; + } + + private static RectF upperHalf(RectF bbox) { + return new RectF(bbox.left, bbox.top, bbox.right, bbox.top + bbox.height() / 2f); + } + + private static RectF lowerHalf(RectF bbox) { + return new RectF(bbox.left, bbox.top + bbox.height() / 2f, bbox.right, bbox.bottom); + } + + public static float[] computeHsvHist(Bitmap bitmap, RectF rect) { + float[] hist = new float[HIST_SIZE]; + if (bitmap == null || rect == null) return hist; + int bw = bitmap.getWidth(); + int bh = bitmap.getHeight(); + int left = clamp((int) rect.left, 0, bw - 1); + int top = clamp((int) rect.top, 0, bh - 1); + int right = clamp((int) rect.right, 1, bw); + int bottom = clamp((int) rect.bottom, 1, bh); + int w = right - left; + int h = bottom - top; + if (w <= 0 || h <= 0) return hist; + int[] pixels = new int[w * h]; + bitmap.getPixels(pixels, 0, w, left, top, w, h); + float[] hsv = new float[3]; + int count = 0; + for (int px : pixels) { + Color.RGBToHSV(Color.red(px), Color.green(px), Color.blue(px), hsv); + if (hsv[1] < 0.1f) continue; + int hb = (int) (hsv[0] / 360f * H_BINS) % H_BINS; + if (hb < 0) hb += H_BINS; + int sb = Math.min(S_BINS - 1, (int) (hsv[1] * S_BINS)); + hist[hb * S_BINS + sb]++; + count++; + } + if (count > 0) { + for (int i = 0; i < HIST_SIZE; i++) hist[i] /= count; + } + return hist; + } + + public static float histIntersection(float[] a, float[] b) { + if (a == null || b == null || a.length == 0 || b.length == 0) return 0f; + float sum = 0f; + int n = Math.min(a.length, b.length); + for (int i = 0; i < n; i++) sum += Math.min(a[i], b[i]); + return sum; + } + + public float colorScore(Bitmap bitmap, RectF bbox) { + float[] upper = computeHsvHist(bitmap, upperHalf(bbox)); + float[] lower = computeHsvHist(bitmap, lowerHalf(bbox)); + float upperScore = histIntersection(upper, upperColorHist); + float lowerScore = histIntersection(lower, lowerColorHist); + return (upperScore + lowerScore) / 2f; + } + + private static int clamp(int v, int lo, int hi) { + return v < lo ? lo : (v > hi ? hi : v); + } +} diff --git a/android/robot/src/main/java/org/openbot/common/FeatureList.java b/android/robot/src/main/java/org/openbot/common/FeatureList.java index 4c5cdbf81..e023f70b7 100644 --- a/android/robot/src/main/java/org/openbot/common/FeatureList.java +++ b/android/robot/src/main/java/org/openbot/common/FeatureList.java @@ -36,6 +36,7 @@ public class FeatureList { public static final String AUTOPILOT = "Autopilot"; public static final String PERSON_FOLLOWING = "Person Following"; public static final String OBJECT_NAV = "Object Tracking"; + public static final String CART_SIMULATOR = "Cart Simulator"; public static final String MODEL_MANAGEMENT = "Model Management"; public static final String POINT_GOAL_NAVIGATION = "Point Goal Navigation"; public static final String AUTONOMOUS_DRIVING = "Autonomous Driving"; @@ -82,6 +83,7 @@ public static ArrayList getCategories() { subCategories = new ArrayList<>(); subCategories.add(new SubCategory(AUTOPILOT, R.drawable.ic_autopilot, "#44525F")); subCategories.add(new SubCategory(OBJECT_NAV, R.drawable.ic_person_search, "#E7CE88")); + subCategories.add(new SubCategory(CART_SIMULATOR, R.drawable.ic_person_search, "#6BBF8A")); subCategories.add( new SubCategory(POINT_GOAL_NAVIGATION, R.drawable.ic_baseline_golf_course, "#1BBFBF")); subCategories.add(new SubCategory(MODEL_MANAGEMENT, R.drawable.ic_list_bulleted_48, "#BC7680")); diff --git a/android/robot/src/main/java/org/openbot/main/MainFragment.java b/android/robot/src/main/java/org/openbot/main/MainFragment.java index a6c63e40e..8f350de77 100644 --- a/android/robot/src/main/java/org/openbot/main/MainFragment.java +++ b/android/robot/src/main/java/org/openbot/main/MainFragment.java @@ -87,6 +87,11 @@ public void onItemClick(SubCategory subCategory) { .navigate(R.id.action_mainFragment_to_objectNavFragment); break; + case FeatureList.CART_SIMULATOR: + Navigation.findNavController(requireView()) + .navigate(R.id.action_mainFragment_to_cartSimFragment); + break; + case FeatureList.POINT_GOAL_NAVIGATION: Navigation.findNavController(requireView()) .navigate(R.id.action_mainFragment_to_pointGoalNavigationFragment); diff --git a/android/robot/src/main/res/layout/fragment_human_cart_simulator.xml b/android/robot/src/main/res/layout/fragment_human_cart_simulator.xml new file mode 100644 index 000000000..1ad1b22f4 --- /dev/null +++ b/android/robot/src/main/res/layout/fragment_human_cart_simulator.xml @@ -0,0 +1,218 @@ + + + + + + + + + + + + + + + + + + + + + +