From 173ef9666a2476d32e6045606ddd4d9f42424a09 Mon Sep 17 00:00:00 2001 From: Yuan SY Date: Mon, 6 Jul 2026 11:54:41 +0800 Subject: [PATCH 1/5] Add DistanceState and ImageSetpointDistanceEstimator for image-based visual servoing --- .../org/openbot/cartfollow/DistanceState.java | 8 + .../ImageSetpointDistanceEstimator.java | 138 ++++++++++++++++++ 2 files changed, 146 insertions(+) create mode 100644 android/robot/src/main/java/org/openbot/cartfollow/DistanceState.java create mode 100644 android/robot/src/main/java/org/openbot/cartfollow/ImageSetpointDistanceEstimator.java diff --git a/android/robot/src/main/java/org/openbot/cartfollow/DistanceState.java b/android/robot/src/main/java/org/openbot/cartfollow/DistanceState.java new file mode 100644 index 000000000..41cf52878 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/DistanceState.java @@ -0,0 +1,8 @@ +package org.openbot.cartfollow; + +public enum DistanceState { + TOO_FAR, + OK, + TOO_CLOSE, + UNKNOWN +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/ImageSetpointDistanceEstimator.java b/android/robot/src/main/java/org/openbot/cartfollow/ImageSetpointDistanceEstimator.java new file mode 100644 index 000000000..b49b5c57b --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/ImageSetpointDistanceEstimator.java @@ -0,0 +1,138 @@ +package org.openbot.cartfollow; + +import android.graphics.RectF; +import org.openbot.tflite.Detector.Recognition; + +/** + * 基于初始化距离标定的图像伺服距离估计器。 + * + *

核心思想：不恢复真实米制距离，而是比较当前目标图像尺度与初始化时记录的期望图像尺度， + * 输出 {@link DistanceState}（TOO_FAR / OK / TOO_CLOSE / UNKNOWN）。 + * + *

主信号：height_scale = current_height_ratio / desired_height_ratio。 + * 辅信号：area_scale = sqrt(current_area_ratio / desired_area_ratio)。 + * 辅信号：bottom_shift = current_bottom_ratio - desired_bottom_ratio（仅用于显示，不参与判态）。 + */ +public class ImageSetpointDistanceEstimator { + + /** heightScale 低于此值判定 TOO_FAR。 */ + public float FAR_THRESHOLD = 0.85f; + /** heightScale 高于此值判定 TOO_CLOSE。 */ + public float CLOSE_THRESHOLD = 1.15f; + /** height_scale 与 area_scale 的对数差异超过此值时判 UNKNOWN。 */ + public float UNKNOWN_HEIGHT_DISAGREE = 0.3f; + /** bbox 高度占比低于此值时判 UNKNOWN（目标过小，不可信）。 */ + public float MIN_BBOX_HEIGHT_RATIO = 0.1f; + + /** 期望图像尺度（初始化标定值）。 */ + public static class Setpoint { + public final float desiredHeightRatio; + public final float desiredAreaRatio; + public final float desiredBottomRatio; + + public Setpoint(float desiredHeightRatio, float desiredAreaRatio, float desiredBottomRatio) { + this.desiredHeightRatio = desiredHeightRatio; + this.desiredAreaRatio = desiredAreaRatio; + this.desiredBottomRatio = desiredBottomRatio; + } + } + + /** 距离估计结果。 */ + public static class DistanceEstimate { + public final float heightScale; + public final float areaScale; + public final float bottomShift; + public final DistanceState state; + public final float confidence; + public final String failureReason; + + public DistanceEstimate( + float heightScale, + float areaScale, + float bottomShift, + DistanceState state, + float confidence, + String failureReason) { + this.heightScale = heightScale; + this.areaScale = areaScale; + this.bottomShift = bottomShift; + this.state = state; + this.confidence = confidence; + this.failureReason = failureReason; + } + + public static DistanceEstimate unknown(String reason) { + return new DistanceEstimate(0f, 0f, 0f, DistanceState.UNKNOWN, 0f, reason); + } + } + + /** + * 估计当前目标距离状态。 + * + * @param target 当前匹配到的目标 Recognition + * @param frameW 分析帧宽（getMaxAnalyseImageSize） + * @param frameH 分析帧高 + * @param sensorOrientation 传感器方向 + * @param setpoint 初始化标定的期望图像尺度，可为 null + */ + public DistanceEstimate estimate( + Recognition target, int frameW, int frameH, int sensorOrientation, Setpoint setpoint) { + if (target == null || target.getLocation() == null || frameW <= 0 || frameH <= 0) { + return DistanceEstimate.unknown("invalid target or frame size"); + } + if (setpoint == null + || setpoint.desiredHeightRatio <= 0f + || setpoint.desiredAreaRatio <= 0f) { + return DistanceEstimate.unknown("setpoint not initialized"); + } + + RectF loc = target.getLocation(); + boolean rotated = sensorOrientation % 180 == 90; + float imgWidth = rotated ? frameH : frameW; + float imgHeight = rotated ? frameW : frameH; + float boxHeight = rotated ? loc.width() : loc.height(); + float boxWidth = rotated ? loc.height() : loc.width(); + float boxBottom = rotated ? loc.right : loc.bottom; + + if (imgHeight <= 0 || imgWidth <= 0 || boxHeight <= 0) { + return DistanceEstimate.unknown("degenerate bbox or image"); + } + + float currentHeightRatio = boxHeight / imgHeight; + float currentAreaRatio = (boxWidth * boxHeight) / (imgWidth * imgHeight); + float currentBottomRatio = boxBottom / imgHeight; + + if (currentHeightRatio < MIN_BBOX_HEIGHT_RATIO) { + return DistanceEstimate.unknown("bbox too small: " + currentHeightRatio); + } + + float heightScale = currentHeightRatio / setpoint.desiredHeightRatio; + float areaScale = + (float) Math.sqrt(currentAreaRatio / setpoint.desiredAreaRatio); + float bottomShift = currentBottomRatio - setpoint.desiredBottomRatio; + + float logDiff = + (float) Math.abs(Math.log(Math.max(1e-6f, heightScale) / Math.max(1e-6f, areaScale))); + if (logDiff > UNKNOWN_HEIGHT_DISAGREE) { + return new DistanceEstimate( + heightScale, + areaScale, + bottomShift, + DistanceState.UNKNOWN, + 0f, + "height/area disagree: " + logDiff); + } + + DistanceState state; + if (heightScale < FAR_THRESHOLD) { + state = DistanceState.TOO_FAR; + } else if (heightScale > CLOSE_THRESHOLD) { + state = DistanceState.TOO_CLOSE; + } else { + state = DistanceState.OK; + } + + float consistency = 1f - Math.min(1f, Math.abs(heightScale - areaScale)); + return new DistanceEstimate(heightScale, areaScale, bottomShift, state, consistency, null); + } +} From 66bbf12ac7abcda148ebc6456ff11633288c23f1 Mon Sep 17 00:00:00 2001 From: Yuan SY Date: Mon, 6 Jul 2026 11:54:59 +0800 Subject: [PATCH 2/5] Calibrate distance setpoint in TargetMemory and refactor ControlGenerator to DistanceState --- .../openbot/cartfollow/ControlGenerator.java | 69 ++++++++++++++----- .../cartfollow/FollowStateMachine.java | 33 ++++++--- .../org/openbot/cartfollow/TargetMemory.java | 48 +++++++++++++ 3 files changed, 123 insertions(+), 27 deletions(-) diff --git a/android/robot/src/main/java/org/openbot/cartfollow/ControlGenerator.java b/android/robot/src/main/java/org/openbot/cartfollow/ControlGenerator.java index a93e0b2af..a9b395dde 100644 --- a/android/robot/src/main/java/org/openbot/cartfollow/ControlGenerator.java +++ b/android/robot/src/main/java/org/openbot/cartfollow/ControlGenerator.java @@ -13,23 +13,29 @@ public static class Result { public final Recognition target; public final List persons; public final boolean tooClose; + public final ImageSetpointDistanceEstimator.DistanceEstimate distanceEstimate; - public Result(Control control, Recognition target, List persons, boolean tooClose) { + public Result( + Control control, + Recognition target, + List persons, + boolean tooClose, + ImageSetpointDistanceEstimator.DistanceEstimate distanceEstimate) { this.control = control; this.target = target; this.persons = persons; this.tooClose = tooClose; + this.distanceEstimate = distanceEstimate; } } 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 final ImageSetpointDistanceEstimator distanceEstimator = new ImageSetpointDistanceEstimator(); + public Result generate(List results, int frameW, int frameH, int sensorOrientation) { List persons = new ArrayList<>(); if (results != null) { @@ -42,7 +48,7 @@ public Result generate(List results, int frameW, int frameH, int se } if (persons.isEmpty() || frameW <= 0 || frameH <= 0) { - return new Result(new Control(0f, 0f), null, persons, false); + return new Result(new Control(0f, 0f), null, persons, false, null); } Recognition target = null; @@ -56,13 +62,23 @@ public Result generate(List results, int frameW, int frameH, int se } } - return generateFromTarget(target, persons, frameW, frameH, sensorOrientation); + return generateFromTarget(target, persons, frameW, frameH, sensorOrientation, null); } public Result generateFromTarget( - Recognition target, List persons, int frameW, int frameH, int sensorOrientation) { + Recognition target, + List persons, + int frameW, + int frameH, + int sensorOrientation, + TargetMemory memory) { if (target == null || target.getLocation() == null || frameW <= 0 || frameH <= 0) { - return new Result(new Control(0f, 0f), null, persons == null ? new ArrayList<>() : persons, false); + return new Result( + new Control(0f, 0f), + null, + persons == null ? new ArrayList<>() : persons, + false, + null); } RectF loc = target.getLocation(); @@ -70,23 +86,42 @@ public Result generateFromTarget( 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; + + ImageSetpointDistanceEstimator.Setpoint setpoint = + memory == null ? null : memory.getDistanceSetpoint(); + ImageSetpointDistanceEstimator.DistanceEstimate est = + distanceEstimator.estimate(target, frameW, frameH, sensorOrientation, setpoint); + + float forward; + boolean tooClose; + switch (est.state) { + case TOO_FAR: + forward = MAX_FORWARD; + tooClose = false; + break; + case TOO_CLOSE: + forward = 0f; + tooClose = true; + break; + case OK: + forward = 0f; + tooClose = false; + break; + case UNKNOWN: + default: + forward = 0f; + tooClose = false; + break; + } 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); + return new Result(new Control(left, right), target, persons, tooClose, est); } } diff --git a/android/robot/src/main/java/org/openbot/cartfollow/FollowStateMachine.java b/android/robot/src/main/java/org/openbot/cartfollow/FollowStateMachine.java index 39c8bc155..c635798ae 100644 --- a/android/robot/src/main/java/org/openbot/cartfollow/FollowStateMachine.java +++ b/android/robot/src/main/java/org/openbot/cartfollow/FollowStateMachine.java @@ -19,6 +19,7 @@ public static class FrameResult { public final boolean tooClose; public final Bitmap snapshot; public final int countdownSec; + public ImageSetpointDistanceEstimator.DistanceEstimate distanceEstimate; public FrameResult( FollowState state, @@ -126,7 +127,7 @@ public FrameResult onFrame( } captureCount++; if (captureCount >= CAPTURE_FRAMES) { - memory.captureFromBitmap(frame, cand.getLocation()); + memory.captureFromBitmap(frame, cand.getLocation(), frameW, frameH, sensorOrientation); snapshot = cropSnapshot(frame, cand.getLocation()); captureCount = 0; state = FollowState.LOCKED_PENDING_CONFIRM; @@ -181,9 +182,13 @@ public FrameResult onFrame( 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); + controlGenerator.generateFromTarget( + m.best, safePersons, frameW, frameH, sensorOrientation, memory); + FrameResult fr = + new FrameResult( + FollowState.FOLLOW, res.control, m.best, null, safePersons, true, res.tooClose, null, -1); + fr.distanceEstimate = res.distanceEstimate; + return fr; } lostCount++; if (lostCount >= FOLLOW_LOST_M) { @@ -201,9 +206,13 @@ public FrameResult onFrame( 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); + controlGenerator.generateFromTarget( + m.best, safePersons, frameW, frameH, sensorOrientation, memory); + FrameResult fr = + new FrameResult( + FollowState.FOLLOW, res.control, m.best, null, safePersons, true, res.tooClose, null, -1); + fr.distanceEstimate = res.distanceEstimate; + return fr; } if (now - stateEnterTime >= LOST_TO_SEARCH_MS) { state = FollowState.SEARCH; @@ -220,9 +229,13 @@ public FrameResult onFrame( 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); + controlGenerator.generateFromTarget( + m.best, safePersons, frameW, frameH, sensorOrientation, memory); + FrameResult fr = + new FrameResult( + FollowState.FOLLOW, res.control, m.best, null, safePersons, true, res.tooClose, null, -1); + fr.distanceEstimate = res.distanceEstimate; + return fr; } if (now - stateEnterTime >= SEARCH_TIMEOUT_MS) { state = FollowState.STOP; diff --git a/android/robot/src/main/java/org/openbot/cartfollow/TargetMemory.java b/android/robot/src/main/java/org/openbot/cartfollow/TargetMemory.java index 8d35c8b66..2953b609e 100644 --- a/android/robot/src/main/java/org/openbot/cartfollow/TargetMemory.java +++ b/android/robot/src/main/java/org/openbot/cartfollow/TargetMemory.java @@ -16,6 +16,10 @@ public class TargetMemory { private float[] upperColorHist; private float[] lowerColorHist; + private float desiredHeightRatio; + private float desiredAreaRatio; + private float desiredBottomRatio; + private RectF lastBbox; private float lastCenterX; private float lastCenterY; @@ -23,6 +27,11 @@ public class TargetMemory { private long lastSeenTimeMs; public void captureFromBitmap(Bitmap bitmap, RectF bbox) { + captureFromBitmap(bitmap, bbox, 0, 0, 0); + } + + public void captureFromBitmap( + Bitmap bitmap, RectF bbox, int frameW, int frameH, int sensorOrientation) { confirmedBbox = new RectF(bbox); confirmedArea = bbox.width() * bbox.height(); confirmedAspectRatio = bbox.width() / Math.max(1f, bbox.height()); @@ -33,6 +42,42 @@ public void captureFromBitmap(Bitmap bitmap, RectF bbox) { lastCenterY = bbox.centerY(); lastArea = confirmedArea; lastSeenTimeMs = System.currentTimeMillis(); + computeDistanceSetpoint(bbox, frameW, frameH, sensorOrientation); + } + + private void computeDistanceSetpoint( + RectF bbox, int frameW, int frameH, int sensorOrientation) { + if (frameW <= 0 || frameH <= 0 || bbox == null) { + desiredHeightRatio = 0f; + desiredAreaRatio = 0f; + desiredBottomRatio = 0f; + return; + } + boolean rotated = sensorOrientation % 180 == 90; + float imgWidth = rotated ? frameH : frameW; + float imgHeight = rotated ? frameW : frameH; + if (imgWidth <= 0 || imgHeight <= 0) { + desiredHeightRatio = 0f; + desiredAreaRatio = 0f; + desiredBottomRatio = 0f; + return; + } + float boxHeight = rotated ? bbox.width() : bbox.height(); + float boxWidth = rotated ? bbox.height() : bbox.width(); + float boxBottom = rotated ? bbox.right : bbox.bottom; + desiredHeightRatio = boxHeight / imgHeight; + desiredAreaRatio = (boxWidth * boxHeight) / (imgWidth * imgHeight); + desiredBottomRatio = boxBottom / imgHeight; + } + + public boolean hasDistanceSetpoint() { + return desiredHeightRatio > 0f && desiredAreaRatio > 0f; + } + + public ImageSetpointDistanceEstimator.Setpoint getDistanceSetpoint() { + if (!hasDistanceSetpoint()) return null; + return new ImageSetpointDistanceEstimator.Setpoint( + desiredHeightRatio, desiredAreaRatio, desiredBottomRatio); } public void updateDynamic(Recognition r) { @@ -51,6 +96,9 @@ public void clear() { confirmedAspectRatio = 0f; upperColorHist = null; lowerColorHist = null; + desiredHeightRatio = 0f; + desiredAreaRatio = 0f; + desiredBottomRatio = 0f; lastBbox = null; lastCenterX = 0f; lastCenterY = 0f; From c880025af0ee760d90cdc611227057fa4f969de8 Mon Sep 17 00:00:00 2001 From: Yuan SY Date: Mon, 6 Jul 2026 11:55:17 +0800 Subject: [PATCH 3/5] Display distance state and scales in Human Cart Simulator --- .../HumanCartSimulatorFragment.java | 33 ++++++++++++++++--- .../cartfollow/HumanCommandInterpreter.java | 30 ++++++++++++++++- 2 files changed, 57 insertions(+), 6 deletions(-) diff --git a/android/robot/src/main/java/org/openbot/cartfollow/HumanCartSimulatorFragment.java b/android/robot/src/main/java/org/openbot/cartfollow/HumanCartSimulatorFragment.java index c282e063c..46c3fd54b 100644 --- a/android/robot/src/main/java/org/openbot/cartfollow/HumanCartSimulatorFragment.java +++ b/android/robot/src/main/java/org/openbot/cartfollow/HumanCartSimulatorFragment.java @@ -157,7 +157,7 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat private void resetUiToIdle() { updateCommandText(getString(R.string.cart_sim_idle)); - updateDebugInfo(FollowState.IDLE, new Control(0f, 0f), 0, 0f); + updateDebugInfo(FollowState.IDLE, new Control(0f, 0f), 0, 0f, null); if (binding != null) { binding.confirmPanel.setVisibility(View.GONE); binding.countdownText.setVisibility(View.GONE); @@ -306,7 +306,7 @@ protected void processFrame(Bitmap bitmap, ImageProxy image) { updateDrawState(fr, frameW, frameH, sensorOrientation); updateCommandText(commandForState(fr)); float fps = lastProcessingTimeMs > 0 ? 1000f / lastProcessingTimeMs : 0f; - updateDebugInfo(fr.state, fr.control, fr.persons.size(), fps); + updateDebugInfo(fr.state, fr.control, fr.persons.size(), fps, fr.distanceEstimate); updateUiForState(fr); binding.trackingOverlay.postInvalidate(); } @@ -403,6 +403,9 @@ private String commandForState(FollowStateMachine.FrameResult fr) { case READY_TO_FOLLOW: return fr.countdownSec >= 0 ? fr.countdownSec + " 秒后启动" : "准备启动"; case FOLLOW: + if (fr.distanceEstimate != null) { + return interpreter.interpret(fr.control, fr.state, fr.distanceEstimate.state); + } return interpreter.interpret(fr.control, fr.state, fr.tooClose); case LOST: return "目标丢失，请停止"; @@ -440,21 +443,41 @@ private void updateUiForState(FollowStateMachine.FrameResult fr) { }); } - private void updateDebugInfo(FollowState state, Control control, int persons, float fps) { + private void updateDebugInfo( + FollowState state, + Control control, + int persons, + float fps, + ImageSetpointDistanceEstimator.DistanceEstimate dist) { if (binding == null) return; float forward = (control.getLeft() + control.getRight()) / 2f; float turn = (control.getRight() - control.getLeft()) / 2f; + String distLine; + if (dist != null) { + distLine = + String.format( + Locale.US, + "dist=%s\nhScale=%.2f\naScale=%.2f\nbShift=%+.3f\ndistConf=%.2f", + dist.state.name(), + dist.heightScale, + dist.areaScale, + dist.bottomShift, + dist.confidence); + } else { + distLine = "dist=-"; + } String info = String.format( Locale.US, - "state=%s\nforward=%.2f\nturn=%.2f\nleft=%.2f\nright=%.2f\npersons=%d\nfps=%.1f", + "state=%s\nforward=%.2f\nturn=%.2f\nleft=%.2f\nright=%.2f\npersons=%d\nfps=%.1f\n%s", state.name(), forward, turn, control.getLeft(), control.getRight(), persons, - fps); + fps, + distLine); requireActivity().runOnUiThread(() -> binding.debugInfo.setText(info)); } diff --git a/android/robot/src/main/java/org/openbot/cartfollow/HumanCommandInterpreter.java b/android/robot/src/main/java/org/openbot/cartfollow/HumanCommandInterpreter.java index ed8e664c6..e00c163b0 100644 --- a/android/robot/src/main/java/org/openbot/cartfollow/HumanCommandInterpreter.java +++ b/android/robot/src/main/java/org/openbot/cartfollow/HumanCommandInterpreter.java @@ -4,14 +4,16 @@ public class HumanCommandInterpreter { - public static final String CMD_FORWARD = "请向前"; + 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_KEEP = "保持距离"; public static final String CMD_LOST = "目标丢失，请停止"; public static final String CMD_TOO_CLOSE = "目标太近，请停止"; + public static final String CMD_DIST_UNKNOWN = "距离不明，请停止"; public float FORWARD_THRESH = 0.2f; public float TURN_THRESH = 0.2f; @@ -34,4 +36,30 @@ public String interpret(Control control, FollowState state, boolean tooClose) { if (turningRight) return CMD_TURN_RIGHT; return CMD_STOP; } + + public String interpret(Control control, FollowState state, DistanceState distState) { + if (state == FollowState.LOST) return CMD_LOST; + if (distState == DistanceState.UNKNOWN) return CMD_DIST_UNKNOWN; + if (distState == DistanceState.TOO_CLOSE) 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 (distState == DistanceState.OK) { + if (turningLeft) return CMD_TURN_LEFT; + if (turningRight) return CMD_TURN_RIGHT; + return CMD_KEEP; + } + + 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_FORWARD; + } } From 4e0927c22002da55d179b2b2738ebfed9e34fedb Mon Sep 17 00:00:00 2001 From: Yuan SY Date: Mon, 6 Jul 2026 11:59:03 +0800 Subject: [PATCH 4/5] Update cartfollow devlog for Phase 2 distance control completion --- android/cartfollow-devlog.md | 117 +++++++++++++++++++++-------------- 1 file changed, 69 insertions(+), 48 deletions(-) diff --git a/android/cartfollow-devlog.md b/android/cartfollow-devlog.md index 95c5d9fea..259d63008 100644 --- a/android/cartfollow-devlog.md +++ b/android/cartfollow-devlog.md @@ -15,14 +15,16 @@ Human Cart Simulator 是购物车跟随功能的上位机核心模块，在 Open | 文件 | 行数 | 作用 | |------|------|------| -| `HumanCartSimulatorFragment.java` | 491 | 主 UI Fragment：摄像头预览 + 检测框绘制 + 确认/重拍/取消 + 倒计时 + 调试信息显示 | -| `ControlGenerator.java` | 92 | 控制算法：从指定目标生成 `Control(left,right)`，支持 `generateFromTarget()` | +| `HumanCartSimulatorFragment.java` | 464 | 主 UI Fragment：摄像头预览 + 检测框绘制 + 确认/重拍/取消 + 倒计时 + 距离调试显示 | +| `ControlGenerator.java` | 110 | 控制算法：基于 DistanceState 决定 forward，转向由 xError 决定 | +| `FollowStateMachine.java` | 262 | 完整状态机：管理两阶段目标初始化、重识别、倒计时、跟随、丢失、搜索、停止 | | `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 颜色直方图、动态位置 | +| `DistanceState.java` | 7 | 距离状态枚举：`TOO_FAR / OK / TOO_CLOSE / UNKNOWN` | +| `ImageSetpointDistanceEstimator.java` | 112 | 图像伺服距离估计器：基于初始化 setpoint 输出 height_scale / area_scale / bottom_shift / state / confidence | +| `TargetMemory.java` | 167 | 目标记忆：confirmedBbox、面积、上下身 HSV 颜色直方图、动态位置、距离 setpoint | | `TargetMatcher.java` | 79 | 目标匹配：position + size + color + confidence 融合评分（ReID 接口预留） | | `ReIDFeatureExtractor.java` | 5 | ReID 接口占位，尚未接入真实 embedding 推理 | -| `HumanCommandInterpreter.java` | 37 | 中文指令解释器：将 Control + 状态翻译为人可读的调试指令 | +| `HumanCommandInterpreter.java` | 52 | 中文指令解释器：支持 DistanceState 重载，输出距离感知指令 | | `fragment_human_cart_simulator.xml` | 218 | 布局文件：OverlayView + 指令文本 + 快照确认面板 + 倒计时 + 调试信息 + 底部面板 | ### 集成点（在 OpenBot App 中的入口） @@ -38,49 +40,62 @@ Human Cart Simulator 是购物车跟随功能的上位机核心模块，在 Open ## 2. 当前实现状态 -### 2.1 已完成（commit `dd6aa95` + `409d85f` + `4da208a`） +### 2.1 已完成（commit `dd6aa95` + `409d85f` + `4da208a` + Phase 2） | 功能 | 状态 | 说明 | |------|------|------| | 人物检测（MobileNet-SSD） | 已完成 | 复用 OpenBot 现有 `Detector`，筛选 `classType="person"` | -| 两阶段目标初始化 | 已完成 | `CAPTURE_TARGET → LOCKED_PENDING_CONFIRM → CONFIRMED_ARMED`，采集时记录 confirmedBbox、面积、上下身颜色直方图，截图供用户确认 | +| 两阶段目标初始化 | 已完成 | `CAPTURE_TARGET → LOCKED_PENDING_CONFIRM → CONFIRMED_ARMED`，采集时记录 confirmedBbox、面积、上下身颜色直方图、距离 setpoint，截图供用户确认 | | 用户确认 / 重拍 / 取消 | 已完成 | 确认面板含快照预览与三按钮，状态切换正确 | -| 目标记忆 `TargetMemory` | 已完成 | 保存 confirmedBbox、confirmedArea、上下身 HSV 直方图、动态 lastBbox/lastCenter/lastArea/lastSeenTime | +| 目标记忆 `TargetMemory` | 已完成 | 保存 confirmedBbox、confirmedArea、上下身 HSV 直方图、动态 lastBbox/lastCenter/lastArea/lastSeenTime、距离 setpoint（desiredHeightRatio/areaRatio/bottomRatio） | | 目标匹配 `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)` | +| **初始化距离标定 + 图像伺服** | 已完成（Phase 2） | `ImageSetpointDistanceEstimator` 基于采集时记录的 setpoint 输出 `height_scale / area_scale / bottom_shift`，无需恢复真实米制距离 | +| **DistanceState 输出** | 已完成（Phase 2） | 输出 `TOO_FAR / OK / TOO_CLOSE / UNKNOWN`，替代线性 distError；UNKNOWN 时停车 | +| **ControlGenerator 基于 DistanceState** | 已完成（Phase 2） | forward 由 DistanceState 决定：TOO_FAR→MAX_FORWARD，其余→0；移除硬编码 TARGET_H_RATIO/K_DIST/TOO_CLOSE_H_RATIO | +| **距离调试显示** | 已完成（Phase 2） | Simulator 显示 `dist / hScale / aScale / bShift / distConf` | +| **距离感知指令** | 已完成（Phase 2） | `HumanCommandInterpreter` 新增 DistanceState 重载：OK→"保持距离"、UNKNOWN→"距离不明，请停止" | | 转向方向修正 | 已完成 | `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 | -| 中文指令提示 | 已完成 | 显示"请向前"等中文建议指令（调试用途） | +| 调试信息面板 | 已完成 | 显示 state / forward / turn / left / right / persons / fps / dist / hScale / aScale / bShift / distConf | | 导航集成 | 已完成 | 已注册到主菜单 "Cart Simulator" 入口 | -### 2.2 核心控制算法（当前状态，Phase 2 将重构） +### 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 +输入：匹配目标 bbox + 画面尺寸 + 传感器角度 + TargetMemory(setpoint) +输出：Control(left, right) + DistanceEstimate + +距离估计（ImageSetpointDistanceEstimator）： + 1. 处理 sensorOrientation 旋转 + 2. currentHeightRatio = boxHeight / imgHeight + currentAreaRatio = boxArea / (imgW * imgH) + currentBottomRatio = boxBottom / imgHeight + 3. heightScale = currentHeightRatio / desiredHeightRatio + areaScale = sqrt(currentAreaRatio / desiredAreaRatio) + bottomShift = currentBottomRatio - desiredBottomRatio + 4. 校验：bbox 过小 / height_scale 与 area_scale 对数差异过大 → UNKNOWN + 5. 判态：heightScale < 0.85 → TOO_FAR + heightScale > 1.15 → TOO_CLOSE + 否则 → OK + +控制生成（ControlGenerator）： + xError = target_centerX / imgWidth - 0.5 + turn = K_TURN × xError × (FLIP_TURN ? -1 : 1) + forward = + TOO_FAR → MAX_FORWARD + OK → 0 + TOO_CLOSE→ 0 （首版不主动后退） + UNKNOWN → 0 （不确定就停） + left = forward - turn, right = forward + turn ``` 当前可调参数（`ControlGenerator`）： @@ -88,13 +103,19 @@ Human Cart Simulator 是购物车跟随功能的上位机核心模块，在 Open | 参数 | 默认值 | 含义 | |------|--------|------| | `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 | 最大前进速度 | +| `MAX_FORWARD` | 0.6 | TOO_FAR 时的固定前进速度 | | `MIN_CONFIDENCE` | 0.5 | 最小检测置信度 | | `FLIP_TURN` | true | 转向方向翻转 | +距离估计参数（`ImageSetpointDistanceEstimator`）： + +| 参数 | 默认值 | 含义 | +|------|--------|------| +| `FAR_THRESHOLD` | 0.85 | heightScale 低于此值判定 TOO_FAR | +| `CLOSE_THRESHOLD` | 1.15 | heightScale 高于此值判定 TOO_CLOSE | +| `UNKNOWN_HEIGHT_DISAGREE` | 0.3 | height/area 对数差异上限 | +| `MIN_BBOX_HEIGHT_RATIO` | 0.1 | bbox 高度占比下限 | + 当前状态机参数（`FollowStateMachine`）： | 参数 | 默认值 | 含义 | @@ -114,13 +135,11 @@ Human Cart Simulator 是购物车跟随功能的上位机核心模块，在 Open | 功能 | 优先级 | 说明 | |------|--------|------| -| **初始化距离标定 + 图像伺服** | 高 | 当前 `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 强确认 | +| **目标重锁定增强** | 中 | 当前 LOST/SEARCH 恢复复用 TargetMatcher，未加入 ReID 强确认。阶段3 处理 | | **参数持久化** | 低 | 当前调参仅内存生效，重启恢复默认 | -| **参数 UI 面板** | 低 | K_TURN / MAX_FORWARD 等参数需通过代码修改，没有 UI 界面 | +| **参数 UI 面板** | 低 | K_TURN / MAX_FORWARD / 阈值等参数需通过代码修改，没有 UI 界面 | +| **bottomShift 参与判态** | 低 | 当前 bottomShift 仅用于显示，未参与距离状态判断。待 90° 旋转下方向实测验证后决定是否纳入 | ### 3.2 状态机（已完整实现） @@ -149,9 +168,8 @@ STOP ──(用户重新开始)──→ CAPTURE_TARGET | 问题 | 说明 | |------|------| | 目标选择策略仍依赖位置+颜色 | TargetMatcher 未接入 ReID，多人长时间交叉下可能误锁。阶段3 处理 | -| 距离控制硬编码 setpoint | `TARGET_H_RATIO=0.5` 不随用户身高/采集距离自适应。Phase 2 解决 | -| forward 限幅非对称 | `forward = max(0, min(forward, MAX_FORWARD))` — 只允许前进，不允许后退。安全优先，合理 | -| sensorOrientation 处理 | 横竖屏切换时 target 位置计算需实测验证；bottom_shift 在旋转下方向待验证 | +| forward 限幅非对称 | forward 只允许 ≥0，不允许后退。安全优先，合理 | +| bottomShift 旋转方向待验证 | 90° 旋转下 boxBottom 映射方向需实测确认，当前仅显示不参与判态 | --- @@ -178,16 +196,16 @@ STOP ──(用户重新开始)──→ CAPTURE_TARGET - [x] Human Cart Simulator 实时提示与调试显示 - [ ] 多人干扰不切换目标（部分保证，待 ReID 增强） -### Phase 2：修正跟随距离控制（进行中） +### 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 目标距离标定验证 +- [x] 新增 `DistanceState` 枚举（`TOO_FAR / OK / TOO_CLOSE / UNKNOWN`） +- [x] 新增 `ImageSetpointDistanceEstimator`，输出 height_scale / area_scale / bottom_shift / state / confidence +- [x] `TargetMemory` 采集时记录 `desired_bbox_height_ratio / area_ratio / bottom_ratio` +- [x] 重构 `ControlGenerator`，forward 由 DistanceState 决定，移除硬编码 setpoint +- [x] `FollowStateMachine.FrameResult` 透传 distanceEstimate +- [x] Human Cart Simulator 显示 dist_state / hScale / aScale / bShift / distConf +- [x] `HumanCommandInterpreter` 纳入 distance state +- [ ] 0.8-1.2 m 目标距离标定验证（待实机/ Simulator 实测） **不在 Phase 2 范围**：`vehicle.setControl()` 接通（阶段6）、ReID 增强（阶段3）、障碍处理（阶段5） @@ -223,6 +241,9 @@ STOP ──(用户重新开始)──→ CAPTURE_TARGET | `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 | +| `173ef96` | 2026-07-06 | Add DistanceState and ImageSetpointDistanceEstimator for image-based visual servoing | +| `66bbf12` | 2026-07-06 | Calibrate distance setpoint in TargetMemory and refactor ControlGenerator to DistanceState | +| `c880025` | 2026-07-06 | Display distance state and scales in Human Cart Simulator | --- From 4dbfc56163ed986a98ea13982988e09aa4d4dfba Mon Sep 17 00:00:00 2001 From: Yuan SY Date: Mon, 6 Jul 2026 14:42:11 +0800 Subject: [PATCH 5/5] Mark Phase 2 distance control verification as passed --- android/cartfollow-devlog.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/android/cartfollow-devlog.md b/android/cartfollow-devlog.md index 259d63008..698cf6e22 100644 --- a/android/cartfollow-devlog.md +++ b/android/cartfollow-devlog.md @@ -205,7 +205,7 @@ STOP ──(用户重新开始)──→ CAPTURE_TARGET - [x] `FollowStateMachine.FrameResult` 透传 distanceEstimate - [x] Human Cart Simulator 显示 dist_state / hScale / aScale / bShift / distConf - [x] `HumanCommandInterpreter` 纳入 distance state -- [ ] 0.8-1.2 m 目标距离标定验证（待实机/ Simulator 实测） +- [x] 0.8-1.2 m 目标距离标定验证（初步测试基本通过，大多数情况下可以把距离保持为初始化时的距离。bShift在人远离的时候会向更负的方向变化，符合预期。） **不在 Phase 2 范围**：`vehicle.setControl()` 接通（阶段6）、ReID 增强（阶段3）、障碍处理（阶段5）