From dd6aa95b7e4dd19fd288b5c1333789c421ceeb38 Mon Sep 17 00:00:00 2001 From: Yuan SY Date: Thu, 2 Jul 2026 15:09:16 +0800 Subject: [PATCH 1/4] Add Human Cart Simulator for shopping cart follow debugging --- .../openbot/cartfollow/ControlGenerator.java | 83 ++++ .../org/openbot/cartfollow/FollowState.java | 9 + .../HumanCartSimulatorFragment.java | 382 ++++++++++++++++++ .../cartfollow/HumanCommandInterpreter.java | 37 ++ .../java/org/openbot/common/FeatureList.java | 2 + .../java/org/openbot/main/MainFragment.java | 5 + .../layout/fragment_human_cart_simulator.xml | 137 +++++++ .../src/main/res/navigation/nav_graph.xml | 9 + android/robot/src/main/res/values/strings.xml | 3 + 9 files changed, 667 insertions(+) create mode 100644 android/robot/src/main/java/org/openbot/cartfollow/ControlGenerator.java create mode 100644 android/robot/src/main/java/org/openbot/cartfollow/FollowState.java create mode 100644 android/robot/src/main/java/org/openbot/cartfollow/HumanCartSimulatorFragment.java create mode 100644 android/robot/src/main/java/org/openbot/cartfollow/HumanCommandInterpreter.java create mode 100644 android/robot/src/main/res/layout/fragment_human_cart_simulator.xml 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..a7a2e7334 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/ControlGenerator.java @@ -0,0 +1,83 @@ +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 = false; + + 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; + } + } + + 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..e6f9ffe72 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/FollowState.java @@ -0,0 +1,9 @@ +package org.openbot.cartfollow; + +public enum FollowState { + IDLE, + FOLLOW, + LOST, + SEARCH, + STOP +} 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..4caa407fa --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/HumanCartSimulatorFragment.java @@ -0,0 +1,382 @@ +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 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 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 personBoxPaint = 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); + personBoxPaint.setColor(Color.WHITE); + personBoxPaint.setStyle(Paint.Style.STROKE); + personBoxPaint.setStrokeWidth(6.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) && f.pathType != Model.PATH_TYPE.URL); + initModelSpinner(binding.modelSpinner, models, preferencesManager.getObjectNavModel()); + + setAnalyserResolution(Enums.Preview.HD.getValue()); + + binding.trackingOverlay.addCallback(canvas -> drawOverlay(canvas)); + + binding.startSwitch.setChecked(false); + binding.startSwitch.setOnClickListener( + v -> { + if (binding.startSwitch.isChecked()) { + binding.modelSpinner.setEnabled(false); + } else { + binding.modelSpinner.setEnabled(true); + updateCommandText(getString(R.string.cart_sim_idle)); + updateDebugInfo(FollowState.IDLE, new Control(0f, 0f), 0, 0f); + } + }); + } + + 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; + if (detector != null) { + detector.close(); + detector = null; + } + try { + detector = Detector.create(requireActivity(), model, device, numThreads); + 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 (IllegalArgumentException | IOException e) { + Timber.e(e, "Failed to create network."); + 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); + if (lensFacing == CameraSelector.LENS_FACING_FRONT) { + canvas.drawBitmap( + CameraUtils.flipBitmapHorizontal(bitmap), frameToCropTransform, null); + } 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(); + ControlGenerator.Result gen = + controlGenerator.generate(mappedRecognitions, frameW, frameH, sensorOrientation); + + FollowState state; + if (gen.target == null) state = FollowState.LOST; + else if (gen.tooClose) state = FollowState.STOP; + else state = FollowState.FOLLOW; + + String command = interpreter.interpret(gen.control, state, gen.tooClose); + + updateDrawState(mappedRecognitions, gen.target, frameW, frameH, sensorOrientation); + updateCommandText(command); + float fps = lastProcessingTimeMs > 0 ? 1000f / lastProcessingTimeMs : 0f; + updateDebugInfo(state, gen.control, gen.persons.size(), fps); + binding.trackingOverlay.postInvalidate(); + } + computingNetwork = false; + }); + } + + private void updateCropImageInfo() { + sensorOrientation = 90 - ImageUtils.getScreenOrientation(requireActivity()); + recreateNetwork(getModel(), getDevice(), getNumThreads()); + } + + private synchronized void updateDrawState( + List recognitions, + Detector.Recognition target, + int frameW, + int frameH, + int sensorOrientation) { + drawBoxes.clear(); + for (Detector.Recognition r : recognitions) { + if (r.getLocation() == null) continue; + drawBoxes.add(new DrawBox(new RectF(r.getLocation()), r == target)); + } + 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 = box.isTarget ? targetBoxPaint : personBoxPaint; + float cornerSize = Math.min(rect.width(), rect.height()) / 8.0f; + canvas.drawRoundRect(rect, cornerSize, cornerSize, paint); + if (box.isTarget) { + canvas.drawText("目标", rect.left + cornerSize, rect.top, boxTextPaint); + } + } + } + + private void updateCommandText(String text) { + if (binding == null) return; + requireActivity().runOnUiThread(() -> binding.commandText.setText(text)); + } + + 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 boolean isTarget; + + DrawBox(RectF location, boolean isTarget) { + this.location = location; + this.isTarget = isTarget; + } + } +} 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/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..a013c5500 --- /dev/null +++ b/android/robot/src/main/res/layout/fragment_human_cart_simulator.xml @@ -0,0 +1,137 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/android/robot/src/main/res/navigation/nav_graph.xml b/android/robot/src/main/res/navigation/nav_graph.xml index 68094be2f..dadbaa40a 100644 --- a/android/robot/src/main/res/navigation/nav_graph.xml +++ b/android/robot/src/main/res/navigation/nav_graph.xml @@ -25,6 +25,9 @@ + @@ -111,6 +114,12 @@ android:label="fragment_object_nav" tools:layout="@layout/fragment_object_nav" /> + + Front LEDs USB Cancel + Cart Simulator + Start + 待命 From 409d85f5be136c3beca0e8aa7c065a169a2ad66b Mon Sep 17 00:00:00 2001 From: Yuan SY Date: Thu, 2 Jul 2026 15:49:18 +0800 Subject: [PATCH 2/4] Fix turn direction, confidence button height, and model selection --- .../openbot/cartfollow/ControlGenerator.java | 2 +- .../HumanCartSimulatorFragment.java | 30 ++++++++++++++----- .../layout/fragment_human_cart_simulator.xml | 4 +-- 3 files changed, 25 insertions(+), 11 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 a7a2e7334..90c0211bf 100644 --- a/android/robot/src/main/java/org/openbot/cartfollow/ControlGenerator.java +++ b/android/robot/src/main/java/org/openbot/cartfollow/ControlGenerator.java @@ -28,7 +28,7 @@ public Result(Control control, Recognition target, List persons, bo public float MAX_FORWARD = 0.6f; public float MIN_CONFIDENCE = 0.5f; public float TOO_CLOSE_H_RATIO = 0.75f; - public boolean FLIP_TURN = false; + public boolean FLIP_TURN = true; public Result generate(List results, int frameW, int frameH, int sensorOrientation) { List persons = new ArrayList<>(); 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 4caa407fa..1f79d3650 100644 --- a/android/robot/src/main/java/org/openbot/cartfollow/HumanCartSimulatorFragment.java +++ b/android/robot/src/main/java/org/openbot/cartfollow/HumanCartSimulatorFragment.java @@ -114,9 +114,7 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat controlGenerator.MIN_CONFIDENCE = minConfidence; }); - List models = - getModelNames( - f -> f.type.equals(Model.TYPE.DETECTOR) && f.pathType != Model.PATH_TYPE.URL); + List models = getModelNames(f -> f.type.equals(Model.TYPE.DETECTOR)); initModelSpinner(binding.modelSpinner, models, preferencesManager.getObjectNavModel()); setAnalyserResolution(Enums.Preview.HD.getValue()); @@ -147,12 +145,28 @@ protected void onInferenceConfigurationChanged() { 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 = null; } + detector = newDetector; try { - detector = Detector.create(requireActivity(), model, device, numThreads); croppedBitmap = Bitmap.createBitmap( detector.getImageSizeX(), detector.getImageSizeY(), Bitmap.Config.ARGB_8888); @@ -167,14 +181,14 @@ private void recreateNetwork(Model model, Network.Device device, int numThreads) detector.getMaintainAspect()); cropToFrameTransform = new Matrix(); frameToCropTransform.invert(cropToFrameTransform); - } catch (IllegalArgumentException | IOException e) { - Timber.e(e, "Failed to create network."); + } catch (Exception e) { + Timber.e(e, "Failed to configure detector."); requireActivity() .runOnUiThread( () -> Toast.makeText( requireContext().getApplicationContext(), - e.getMessage(), + "模型配置失败: " + e.getMessage(), Toast.LENGTH_LONG) .show()); } 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 index a013c5500..bb506180a 100644 --- a/android/robot/src/main/res/layout/fragment_human_cart_simulator.xml +++ b/android/robot/src/main/res/layout/fragment_human_cart_simulator.xml @@ -96,7 +96,7 @@ @@ -114,7 +114,7 @@ From 4da208a32bb05f34bb1346dad374cdef1580c68e Mon Sep 17 00:00:00 2001 From: Yuan SY Date: Thu, 2 Jul 2026 21:27:53 +0800 Subject: [PATCH 3/4] Add two-stage target init, target memory and full follow state machine --- .../openbot/cartfollow/ControlGenerator.java | 9 + .../org/openbot/cartfollow/FollowState.java | 5 + .../cartfollow/FollowStateMachine.java | 277 ++++++++++++++++++ .../HumanCartSimulatorFragment.java | 155 ++++++++-- .../cartfollow/ReIDFeatureExtractor.java | 5 + .../org/openbot/cartfollow/TargetMatcher.java | 79 +++++ .../org/openbot/cartfollow/TargetMemory.java | 143 +++++++++ .../layout/fragment_human_cart_simulator.xml | 81 +++++ 8 files changed, 724 insertions(+), 30 deletions(-) create mode 100644 android/robot/src/main/java/org/openbot/cartfollow/FollowStateMachine.java create mode 100644 android/robot/src/main/java/org/openbot/cartfollow/ReIDFeatureExtractor.java create mode 100644 android/robot/src/main/java/org/openbot/cartfollow/TargetMatcher.java create mode 100644 android/robot/src/main/java/org/openbot/cartfollow/TargetMemory.java 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 90c0211bf..a93e0b2af 100644 --- a/android/robot/src/main/java/org/openbot/cartfollow/ControlGenerator.java +++ b/android/robot/src/main/java/org/openbot/cartfollow/ControlGenerator.java @@ -56,6 +56,15 @@ public Result generate(List results, int frameW, int frameH, int se } } + 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; diff --git a/android/robot/src/main/java/org/openbot/cartfollow/FollowState.java b/android/robot/src/main/java/org/openbot/cartfollow/FollowState.java index e6f9ffe72..aa59c4223 100644 --- a/android/robot/src/main/java/org/openbot/cartfollow/FollowState.java +++ b/android/robot/src/main/java/org/openbot/cartfollow/FollowState.java @@ -2,6 +2,11 @@ public enum FollowState { IDLE, + CAPTURE_TARGET, + LOCKED_PENDING_CONFIRM, + CONFIRMED_ARMED, + REACQUIRE_TARGET, + READY_TO_FOLLOW, FOLLOW, LOST, SEARCH, 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 index 1f79d3650..c282e063c 100644 --- a/android/robot/src/main/java/org/openbot/cartfollow/HumanCartSimulatorFragment.java +++ b/android/robot/src/main/java/org/openbot/cartfollow/HumanCartSimulatorFragment.java @@ -37,6 +37,11 @@ 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; @@ -60,6 +65,9 @@ public class HumanCartSimulatorFragment extends CameraFragment { 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; @@ -67,7 +75,9 @@ public class HumanCartSimulatorFragment extends CameraFragment { 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 @@ -76,9 +86,15 @@ public void onCreate(@Nullable Bundle 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); } @@ -121,19 +137,34 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat 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); - updateCommandText(getString(R.string.cart_sim_idle)); - updateDebugInfo(FollowState.IDLE, new Control(0f, 0f), 0, 0f); + 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; @@ -241,9 +272,11 @@ protected void processFrame(Bitmap bitmap, ImageProxy image) { runInBackground( () -> { final Canvas canvas = new Canvas(croppedBitmap); + Bitmap workingFrame = bitmap; if (lensFacing == CameraSelector.LENS_FACING_FRONT) { - canvas.drawBitmap( - CameraUtils.flipBitmapHorizontal(bitmap), frameToCropTransform, null); + Bitmap flipped = CameraUtils.flipBitmapHorizontal(bitmap); + canvas.drawBitmap(flipped, frameToCropTransform, null); + workingFrame = flipped; } else { canvas.drawBitmap(bitmap, frameToCropTransform, null); } @@ -266,20 +299,15 @@ protected void processFrame(Bitmap bitmap, ImageProxy image) { int frameW = getMaxAnalyseImageSize().getWidth(); int frameH = getMaxAnalyseImageSize().getHeight(); - ControlGenerator.Result gen = - controlGenerator.generate(mappedRecognitions, frameW, frameH, sensorOrientation); + FollowStateMachine.FrameResult fr = + stateMachine.onFrame( + mappedRecognitions, workingFrame, frameW, frameH, sensorOrientation); - FollowState state; - if (gen.target == null) state = FollowState.LOST; - else if (gen.tooClose) state = FollowState.STOP; - else state = FollowState.FOLLOW; - - String command = interpreter.interpret(gen.control, state, gen.tooClose); - - updateDrawState(mappedRecognitions, gen.target, frameW, frameH, sensorOrientation); - updateCommandText(command); + updateDrawState(fr, frameW, frameH, sensorOrientation); + updateCommandText(commandForState(fr)); float fps = lastProcessingTimeMs > 0 ? 1000f / lastProcessingTimeMs : 0f; - updateDebugInfo(state, gen.control, gen.persons.size(), fps); + updateDebugInfo(fr.state, fr.control, fr.persons.size(), fps); + updateUiForState(fr); binding.trackingOverlay.postInvalidate(); } computingNetwork = false; @@ -292,15 +320,17 @@ private void updateCropImageInfo() { } private synchronized void updateDrawState( - List recognitions, - Detector.Recognition target, - int frameW, - int frameH, - int sensorOrientation) { + FollowStateMachine.FrameResult fr, int frameW, int frameH, int sensorOrientation) { drawBoxes.clear(); - for (Detector.Recognition r : recognitions) { - if (r.getLocation() == null) continue; - drawBoxes.add(new DrawBox(new RectF(r.getLocation()), r == target)); + 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; @@ -331,20 +361,85 @@ private void drawOverlay(Canvas canvas) { for (DrawBox box : snapshot) { RectF rect = new RectF(box.location); matrix.mapRect(rect); - Paint paint = box.isTarget ? targetBoxPaint : personBoxPaint; + 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 (box.isTarget) { - canvas.drawText("目标", rect.left + cornerSize, rect.top, boxTextPaint); + 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; @@ -386,11 +481,11 @@ protected int getNumThreads() { private static class DrawBox { final RectF location; - final boolean isTarget; + final int colorType; - DrawBox(RectF location, boolean isTarget) { + DrawBox(RectF location, int colorType) { this.location = location; - this.isTarget = isTarget; + this.colorType = colorType; } } } 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/res/layout/fragment_human_cart_simulator.xml b/android/robot/src/main/res/layout/fragment_human_cart_simulator.xml index bb506180a..1ad1b22f4 100644 --- a/android/robot/src/main/res/layout/fragment_human_cart_simulator.xml +++ b/android/robot/src/main/res/layout/fragment_human_cart_simulator.xml @@ -46,6 +46,87 @@ app:layout_constraintTop_toTopOf="parent" tools:text="state=IDLE\nforward=0.00\nturn=0.00\nleft=0.00\nright=0.00\npersons=0\nfps=0.0" /> + + + + + + + + + + + + +