diff --git a/photon-server/src/main/java/org/photonvision/common/configuration/HardwareConfig.java b/photon-server/src/main/java/org/photonvision/common/configuration/HardwareConfig.java index 1fab803e9..abe00db84 100644 --- a/photon-server/src/main/java/org/photonvision/common/configuration/HardwareConfig.java +++ b/photon-server/src/main/java/org/photonvision/common/configuration/HardwareConfig.java @@ -35,6 +35,7 @@ public class HardwareConfig { public final boolean ledsCanDim; public final ArrayList ledPWMRange; public final String ledPWMSetRange; + public final int ledPWMFrequency; public final String ledDimCommand; public final String ledBlinkCommand; @@ -54,6 +55,7 @@ public class HardwareConfig { ledSetCommand = ""; ledsCanDim = false; ledPWMRange = new ArrayList<>(); + ledPWMFrequency = 0; ledPWMSetRange = ""; ledDimCommand = ""; @@ -81,6 +83,7 @@ public class HardwareConfig { this.ledsCanDim = (Boolean) hardware.get("ledsCanDim"); this.ledPWMRange = (ArrayList) hardware.get("ledPWMRange"); this.ledPWMSetRange = (String) hardware.get("ledPWMSetRange"); + this.ledPWMFrequency = (Integer) hardware.get("ledPWMFrequency"); this.ledDimCommand = (String) hardware.get("ledDimCommand"); this.ledBlinkCommand = (String) hardware.get("ledBlinkCommand"); diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/PiGPIO.java b/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/PiGPIO.java index 946b3f00b..a8e2c1732 100644 --- a/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/PiGPIO.java +++ b/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/PiGPIO.java @@ -35,10 +35,10 @@ public class PiGPIO extends GPIOBase { return Singleton.INSTANCE; } - public PiGPIO(int address, int value, int range) { + public PiGPIO(int address, int frequency, int range) { this.pin = address; try { - getPigpioDaemon().setPWMFrequency(this.pin, value); + getPigpioDaemon().setPWMFrequency(this.pin, frequency); getPigpioDaemon().setPWMRange(this.pin, range); } catch (PigpioException e) { logger.error("Could not set PWM settings on port " + this.pin); diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/HardwareManager.java b/photon-server/src/main/java/org/photonvision/common/hardware/HardwareManager.java index a75c1ddb3..db57715e8 100644 --- a/photon-server/src/main/java/org/photonvision/common/hardware/HardwareManager.java +++ b/photon-server/src/main/java/org/photonvision/common/hardware/HardwareManager.java @@ -24,12 +24,9 @@ import org.photonvision.common.hardware.GPIO.GPIOBase; import org.photonvision.common.hardware.GPIO.PiGPIO; import org.photonvision.common.hardware.metrics.MetricsBase; import org.photonvision.common.hardware.metrics.MetricsPublisher; -import org.photonvision.common.logging.LogGroup; -import org.photonvision.common.logging.Logger; public class HardwareManager { HardwareConfig hardwareConfig; - private static final Logger logger = new Logger(HardwareManager.class, LogGroup.General); private static final HashMap LEDs = new HashMap<>(); public static HardwareManager getInstance() { @@ -44,14 +41,16 @@ public class HardwareManager { hardwareConfig.ledPins.forEach( pin -> { if (Platform.isRaspberryPi()) { - LEDs.put(pin, new PiGPIO(pin, 0, hardwareConfig.ledPWMRange.get(1))); + LEDs.put( + pin, + new PiGPIO(pin, hardwareConfig.ledPWMFrequency, hardwareConfig.ledPWMRange.get(1))); } else { LEDs.put(pin, new CustomGPIO(pin)); } }); // Start hardware metrics thread - MetricsPublisher.getInstance().startThread(); + MetricsPublisher.getInstance().startTask(); } /** Example: HardwareManager.getInstance().getPWM(port).dimLEDs(int dimValue); */ public GPIOBase getGPIO(int pin) { diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/metrics/MetricsBase.java b/photon-server/src/main/java/org/photonvision/common/hardware/metrics/MetricsBase.java index c793a2536..b9e2b8eb7 100644 --- a/photon-server/src/main/java/org/photonvision/common/hardware/metrics/MetricsBase.java +++ b/photon-server/src/main/java/org/photonvision/common/hardware/metrics/MetricsBase.java @@ -17,7 +17,7 @@ package org.photonvision.common.hardware.metrics; -import java.util.Arrays; +import java.io.IOException; import org.photonvision.common.configuration.HardwareConfig; import org.photonvision.common.hardware.Platform; import org.photonvision.common.logging.LogGroup; @@ -58,9 +58,25 @@ public abstract class MetricsBase { try { runCommand.executeBashCommand(command); return Double.parseDouble(runCommand.getOutput()); - } catch (Exception e) { - logger.error(Arrays.toString(e.getStackTrace())); + } catch (NumberFormatException e) { + logger.error( + "Command: \"" + + command + + "\" returned a non-double output!" + + "\nOutput Received: " + + runCommand.getOutput() + + "\nStandard Error: " + + runCommand.getError() + + "\nCommand completed: " + + runCommand.isOutputCompleted() + + "\nError completed: " + + runCommand.isErrorCompleted() + + "\nExit code: " + + runCommand.getExitCode()); return Double.NaN; + } catch (IOException e) { + MetricsPublisher.getInstance().stopTask(); + return -1; } } } diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/metrics/MetricsPublisher.java b/photon-server/src/main/java/org/photonvision/common/hardware/metrics/MetricsPublisher.java index b35244b90..e213e581c 100644 --- a/photon-server/src/main/java/org/photonvision/common/hardware/metrics/MetricsPublisher.java +++ b/photon-server/src/main/java/org/photonvision/common/hardware/metrics/MetricsPublisher.java @@ -18,54 +18,54 @@ package org.photonvision.common.hardware.metrics; import java.util.HashMap; -import java.util.Timer; -import java.util.TimerTask; import org.photonvision.common.dataflow.DataChangeService; import org.photonvision.common.dataflow.events.OutgoingUIEvent; +import org.photonvision.common.logging.LogGroup; +import org.photonvision.common.logging.Logger; +import org.photonvision.common.util.TimedTaskManager; import org.photonvision.server.UIUpdateType; public class MetricsPublisher { private final HashMap metrics; - private final Thread metricsThread; + private static final Logger logger = new Logger(MetricsPublisher.class, LogGroup.General); + private static CPU cpu; + private static GPU gpu; + private static RAM ram; public static MetricsPublisher getInstance() { return Singleton.INSTANCE; } private MetricsPublisher() { - var cpu = CPU.getInstance(); - var gpu = GPU.getInstance(); - var ram = RAM.getInstance(); + cpu = CPU.getInstance(); + gpu = GPU.getInstance(); + ram = RAM.getInstance(); metrics = new HashMap<>(); - - this.metricsThread = - new Thread( - () -> { - var timer = new Timer(); - timer.schedule( - new TimerTask() { - public void run() { - metrics.put("cpuTemp", cpu.getTemp()); - metrics.put("cpuUtil", cpu.getUtilization()); - metrics.put("cpuMem", cpu.getMemory()); - metrics.put("gpuTemp", gpu.getTemp()); - metrics.put("gpuMem", gpu.getMemory()); - metrics.put("ramUtil", ram.getUsedRam()); - - DataChangeService.getInstance() - .publishEvent( - new OutgoingUIEvent<>( - UIUpdateType.BROADCAST, "metrics", metrics, null)); - } - }, - 0, - 1000); - }); } - public void startThread() { - metricsThread.start(); + public void startTask() { + TimedTaskManager.getInstance() + .addTask( + "Metrics", + () -> { + metrics.put("cpuTemp", cpu.getTemp()); + metrics.put("cpuUtil", cpu.getUtilization()); + metrics.put("cpuMem", cpu.getMemory()); + metrics.put("gpuTemp", gpu.getTemp()); + metrics.put("gpuMem", gpu.getMemory()); + metrics.put("ramUtil", ram.getUsedRam()); + + DataChangeService.getInstance() + .publishEvent( + new OutgoingUIEvent<>(UIUpdateType.BROADCAST, "metrics", metrics, null)); + }, + 1000); + } + + public void stopTask() { + TimedTaskManager.getInstance().cancelTask("Metrics"); + logger.info("This device does not support running bash commands. Stopped metrics thread."); } private static class Singleton { diff --git a/photon-server/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java b/photon-server/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java index 8dd6b5b03..93b89564e 100644 --- a/photon-server/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java +++ b/photon-server/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java @@ -38,16 +38,21 @@ public class CameraCalibrationCoefficients implements Releasable { @JsonProperty("perViewErrors") public final double[] perViewErrors; + @JsonProperty("standardDeviation") + public final double standardDeviation; + @JsonCreator public CameraCalibrationCoefficients( @JsonProperty("resolution") Size resolution, @JsonProperty("cameraIntrinsics") JsonMat cameraIntrinsics, @JsonProperty("cameraExtrinsics") JsonMat cameraExtrinsics, - @JsonProperty("perViewErrors") double[] perViewErrors) { + @JsonProperty("perViewErrors") double[] perViewErrors, + @JsonProperty("standardDeviation") double standardDeviation) { this.resolution = resolution; this.cameraIntrinsics = cameraIntrinsics; this.cameraExtrinsics = cameraExtrinsics; this.perViewErrors = perViewErrors; + this.standardDeviation = standardDeviation; } @JsonIgnore @@ -65,6 +70,11 @@ public class CameraCalibrationCoefficients implements Releasable { return perViewErrors; } + @JsonIgnore + public double getStandardDeviation() { + return standardDeviation; + } + @Override public void release() { cameraIntrinsics.release(); diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java index a21e745ab..35d6e1432 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java @@ -85,6 +85,13 @@ public class Calibrate3dPipe } JsonMat cameraMatrixMat = JsonMat.fromMat(cameraMatrix); JsonMat distortionCoefficientsMat = JsonMat.fromMat(distortionCoefficients); + // Create a new CameraCalibrationCoefficients object to pass onto SolvePnP + double[] perViewErrorsArray = + new double[(int) perViewErrors.total() * perViewErrors.channels()]; + perViewErrors.get(0, 0, perViewErrorsArray); + + // Standard deviation of results + double stdDev = calculateSD(perViewErrorsArray); try { // Print calibration successful logger.info( @@ -94,16 +101,32 @@ public class Calibrate3dPipe + new ObjectMapper().writeValueAsString(cameraMatrixMat) + "\ndistortionCoeffs:\n" + new ObjectMapper().writeValueAsString(distortionCoefficientsMat) + + "\nWith Standard Deviation Of\n" + + stdDev + "\n"); } catch (JsonProcessingException e) { logger.error("Failed to parse calibration data to json!", e); } - // Create a new CameraCalibrationCoefficients object to pass onto SolvePnP - double[] perViewErrorsArray = - new double[(int) perViewErrors.total() * perViewErrors.channels()]; - perViewErrors.get(0, 0, perViewErrorsArray); return new CameraCalibrationCoefficients( - params.resolution, cameraMatrixMat, distortionCoefficientsMat, perViewErrorsArray); + params.resolution, cameraMatrixMat, distortionCoefficientsMat, perViewErrorsArray, stdDev); + } + + // Calculate standard deviation of the RMS error of the snapshots + private static double calculateSD(double numArray[]) { + double sum = 0.0, standardDeviation = 0.0; + int length = numArray.length; + + for (double num : numArray) { + sum += num; + } + + double mean = sum / length; + + for (double num : numArray) { + standardDeviation += Math.pow(num - mean, 2); + } + + return Math.sqrt(standardDeviation / length); } public static class CalibratePipeParams { diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipeline.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipeline.java index 8f920e2b9..f2513e553 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipeline.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipeline.java @@ -20,6 +20,8 @@ package org.photonvision.vision.pipeline; import java.util.ArrayList; import java.util.List; import org.opencv.core.Mat; +import org.photonvision.common.logging.LogGroup; +import org.photonvision.common.logging.Logger; import org.photonvision.common.util.math.MathUtils; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.frame.Frame; @@ -33,6 +35,9 @@ import org.photonvision.vision.pipeline.result.CVPipelineResult; public class Calibration3dPipeline extends CVPipeline { + // For loggging + private static final Logger logger = new Logger(Calibration3dPipeline.class, LogGroup.General); + // Only 2 pipes needed, one for finding the board corners and one for actually calibrating private final FindBoardCornersPipe findBoardCornersPipe = new FindBoardCornersPipe(); private final Calibrate3dPipe calibrate3dPipe = new Calibrate3dPipe(); @@ -79,6 +84,9 @@ public class Calibration3dPipeline long sumPipeNanosElapsed = 0L; + // Check if the frame has chessboard corners + var hasBoard = findBoardCornersPipe.findBoardCorners(frame.image.getMat()); + // hasEnough() is a getter method for numSnapshots that checks if there are more than 25 // snapshots // calibrate will be true when it is get by it's putter method @@ -94,28 +102,26 @@ public class Calibration3dPipeline sumPipeNanosElapsed += calibrationOutput.nanosElapsed; calibrate = false; - numSnapshots = 0; - boardSnapshots.clear(); - } else if (takeSnapshot) { - var hasBoard = findBoardCornersPipe.findBoardCorners(frame.image.getMat()); if (hasBoard.getLeft()) { Mat board = new Mat(); frame.image.getMat().copyTo(board); - // See if mat is empty + // Add board to snapshots boardSnapshots.add(board); // Set snapshot to false and increment number of snapshots taken takeSnapshot = false; numSnapshots++; - return new CVPipelineResult( - MathUtils.nanosToMillis(sumPipeNanosElapsed), - null, - new Frame(new CVMat(hasBoard.getRight()), frame.frameStaticProperties)); } } - return new CVPipelineResult(MathUtils.nanosToMillis(sumPipeNanosElapsed), null, frame); + // Return the drawn chessboard if corners are found, if not, then return the input image. + return new CVPipelineResult( + MathUtils.nanosToMillis(sumPipeNanosElapsed), + null, + new Frame( + new CVMat(hasBoard.getLeft() ? hasBoard.getRight() : frame.image.getMat()), + frame.frameStaticProperties)); } public boolean hasEnough() { @@ -134,6 +140,21 @@ public class Calibration3dPipeline return calibrationOutput.output.perViewErrors; } + public void finishCalibration() { + numSnapshots = 0; + boardSnapshots.clear(); + } + + public boolean removeSnapshot(int index) { + try { + boardSnapshots.remove(index); + return true; + } catch (ArrayIndexOutOfBoundsException e) { + logger.error("Could not remove snapshot at index " + index, e); + return false; + } + } + public CameraCalibrationCoefficients cameraCalibrationCoefficients() { return calibrationOutput.output; } diff --git a/photon-server/src/test/java/org/photonvision/hardware/HardwareConfigTest.java b/photon-server/src/test/java/org/photonvision/hardware/HardwareConfigTest.java index 3859e7f4f..fca3d3baf 100644 --- a/photon-server/src/test/java/org/photonvision/hardware/HardwareConfigTest.java +++ b/photon-server/src/test/java/org/photonvision/hardware/HardwareConfigTest.java @@ -42,6 +42,7 @@ public class HardwareConfigTest { config.ledPins.stream().mapToInt(i -> i).toArray(), new int[] {2, 13}); Assertions.assertArrayEquals( config.ledPWMRange.stream().mapToInt(i -> i).toArray(), new int[] {0, 100}); + Assertions.assertEquals(config.ledPWMFrequency, 800); CustomGPIO.setConfig(config); diff --git a/photon-server/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java b/photon-server/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java index c43a93a4f..59111daa2 100644 --- a/photon-server/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java +++ b/photon-server/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java @@ -27,7 +27,6 @@ import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.opencv.core.Mat; import org.opencv.core.Size; -import org.opencv.highgui.HighGui; import org.opencv.imgcodecs.Imgcodecs; import org.photonvision.common.util.TestUtils; import org.photonvision.vision.frame.Frame; @@ -86,14 +85,16 @@ public class Calibrate3dPipeTest { new Frame( new CVMat(Imgcodecs.imread(file.getAbsolutePath())), new FrameStaticProperties(640, 480, 60))); - HighGui.imshow("Calibration Output Frame", output.outputFrame.image.getMat()); + TestUtils.showImage(output.outputFrame.image.getMat()); } + calibration3dPipeline.removeSnapshot(0); calibration3dPipeline.startCalibration(); calibration3dPipeline.run( new Frame( new CVMat(Imgcodecs.imread(directoryListing[0].getAbsolutePath())), new FrameStaticProperties(640, 480, 60))); + calibration3dPipeline.finishCalibration(); System.out.println( "Per View Errors: " + Arrays.toString(calibration3dPipeline.perViewErrors())); System.out.println( @@ -102,5 +103,10 @@ public class Calibrate3dPipeTest { System.out.println( "Camera Extrinsics : " + calibration3dPipeline.cameraCalibrationCoefficients().cameraExtrinsics.toString()); + System.out.println( + "Standard Deviation: " + + calibration3dPipeline.cameraCalibrationCoefficients().standardDeviation); + System.out.println( + "Mean: " + Arrays.stream(calibration3dPipeline.perViewErrors()).average().toString()); } } diff --git a/photon-server/src/test/resources/hardware/HardwareConfig.json b/photon-server/src/test/resources/hardware/HardwareConfig.json index 279a9cb2a..70ee11600 100644 --- a/photon-server/src/test/resources/hardware/HardwareConfig.json +++ b/photon-server/src/test/resources/hardware/HardwareConfig.json @@ -7,6 +7,7 @@ "ledSetCommand": "", "ledsCanDim": true, "ledPWMRange": [0, 100], + "ledPWMFrequency" : 800, "ledPWMSetRange": "", "ledDimCommand": "", "ledBlinkCommand": ""