Compare commits

...

8 Commits

Author SHA1 Message Date
Matt
5dc70e4d3f Run resize in CPU and more aggressively release rknn resources (#1287)
With the latest dev opi image, i saw this stack trace when object detection stopped working (threads hanging forever on detect(). The stack pointed me to somewhere inside the RGA. Based on this i moved resize into CPU (as our [native code already is lazy](6934abb26c/src/main/native/cpp/yolo_common.cpp (L227))), and was not able to see more crashes

[message.txt](https://github.com/PhotonVision/photonvision/files/14630158/message.txt)

Includes also a quick hack to add a shutdown hook that releases pipelines at exit.
2024-03-18 16:36:14 -04:00
Matt
5597f5acd9 Set default pipeline idx in PipelineManager constructor (#1286)
Addresses #1285
2024-03-18 13:21:41 -04:00
Matt
fae3116951 Bump to 2024.3.2 (#1283) 2024-03-17 23:00:22 -05:00
Gautam
def37b92ba Add proper exposure range for OV2311 (#1282) 2024-03-16 20:28:52 -04:00
Phill Tran
5b878fe3a3 Disable camera orientation option when camera is calibrated (#1277)
* Disable camera orientation option when camera is calibrated.

* Flip logic on if camera is calibrated when disabling camera orientation rotation

* Add comment on why orientation is disabled when camera is calibrated

* Add v banner warning regarding rotating calibrated camera bug

* Run lint

---------

Co-authored-by: Matt <matthew.morley.ca@gmail.com>
2024-03-15 10:39:04 -04:00
Matt
d9c2a382f1 Update build.yml (#1276) 2024-03-14 00:32:08 -05:00
Matt
e125632960 Free native resources in apriltag pipelines (#1272)
Addresses memory leak when switching between apriltag/aruco pipelines
2024-03-14 01:22:32 -04:00
Matt
d8f82bf9ee Opencv cal: CALIB_USE_LU and use camera focal length guess (#1268) 2024-03-09 08:31:54 -05:00
40 changed files with 401 additions and 116 deletions

View File

@@ -367,13 +367,13 @@ jobs:
- os: ubuntu-latest
artifact-name: LinuxArm64
image_suffix: orangepi5
image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/v2024.0.9/photonvision_opi5.img.xz
image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/v2024.0.10/photonvision_opi5.img.xz
cpu: cortex-a8
image_additional_mb: 4096
- os: ubuntu-latest
artifact-name: LinuxArm64
image_suffix: orangepi5plus
image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/v2024.0.9/photonvision_opi5plus.img.xz
image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/v2024.0.10/photonvision_opi5plus.img.xz
cpu: cortex-a8
image_additional_mb: 4096

View File

@@ -4,7 +4,7 @@ plugins {
id "com.diffplug.spotless" version "6.24.0"
id "edu.wpi.first.NativeUtils" version "2024.6.1" apply false
id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2020.2"
id "edu.wpi.first.GradleRIO" version "2024.3.1"
id "edu.wpi.first.GradleRIO" version "2024.3.2"
id 'edu.wpi.first.WpilibTools' version '1.3.0'
id 'com.google.protobuf' version '0.9.4' apply false
}
@@ -24,7 +24,7 @@ allprojects {
apply from: "versioningHelper.gradle"
ext {
wpilibVersion = "2024.3.1"
wpilibVersion = "2024.3.2"
wpimathVersion = wpilibVersion
openCVversion = "4.8.0-2"
joglVersion = "2.4.0-rc-20200307"

View File

@@ -13,11 +13,7 @@ defineProps<{
const driverMode = computed<boolean>({
get: () => useCameraSettingsStore().isDriverMode,
set: (v) =>
useCameraSettingsStore().changeCurrentPipelineIndex(
v ? -1 : useCameraSettingsStore().currentCameraSettings.lastPipelineIndex || 0,
true
)
set: (v) => useCameraSettingsStore().setDriverMode(v)
});
const fpsTooLow = computed<boolean>(() => {

View File

@@ -130,12 +130,32 @@ const interactiveCols = computed(() =>
tooltip="Controls blue automatic white balance gain, which affects how the camera captures colors in different conditions"
@input="(args) => useCameraSettingsStore().changeCurrentPipelineSetting({ cameraBlueGain: args }, false)"
/>
<!-- Disable camera orientation as stop gap for Issue 1084 until calibration data gets rotated. https://github.com/PhotonVision/photonvision/issues/1084 -->
<v-banner
v-show="
useCameraSettingsStore().isCurrentVideoFormatCalibrated &&
useCameraSettingsStore().currentPipelineSettings.inputImageRotationMode != 0
"
rounded
dark
color="red"
text-color="white"
class="mt-3"
icon="mdi-alert-circle-outline"
>
Warning! A known bug affects rotation of calibrated camera. Turn off rotation here and rotate using
cameraToRobotTransform in your robot code.
</v-banner>
<pv-select
v-model="useCameraSettingsStore().currentPipelineSettings.inputImageRotationMode"
label="Orientation"
tooltip="Rotates the camera stream"
tooltip="Rotates the camera stream. Rotation not available when camera has been calibrated."
:items="cameraRotations"
:select-cols="interactiveCols"
:disabled="
useCameraSettingsStore().isCurrentVideoFormatCalibrated &&
useCameraSettingsStore().currentPipelineSettings.inputImageRotationMode == 0
"
@input="(args) => useCameraSettingsStore().changeCurrentPipelineSetting({ inputImageRotationMode: args }, false)"
/>
<pv-select

View File

@@ -236,6 +236,13 @@ export const useCameraSettingsStore = defineStore("cameraSettings", {
}
useStateStore().websocket?.send(payload, true);
},
setDriverMode(isDriverMode: boolean, cameraIndex: number = useStateStore().currentCameraIndex) {
const payload = {
driverMode: isDriverMode,
cameraIndex: cameraIndex
};
useStateStore().websocket?.send(payload, true);
},
/**
* Change the currently selected pipeline of the provided camera.
*

View File

@@ -22,12 +22,12 @@ import java.util.Arrays;
import java.util.List;
import java.util.concurrent.CopyOnWriteArrayList;
import java.util.stream.Collectors;
import org.opencv.core.Mat;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.TestUtils;
import org.photonvision.rknn.RknnJNI;
import org.photonvision.rknn.RknnJNI.RknnResult;
import org.photonvision.vision.opencv.CVMat;
import org.photonvision.vision.pipe.impl.NeuralNetworkPipeResult;
public class RknnDetectorJNI extends PhotonJNICommon {
@@ -65,16 +65,38 @@ public class RknnDetectorJNI extends PhotonJNICommon {
long objPointer = -1;
private List<String> labels;
private final Object lock = new Object();
private static final CopyOnWriteArrayList<Long> detectors = new CopyOnWriteArrayList<>();
private static final CopyOnWriteArrayList<RknnObjectDetector> detectors =
new CopyOnWriteArrayList<>();
static volatile boolean hook = false;
public RknnObjectDetector(String modelPath, List<String> labels, RknnJNI.ModelVersion version) {
synchronized (lock) {
objPointer = RknnJNI.create(modelPath, labels.size(), version.ordinal(), -1);
detectors.add(objPointer);
System.out.println(
"Created " + objPointer + "! Detectors: " + Arrays.toString(detectors.toArray()));
detectors.add(this);
logger.debug(
"Created detector "
+ objPointer
+ " from path "
+ modelPath
+ "! Detectors: "
+ Arrays.toString(detectors.toArray()));
}
this.labels = labels;
// the kernel should probably alredy deal with this for us, but I'm gunna be paranoid anyways.
if (!hook) {
Runtime.getRuntime()
.addShutdownHook(
new Thread(
() -> {
System.err.println("Shutdown hook rknn");
for (var d : detectors) {
d.release();
}
}));
hook = true;
}
}
public List<String> getClasses() {
@@ -89,14 +111,14 @@ public class RknnDetectorJNI extends PhotonJNICommon {
* @param boxThresh Minimum confidence for a box to be added. Basically just confidence
* threshold
*/
public List<NeuralNetworkPipeResult> detect(CVMat in, double nmsThresh, double boxThresh) {
public List<NeuralNetworkPipeResult> detect(Mat in, double nmsThresh, double boxThresh) {
RknnResult[] ret;
synchronized (lock) {
// We can technically be asked to detect and the lock might be acquired _after_ release has
// been called. This would mean objPointer would be invalid which would call everything to
// explode.
if (objPointer > 0) {
ret = RknnJNI.detect(objPointer, in.getMat().getNativeObjAddr(), nmsThresh, boxThresh);
ret = RknnJNI.detect(objPointer, in.getNativeObjAddr(), nmsThresh, boxThresh);
} else {
logger.warn("Detect called after destroy -- giving up");
return List.of();
@@ -114,7 +136,7 @@ public class RknnDetectorJNI extends PhotonJNICommon {
synchronized (lock) {
if (objPointer > 0) {
RknnJNI.destroy(objPointer);
detectors.remove(objPointer);
detectors.remove(this);
System.out.println(
"Killed " + objPointer + "! Detectors: " + Arrays.toString(detectors.toArray()));
objPointer = -1;
@@ -124,14 +146,4 @@ public class RknnDetectorJNI extends PhotonJNICommon {
}
}
}
// public static void createRknnDetector() {
// objPointer =
// RknnJNI.create(
// NeuralNetworkModelManager.getInstance()
// .getDefaultRknnModel()
// .getAbsolutePath()
// .toString(),
// NeuralNetworkModelManager.getInstance().getLabels().size());
// }
}

View File

@@ -23,16 +23,37 @@ import java.util.Comparator;
import org.opencv.core.Mat;
import org.opencv.objdetect.ArucoDetector;
import org.opencv.objdetect.DetectorParameters;
import org.opencv.objdetect.Dictionary;
import org.opencv.objdetect.Objdetect;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.vision.opencv.Releasable;
/** This class wraps an {@link ArucoDetector} for convenience. */
public class PhotonArucoDetector {
public class PhotonArucoDetector implements Releasable {
private static final Logger logger = new Logger(PhotonArucoDetector.class, LogGroup.VisionModule);
private final ArucoDetector detector =
new ArucoDetector(Objdetect.getPredefinedDictionary(Objdetect.DICT_APRILTAG_16h5));
private static class ArucoDetectorHack extends ArucoDetector {
public ArucoDetectorHack(Dictionary predefinedDictionary) {
super(predefinedDictionary);
}
// avoid double-free by keeping track of this ourselves (ew)
private boolean freed = false;
@Override
public void finalize() throws Throwable {
if (freed) {
return;
}
super.finalize();
freed = true;
}
}
private final ArucoDetectorHack detector =
new ArucoDetectorHack(Objdetect.getPredefinedDictionary(Objdetect.DICT_APRILTAG_16h5));
private final Mat ids = new Mat();
private final ArrayList<Mat> cornerMats = new ArrayList<>();
@@ -95,4 +116,16 @@ public class PhotonArucoDetector {
return results;
}
@Override
public void release() {
try {
detector.finalize();
} catch (Throwable e) {
logger.error("Exception destroying PhotonArucoDetector", e);
}
ids.release();
for (var m : cornerMats) m.release();
cornerMats.clear();
}
}

View File

@@ -270,9 +270,13 @@ public class USBCameraSource extends VisionSource {
if (getCameraConfiguration().cameraQuirks.hasQuirk(CameraQuirk.ArduOV9281)) {
propMin = 1;
propMax = 75;
} else if (getCameraConfiguration().cameraQuirks.hasQuirk(CameraQuirk.ArduOV2311)) {
propMin = 1;
propMax = 140;
}
var exposure_manual_val = MathUtils.map(Math.round(exposure), 0, 100, propMin, propMax);
logger.debug("Setting camera exposure to " + exposure_manual_val);
prop.set((int) exposure_manual_val);
} else {
scaledExposure = (int) Math.round(exposure);

View File

@@ -33,7 +33,7 @@ import org.photonvision.vision.opencv.CVMat;
* path}.
*/
public class FileFrameProvider extends CpuImageProcessor {
public static final int MAX_FPS = 5;
public static final int MAX_FPS = 10;
private static int count = 0;
private final int thisIndex = count++;

View File

@@ -21,11 +21,13 @@ import edu.wpi.first.apriltag.AprilTagDetection;
import edu.wpi.first.apriltag.AprilTagDetector;
import java.util.List;
import org.photonvision.vision.opencv.CVMat;
import org.photonvision.vision.opencv.Releasable;
import org.photonvision.vision.pipe.CVPipe;
public class AprilTagDetectionPipe
extends CVPipe<CVMat, List<AprilTagDetection>, AprilTagDetectionPipeParams> {
private final AprilTagDetector m_detector = new AprilTagDetector();
extends CVPipe<CVMat, List<AprilTagDetection>, AprilTagDetectionPipeParams>
implements Releasable {
private AprilTagDetector m_detector = new AprilTagDetector();
public AprilTagDetectionPipe() {
super();
@@ -40,6 +42,10 @@ public class AprilTagDetectionPipe
return List.of();
}
if (m_detector == null) {
throw new RuntimeException("Apriltag detector was released!");
}
var ret = m_detector.detect(in.getMat());
if (ret == null) {
@@ -60,4 +66,10 @@ public class AprilTagDetectionPipe
super.setParams(newParams);
}
@Override
public void release() {
m_detector.close();
m_detector = null;
}
}

View File

@@ -25,13 +25,15 @@ import org.opencv.calib3d.Calib3d;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.opencv.Releasable;
import org.photonvision.vision.pipe.CVPipe;
public class AprilTagPoseEstimatorPipe
extends CVPipe<
AprilTagDetection,
AprilTagPoseEstimate,
AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams> {
AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams>
implements Releasable {
private final AprilTagPoseEstimator m_poseEstimator =
new AprilTagPoseEstimator(new AprilTagPoseEstimator.Config(0, 0, 0, 0, 0));
@@ -92,6 +94,11 @@ public class AprilTagPoseEstimatorPipe
super.setParams(newParams);
}
@Override
public void release() {
temp.release();
}
public static class AprilTagPoseEstimatorPipeParams {
final AprilTagPoseEstimator.Config config;
final CameraCalibrationCoefficients calibration;

View File

@@ -29,10 +29,12 @@ import org.opencv.objdetect.Objdetect;
import org.photonvision.vision.aruco.ArucoDetectionResult;
import org.photonvision.vision.aruco.PhotonArucoDetector;
import org.photonvision.vision.opencv.CVMat;
import org.photonvision.vision.opencv.Releasable;
import org.photonvision.vision.pipe.CVPipe;
public class ArucoDetectionPipe
extends CVPipe<CVMat, List<ArucoDetectionResult>, ArucoDetectionPipeParams> {
extends CVPipe<CVMat, List<ArucoDetectionResult>, ArucoDetectionPipeParams>
implements Releasable {
// ArucoDetector wrapper class
private final PhotonArucoDetector photonDetector = new PhotonArucoDetector();
@@ -131,4 +133,9 @@ public class ArucoDetectionPipe
var pt2 = new Point(corner.x + windowSize, corner.y + windowSize);
Imgproc.rectangle(outputMat, pt1, pt2, new Scalar(0, 0, 255), thickness);
}
@Override
public void release() {
photonDetector.release();
}
}

View File

@@ -32,13 +32,15 @@ import org.opencv.core.MatOfPoint3f;
import org.opencv.core.Point3;
import org.photonvision.vision.aruco.ArucoDetectionResult;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.opencv.Releasable;
import org.photonvision.vision.pipe.CVPipe;
public class ArucoPoseEstimatorPipe
extends CVPipe<
ArucoDetectionResult,
AprilTagPoseEstimate,
ArucoPoseEstimatorPipe.ArucoPoseEstimatorPipeParams> {
ArucoPoseEstimatorPipe.ArucoPoseEstimatorPipeParams>
implements Releasable {
// image points of marker corners
private final MatOfPoint2f imagePoints = new MatOfPoint2f(Mat.zeros(4, 1, CvType.CV_32FC2));
// rvec/tvec estimations from solvepnp
@@ -117,6 +119,18 @@ public class ArucoPoseEstimatorPipe
super.setParams(newParams);
}
@Override
public void release() {
imagePoints.release();
for (var m : rvecs) m.release();
rvecs.clear();
for (var m : tvecs) m.release();
tvecs.clear();
rvec.release();
tvec.release();
reprojectionErrors.release();
}
public static class ArucoPoseEstimatorPipeParams {
final CameraCalibrationCoefficients calibration;
final double tagSize;

View File

@@ -38,13 +38,26 @@ import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.calibration.CameraLensModel;
import org.photonvision.vision.calibration.JsonImageMat;
import org.photonvision.vision.calibration.JsonMatOfDouble;
import org.photonvision.vision.frame.FrameStaticProperties;
import org.photonvision.vision.pipe.CVPipe;
import org.photonvision.vision.pipe.impl.FindBoardCornersPipe.FindBoardCornersPipeResult;
public class Calibrate3dPipe
extends CVPipe<
List<FindBoardCornersPipe.FindBoardCornersPipeResult>,
Calibrate3dPipe.CalibrationInput,
CameraCalibrationCoefficients,
Calibrate3dPipe.CalibratePipeParams> {
public static class CalibrationInput {
final List<FindBoardCornersPipe.FindBoardCornersPipeResult> observations;
final FrameStaticProperties imageProps;
public CalibrationInput(
List<FindBoardCornersPipeResult> observations, FrameStaticProperties imageProps) {
this.observations = observations;
this.imageProps = imageProps;
}
}
// For logging
private static final Logger logger = new Logger(Calibrate3dPipe.class, LogGroup.General);
@@ -63,10 +76,9 @@ public class Calibrate3dPipe
* @return Result of processing.
*/
@Override
protected CameraCalibrationCoefficients process(
List<FindBoardCornersPipe.FindBoardCornersPipeResult> in) {
in =
in.stream()
protected CameraCalibrationCoefficients process(CalibrationInput in) {
var filteredIn =
in.observations.stream()
.filter(
it ->
it != null
@@ -79,17 +91,21 @@ public class Calibrate3dPipe
var start = System.nanoTime();
if (MrCalJNILoader.getInstance().isLoaded() && params.useMrCal) {
logger.debug("Calibrating with mrcal!");
ret = calibrateMrcal(in);
ret =
calibrateMrcal(
filteredIn, in.imageProps.horizontalFocalLength, in.imageProps.verticalFocalLength);
} else {
logger.debug("Calibrating with opencv!");
ret = calibrateOpenCV(in);
ret =
calibrateOpenCV(
filteredIn, in.imageProps.horizontalFocalLength, in.imageProps.verticalFocalLength);
}
var dt = System.nanoTime() - start;
if (ret != null)
logger.info(
"CALIBRATION SUCCESS for res "
+ in.get(0).size
+ in.observations.get(0).size
+ " in "
+ dt / 1e6
+ "ms! camMatrix: \n"
@@ -103,7 +119,7 @@ public class Calibrate3dPipe
}
protected CameraCalibrationCoefficients calibrateOpenCV(
List<FindBoardCornersPipe.FindBoardCornersPipeResult> in) {
List<FindBoardCornersPipe.FindBoardCornersPipeResult> in, double fxGuess, double fyGuess) {
List<Mat> objPoints = in.stream().map(it -> it.objectPoints).collect(Collectors.toList());
List<Mat> imgPts = in.stream().map(it -> it.imagePoints).collect(Collectors.toList());
if (objPoints.size() != imgPts.size()) {
@@ -111,30 +127,32 @@ public class Calibrate3dPipe
return null;
}
Mat cameraMatrix = new Mat();
Mat cameraMatrix = new Mat(3, 3, CvType.CV_64F);
MatOfDouble distortionCoefficients = new MatOfDouble();
List<Mat> rvecs = new ArrayList<>();
List<Mat> tvecs = new ArrayList<>();
// RMS of the calibration
double calibrationAccuracy;
// initial camera matrix guess
double cx = (in.get(0).size.width / 2.0) - 0.5;
double cy = (in.get(0).size.width / 2.0) - 0.5;
cameraMatrix.put(0, 0, new double[] {fxGuess, 0, cx, 0, fyGuess, cy, 0, 0, 1});
try {
// FindBoardCorners pipe outputs all the image points, object points, and frames to calculate
// imageSize from, other parameters are output Mats
calibrationAccuracy =
Calib3d.calibrateCameraExtended(
objPoints,
imgPts,
new Size(in.get(0).size.width, in.get(0).size.height),
cameraMatrix,
distortionCoefficients,
rvecs,
tvecs,
stdDeviationsIntrinsics,
stdDeviationsExtrinsics,
perViewErrors);
Calib3d.calibrateCameraExtended(
objPoints,
imgPts,
new Size(in.get(0).size.width, in.get(0).size.height),
cameraMatrix,
distortionCoefficients,
rvecs,
tvecs,
stdDeviationsIntrinsics,
stdDeviationsExtrinsics,
perViewErrors,
Calib3d.CALIB_USE_LU + Calib3d.CALIB_USE_INTRINSIC_GUESS);
} catch (Exception e) {
logger.error("Calibration failed!", e);
e.printStackTrace();
@@ -164,13 +182,12 @@ public class Calibrate3dPipe
}
protected CameraCalibrationCoefficients calibrateMrcal(
List<FindBoardCornersPipe.FindBoardCornersPipeResult> in) {
List<FindBoardCornersPipe.FindBoardCornersPipeResult> in, double fxGuess, double fyGuess) {
List<MatOfPoint2f> corner_locations =
in.stream().map(it -> it.imagePoints).map(MatOfPoint2f::new).collect(Collectors.toList());
int imageWidth = (int) in.get(0).size.width;
int imageHeight = (int) in.get(0).size.height;
final double FOCAL_LENGTH_GUESS = 1200;
MrCalResult result =
MrCalJNI.calibrateCamera(
@@ -180,7 +197,7 @@ public class Calibrate3dPipe
params.squareSize,
imageWidth,
imageHeight,
FOCAL_LENGTH_GUESS);
(fxGuess + fyGuess) / 2.0);
// intrinsics are fx fy cx cy from mrcal
JsonMatOfDouble cameraMatrixMat =

View File

@@ -36,6 +36,7 @@ import org.photonvision.vision.frame.FrameThresholdType;
import org.photonvision.vision.opencv.CVMat;
import org.photonvision.vision.opencv.ImageRotationMode;
import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
import org.photonvision.vision.pipe.impl.Calibrate3dPipe.CalibrationInput;
import org.photonvision.vision.pipe.impl.FindBoardCornersPipe.FindBoardCornersPipeResult;
import org.photonvision.vision.pipeline.CVPipeline;
import org.photonvision.vision.pipeline.Calibration3dPipelineSettings;
@@ -176,7 +177,8 @@ public class Calibrate3dPipeline
/*Pass the board corners to the pipe, which will check again to see if all boards are valid
and returns the corresponding image and object points*/
calibrationOutput = calibrate3dPipe.run(foundCornersList);
calibrationOutput =
calibrate3dPipe.run(new CalibrationInput(foundCornersList, frameStaticProperties));
this.calibrating = false;
@@ -229,4 +231,9 @@ public class Calibrate3dPipeline
public CameraCalibrationCoefficients cameraCalibrationCoefficients() {
return calibrationOutput.output;
}
@Override
public void release() {
// we never actually need to give resources up since pipelinemanager only makes one of us
}
}

View File

@@ -41,7 +41,7 @@ public class FilterObjectDetectionsPipe
}
private void filterContour(NeuralNetworkPipeResult contour) {
var boc = contour.box;
var boc = contour.bbox;
// Area filtering
double areaPercentage = boc.area() / params.getFrameStaticProperties().imageArea * 100.0;

View File

@@ -20,13 +20,13 @@ package org.photonvision.vision.pipe.impl;
import org.opencv.core.Rect2d;
public class NeuralNetworkPipeResult {
public NeuralNetworkPipeResult(Rect2d box2, Integer classIdx, Float confidence) {
box = box2;
public NeuralNetworkPipeResult(Rect2d boundingBox, int classIdx, double confidence) {
bbox = boundingBox;
this.classIdx = classIdx;
this.confidence = confidence;
}
public final int classIdx;
public final Rect2d box;
public final Rect2d bbox;
public final double confidence;
}

View File

@@ -17,8 +17,17 @@
package org.photonvision.vision.pipe.impl;
import java.awt.Color;
import java.util.ArrayList;
import java.util.List;
import org.opencv.core.Core;
import org.opencv.core.Mat;
import org.opencv.core.Rect2d;
import org.opencv.core.Scalar;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
import org.photonvision.common.configuration.NeuralNetworkModelManager;
import org.photonvision.common.util.ColorHelper;
import org.photonvision.jni.RknnDetectorJNI.RknnObjectDetector;
import org.photonvision.vision.opencv.CVMat;
import org.photonvision.vision.opencv.Releasable;
@@ -30,8 +39,10 @@ public class RknnDetectionPipe
private RknnObjectDetector detector;
public RknnDetectionPipe() {
// For now this is hard-coded to defaults. Should be refactored into set pipe params, though.
// And ideally a little wrapper helper for only changing native stuff on content change created.
// For now this is hard-coded to defaults. Should be refactored into set pipe
// params, though.
// And ideally a little wrapper helper for only changing native stuff on content
// change created.
this.detector =
new RknnObjectDetector(
NeuralNetworkModelManager.getInstance().getDefaultRknnModel().getAbsolutePath(),
@@ -39,6 +50,18 @@ public class RknnDetectionPipe
NeuralNetworkModelManager.getInstance().getModelVersion());
}
private static class Letterbox {
double dx;
double dy;
double scale;
public Letterbox(double dx, double dy, double scale) {
this.dx = dx;
this.dy = dy;
this.scale = scale;
}
}
@Override
protected List<NeuralNetworkPipeResult> process(CVMat in) {
var frame = in.getMat();
@@ -48,7 +71,66 @@ public class RknnDetectionPipe
return List.of();
}
return detector.detect(in, params.nms, params.confidence);
// letterbox
var letterboxed = new Mat();
var scale =
letterbox(frame, letterboxed, new Size(640, 640), ColorHelper.colorToScalar(Color.GRAY));
if (letterboxed.width() != 640 || letterboxed.height() != 640) {
// huh whack give up lol
throw new RuntimeException("RGA bugged but still wrong size");
}
var ret = detector.detect(letterboxed, params.nms, params.confidence);
return resizeDetections(ret, scale);
}
private List<NeuralNetworkPipeResult> resizeDetections(
List<NeuralNetworkPipeResult> unscaled, Letterbox letterbox) {
var ret = new ArrayList<NeuralNetworkPipeResult>();
for (var t : unscaled) {
var scale = 1.0 / letterbox.scale;
var boundingBox = t.bbox;
double x = (boundingBox.x - letterbox.dx) * scale;
double y = (boundingBox.y - letterbox.dy) * scale;
double width = boundingBox.width * scale;
double height = boundingBox.height * scale;
ret.add(
new NeuralNetworkPipeResult(new Rect2d(x, y, width, height), t.classIdx, t.confidence));
}
return ret;
}
private static Letterbox letterbox(Mat frame, Mat letterboxed, Size newShape, Scalar color) {
// from https://github.com/ultralytics/yolov5/issues/8427#issuecomment-1172469631
var frameSize = frame.size();
var r = Math.min(newShape.height / frameSize.height, newShape.width / frameSize.width);
var newUnpad = new Size(Math.round(frameSize.width * r), Math.round(frameSize.height * r));
if (!(frameSize.equals(newUnpad))) {
Imgproc.resize(frame, letterboxed, newUnpad, Imgproc.INTER_LINEAR);
} else {
frame.copyTo(letterboxed);
}
var dw = newShape.width - newUnpad.width;
var dh = newShape.height - newUnpad.height;
dw /= 2;
dh /= 2;
int top = (int) (Math.round(dh - 0.1f));
int bottom = (int) (Math.round(dh + 0.1f));
int left = (int) (Math.round(dw - 0.1f));
int right = (int) (Math.round(dw + 0.1f));
Core.copyMakeBorder(
letterboxed, letterboxed, top, bottom, left, right, Core.BORDER_CONSTANT, color);
return new Letterbox(dw, dh, r);
}
public static class RknnDetectionPipeParams {

View File

@@ -221,4 +221,11 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, multiTagResult, frame);
}
@Override
public void release() {
aprilTagDetectionPipe.release();
singleTagPoseEstimatorPipe.release();
super.release();
}
}

View File

@@ -60,8 +60,8 @@ import org.photonvision.vision.target.TrackedTarget;
import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters;
public class ArucoPipeline extends CVPipeline<CVPipelineResult, ArucoPipelineSettings> {
private final ArucoDetectionPipe arucoDetectionPipe = new ArucoDetectionPipe();
private final ArucoPoseEstimatorPipe singleTagPoseEstimatorPipe = new ArucoPoseEstimatorPipe();
private ArucoDetectionPipe arucoDetectionPipe = new ArucoDetectionPipe();
private ArucoPoseEstimatorPipe singleTagPoseEstimatorPipe = new ArucoPoseEstimatorPipe();
private final MultiTargetPNPPipe multiTagPNPPipe = new MultiTargetPNPPipe();
private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
@@ -250,4 +250,13 @@ public class ArucoPipeline extends CVPipeline<CVPipelineResult, ArucoPipelineSet
windowSize,
constant);
}
@Override
public void release() {
arucoDetectionPipe.release();
singleTagPoseEstimatorPipe.release();
arucoDetectionPipe = null;
singleTagPoseEstimatorPipe = null;
super.release();
}
}

View File

@@ -34,6 +34,9 @@ public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelin
private final FrameThresholdType thresholdType;
// So releaseable doesn't keep track of if we double-free something. so (ew) remember that here
protected volatile boolean released = false;
public CVPipeline(FrameThresholdType thresholdType) {
this.thresholdType = thresholdType;
}
@@ -64,6 +67,9 @@ public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelin
}
public R run(Frame frame, QuirkyCamera cameraQuirks) {
if (released) {
throw new RuntimeException("Pipeline use-after-free!");
}
if (settings == null) {
throw new RuntimeException("No settings provided for pipeline!");
}
@@ -85,5 +91,7 @@ public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelin
* switch. Stubbed out, but override if needed.
*/
@Override
public void release() {}
public void release() {
released = true;
}
}

View File

@@ -88,4 +88,9 @@ public class DriverModePipeline
fps,
new Frame(frame.processedImage, frame.colorImage, frame.type, frame.frameStaticProperties));
}
@Override
public void release() {
// we never actually need to give resources up since pipelinemanager only makes one of us
}
}

View File

@@ -131,5 +131,6 @@ public class ObjectDetectionPipeline
@Override
public void release() {
rknnPipe.release();
super.release();
}
}

View File

@@ -61,7 +61,8 @@ public class PipelineManager {
PipelineManager(
DriverModePipelineSettings driverSettings,
List<CVPipelineSettings> userPipelines,
String uniqueName) {
String uniqueName,
int defaultIndex) {
this.userPipelineSettings = new ArrayList<>(userPipelines);
// This is to respect the default res idx for vendor cameras
@@ -70,10 +71,19 @@ public class PipelineManager {
if (userPipelines.isEmpty()) addPipeline(PipelineType.Reflective);
calibration3dPipeline = new Calibrate3dPipeline(uniqueName);
// We know that at this stage, VisionRunner hasn't yet started so we're good to do this from
// this thread
this.setIndex(defaultIndex);
updatePipelineFromRequested();
}
public PipelineManager(CameraConfiguration config) {
this(config.driveModeSettings, config.pipelineSettings, config.uniqueName);
this(
config.driveModeSettings,
config.pipelineSettings,
config.uniqueName,
config.currentPipelineIndex);
}
/**
@@ -148,6 +158,7 @@ public class PipelineManager {
* @return The currently active pipeline.
*/
public CVPipeline getCurrentPipeline() {
updatePipelineFromRequested();
if (currentPipelineIndex < 0) {
switch (currentPipelineIndex) {
case CAL_3D_INDEX:
@@ -170,6 +181,16 @@ public class PipelineManager {
return getPipelineSettings(currentPipelineIndex);
}
private volatile int requestedIndex = 0;
/**
* Grab the currently requested pipeline index. The VisionRunner may not have changed over to this
* pipeline yet.
*/
public int getRequestedIndex() {
return requestedIndex;
}
/**
* Internal method for setting the active pipeline. <br>
* <br>
@@ -179,6 +200,22 @@ public class PipelineManager {
* @param newIndex Index of pipeline to be active
*/
private void setPipelineInternal(int newIndex) {
requestedIndex = newIndex;
}
/**
* Based on a requested pipeline index, create/destroy pipelines as necessary. We do this as a
* side effect of the main thread that calls getCurrentPipeline to avoid race conditions between
* server threads and the VisionRunner TODO: this should be refactored. Shame Java doesn't have
* RAII
*/
private void updatePipelineFromRequested() {
int newIndex = requestedIndex;
if (newIndex == currentPipelineIndex) {
// nothing to do, probably no change -- give up
return;
}
if (newIndex < 0 && currentPipelineIndex >= 0) {
// Transitioning to a built-in pipe, save off the current user one
lastUserPipelineIdx = currentPipelineIndex;
@@ -189,8 +226,8 @@ public class PipelineManager {
return;
}
// Cleanup potential old native resources before swapping over
if (currentUserPipeline != null) {
// Cleanup potential old native resources before swapping over for user pipelines
if (currentUserPipeline != null && !(newIndex < 0)) {
currentUserPipeline.release();
}

View File

@@ -143,7 +143,7 @@ public class VisionModule {
ntConsumer =
new NTDataPublisher(
visionSource.getSettables().getConfiguration().nickname,
pipelineManager::getCurrentPipelineIndex,
pipelineManager::getRequestedIndex,
this::setPipeline,
pipelineManager::getDriverMode,
this::setDriverMode);
@@ -156,6 +156,7 @@ public class VisionModule {
(result) ->
lastPipelineResultBestTarget = result.hasTargets() ? result.targets.get(0) : null);
// Sync VisionModule state with the first pipeline index
setPipeline(visionSource.getSettables().getConfiguration().currentPipelineIndex);
// Set vendor FOV
@@ -321,7 +322,7 @@ public class VisionModule {
void changePipelineType(int newType) {
pipelineManager.changePipelineType(newType);
setPipeline(pipelineManager.getCurrentPipelineIndex());
setPipeline(pipelineManager.getRequestedIndex());
saveAndBroadcastAll();
}
@@ -329,9 +330,7 @@ public class VisionModule {
pipelineManager.setDriverMode(isDriverMode);
setVisionLEDs(!isDriverMode);
setPipeline(
isDriverMode
? PipelineManager.DRIVERMODE_INDEX
: pipelineManager.getCurrentPipelineIndex());
isDriverMode ? PipelineManager.DRIVERMODE_INDEX : pipelineManager.getRequestedIndex());
saveAndBroadcastAll();
}
@@ -385,7 +384,7 @@ public class VisionModule {
var ret = pipelineManager.calibration3dPipeline.tryCalibration();
pipelineManager.setCalibrationMode(false);
setPipeline(pipelineManager.getCurrentPipelineIndex());
setPipeline(pipelineManager.getRequestedIndex());
if (ret != null) {
logger.debug("Saving calibration...");
@@ -447,7 +446,7 @@ public class VisionModule {
setVisionLEDs(pipelineSettings.ledMode);
visionSource.getSettables().getConfiguration().currentPipelineIndex =
pipelineManager.getCurrentPipelineIndex();
pipelineManager.getRequestedIndex();
return true;
}
@@ -511,7 +510,7 @@ public class VisionModule {
ret.uniqueName = visionSource.getSettables().getConfiguration().uniqueName;
ret.currentPipelineSettings =
SerializationUtils.objectToHashMap(pipelineManager.getCurrentPipelineSettings());
ret.currentPipelineIndex = pipelineManager.getCurrentPipelineIndex();
ret.currentPipelineIndex = pipelineManager.getRequestedIndex();
ret.pipelineNicknames = pipelineManager.getPipelineNicknames();
ret.cameraQuirks = visionSource.getSettables().getConfiguration().cameraQuirks;
@@ -553,7 +552,7 @@ public class VisionModule {
var config = visionSource.getSettables().getConfiguration();
config.setPipelineSettings(pipelineManager.userPipelineSettings);
config.driveModeSettings = pipelineManager.driverModePipeline.getSettings();
config.currentPipelineIndex = Math.max(pipelineManager.getCurrentPipelineIndex(), -1);
config.currentPipelineIndex = Math.max(pipelineManager.getRequestedIndex(), -1);
return config;
}

View File

@@ -88,7 +88,7 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
parentModule.saveAndBroadcastAll();
return;
case "deleteCurrPipeline":
var indexToDelete = parentModule.pipelineManager.getCurrentPipelineIndex();
var indexToDelete = parentModule.pipelineManager.getRequestedIndex();
logger.info("Deleting current pipe at index " + indexToDelete);
int newIndex = parentModule.pipelineManager.removePipeline(indexToDelete);
parentModule.setPipeline(newIndex);
@@ -96,7 +96,7 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
return;
case "changePipeline": // change active pipeline
var index = (Integer) newPropValue;
if (index == parentModule.pipelineManager.getCurrentPipelineIndex()) {
if (index == parentModule.pipelineManager.getRequestedIndex()) {
logger.debug("Skipping pipeline change, index " + index + " already active");
return;
}
@@ -181,6 +181,9 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
parentModule.changePipelineType((Integer) newPropValue);
parentModule.saveAndBroadcastAll();
return;
case "isDriverMode":
parentModule.setDriverMode((Boolean) newPropValue);
return;
}
// special case for camera settables

View File

@@ -56,7 +56,7 @@ public class PotentialTarget implements Releasable {
}
public PotentialTarget(NeuralNetworkPipeResult det) {
this.shape = new CVShape(new Contour(det.box), ContourShape.Quadrilateral);
this.shape = new CVShape(new Contour(det.bbox), ContourShape.Quadrilateral);
this.m_mainContour = this.shape.getContour();
m_subContours = List.of();
this.clsId = det.classIdx;

View File

@@ -30,7 +30,7 @@ public class PipelineManagerTest {
public void testUniqueName() {
TestUtils.loadLibraries();
PipelineManager manager =
new PipelineManager(new DriverModePipelineSettings(), List.of(), "meme_name");
new PipelineManager(new DriverModePipelineSettings(), List.of(), "meme_name", -1);
manager.addPipeline(PipelineType.Reflective, "Another");
// We now have ["New Pipeline", "Another"]

View File

@@ -277,7 +277,7 @@ public class Main {
CameraConfiguration camConf2024 =
ConfigManager.getInstance().getConfig().getCameraConfigurations().get("WPI2024");
if (camConf2024 == null || true) {
if (camConf2024 == null) {
camConf2024 =
new CameraConfiguration(
"WPI2024",

View File

@@ -131,18 +131,16 @@ public class DataSocketHandler {
case SMT_DRIVERMODE:
{
// TODO: what is this event?
var data = (HashMap<String, Object>) entryValue;
var dmExpEvent =
new IncomingWebSocketEvent<Integer>(
DataChangeDestination.DCD_ACTIVEMODULE, "driverExposure", data);
var dmBrightEvent =
new IncomingWebSocketEvent<Integer>(
DataChangeDestination.DCD_ACTIVEMODULE, "driverBrightness", data);
var data = (Boolean) entryValue;
var dmIsDriverEvent =
new IncomingWebSocketEvent<Boolean>(
DataChangeDestination.DCD_ACTIVEMODULE, "isDriver", data);
DataChangeDestination.DCD_ACTIVEMODULE,
"isDriverMode",
data,
cameraIndex,
context);
dcService.publishEvents(dmExpEvent, dmBrightEvent, dmIsDriverEvent);
dcService.publishEvents(dmIsDriverEvent);
break;
}
case SMT_CHANGECAMERANAME:

View File

@@ -1,7 +1,7 @@
plugins {
id "cpp"
id "google-test-test-suite"
id "edu.wpi.first.GradleRIO" version "2024.3.1"
id "edu.wpi.first.GradleRIO" version "2024.3.2"
id "com.dorongold.task-tree" version "2.1.0"
}

View File

@@ -1,7 +1,7 @@
plugins {
id "cpp"
id "google-test-test-suite"
id "edu.wpi.first.GradleRIO" version "2024.3.1"
id "edu.wpi.first.GradleRIO" version "2024.3.2"
id "com.dorongold.task-tree" version "2.1.0"
}

View File

@@ -1,7 +1,7 @@
plugins {
id "cpp"
id "google-test-test-suite"
id "edu.wpi.first.GradleRIO" version "2024.3.1"
id "edu.wpi.first.GradleRIO" version "2024.3.2"
id "com.dorongold.task-tree" version "2.1.0"
}

View File

@@ -1,7 +1,7 @@
plugins {
id "cpp"
id "google-test-test-suite"
id "edu.wpi.first.GradleRIO" version "2024.3.1"
id "edu.wpi.first.GradleRIO" version "2024.3.2"
id "com.dorongold.task-tree" version "2.1.0"
}

View File

@@ -1,7 +1,7 @@
plugins {
id "cpp"
id "google-test-test-suite"
id "edu.wpi.first.GradleRIO" version "2024.3.1"
id "edu.wpi.first.GradleRIO" version "2024.3.2"
id "com.dorongold.task-tree" version "2.1.0"
}
@@ -12,8 +12,8 @@ repositories {
}
wpi.maven.useDevelopment = true
wpi.versions.wpilibVersion = "2024.3.1"
wpi.versions.wpimathVersion = "2024.3.1"
wpi.versions.wpilibVersion = "2024.3.2"
wpi.versions.wpimathVersion = "2024.3.2"
apply from: "${rootDir}/../shared/examples_common.gradle"

View File

@@ -1,6 +1,6 @@
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2024.3.1"
id "edu.wpi.first.GradleRIO" version "2024.3.2"
}
sourceCompatibility = JavaVersion.VERSION_11

View File

@@ -1,6 +1,6 @@
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2024.3.1"
id "edu.wpi.first.GradleRIO" version "2024.3.2"
}
sourceCompatibility = JavaVersion.VERSION_11

View File

@@ -1,6 +1,6 @@
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2024.3.1"
id "edu.wpi.first.GradleRIO" version "2024.3.2"
}
sourceCompatibility = JavaVersion.VERSION_11

View File

@@ -1,6 +1,6 @@
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2024.3.1"
id "edu.wpi.first.GradleRIO" version "2024.3.2"
}
sourceCompatibility = JavaVersion.VERSION_11

View File

@@ -1,6 +1,6 @@
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2024.3.1"
id "edu.wpi.first.GradleRIO" version "2024.3.2"
}
sourceCompatibility = JavaVersion.VERSION_11
@@ -11,8 +11,8 @@ apply from: "${rootDir}/../shared/examples_common.gradle"
def ROBOT_MAIN_CLASS = "frc.robot.Main"
wpi.maven.useDevelopment = true
wpi.versions.wpilibVersion = "2024.3.1"
wpi.versions.wpimathVersion = "2024.3.1"
wpi.versions.wpilibVersion = "2024.3.2"
wpi.versions.wpimathVersion = "2024.3.2"
// Define my targets (RoboRIO) and artifacts (deployable files)