Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-20 00:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Calibration and metrics clean up. (#68)

Browse Source 
* Add more calibration metrics
* Cleanup metrics
* Rename PWM fields to match convention
This commit is contained in:
Xzibit
2020-08-04 16:19:40 -04:00
committed by GitHub
parent e2768eaee8
commit dea68041fb
11 changed files with 140 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
photon-server/src/main/java/org/photonvision/common/configuration/HardwareConfig.java
View File

@@ -35,6 +35,7 @@ public class HardwareConfig {
public final boolean ledsCanDim;
public final ArrayList<Integer> 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<Integer>) 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");

4
photon-server/src/main/java/org/photonvision/common/hardware/GPIO/PiGPIO.java
View File

@@ -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);

9
photon-server/src/main/java/org/photonvision/common/hardware/HardwareManager.java
View File

@@ -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<Integer, GPIOBase> 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) {

22
photon-server/src/main/java/org/photonvision/common/hardware/metrics/MetricsBase.java
View File

@@ -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;
}
}
}

64
photon-server/src/main/java/org/photonvision/common/hardware/metrics/MetricsPublisher.java
View File

@@ -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<String, Double> 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 {

12
photon-server/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java
View File

@@ -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();

33
photon-server/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java
View File

@@ -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 {

41
photon-server/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipeline.java
View File

@@ -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<CVPipelineResult, Calibration3dPipelineSettings> {
// 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;
}