Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-22 01:11:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

standardize on calibration in inches and add square size as a divisor

Browse Source 
This should do the same thing as calibrating in the correct units but it doesnt like meeeeee
This commit is contained in:
Matt
2019-12-31 16:38:52 -08:00
parent 347b3274d7
commit 9559bb360e
5 changed files with 32 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
chameleon-server/src/main/java/com/chameleonvision/config/CameraCalibrationConfig.java
View File

@@ -15,21 +15,25 @@ public class CameraCalibrationConfig {
@JsonProperty("resolution") public final Size resolution;
@JsonProperty("cameraMatrix") public final JsonMat cameraMatrix;
@JsonProperty("distortionCoeffs") public final JsonMat distortionCoeffs;
@JsonProperty("squareSize") public final double squareSize;
@JsonCreator
public CameraCalibrationConfig(
@JsonProperty("resolution") Size resolution,
@JsonProperty("cameraMatrix") JsonMat cameraMatrix,
@JsonProperty("distortionCoeffs") JsonMat distortionCoeffs) {
@JsonProperty("distortionCoeffs") JsonMat distortionCoeffs,
@JsonProperty("squareSize") double squareSize) {
this.resolution = resolution;
this.cameraMatrix = cameraMatrix;
this.distortionCoeffs = distortionCoeffs;
this.squareSize = squareSize;
}
public CameraCalibrationConfig(Size resolution, Mat cameraMatrix, Mat distortionCoeffs) {
public CameraCalibrationConfig(Size resolution, Mat cameraMatrix, Mat distortionCoeffs, double squareSize) {
this.resolution = resolution;
this.cameraMatrix = JsonMat.fromMat(cameraMatrix);
this.distortionCoeffs = JsonMat.fromMat(distortionCoeffs);
this.squareSize = squareSize;
}
@JsonIgnoreType

5
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/Calibrate3dPipeline.java
View File

@@ -40,6 +40,7 @@ public class Calibrate3dPipeline extends CVPipeline<DriverVisionPipeline.DriverP
private int captureCount = 0;
private boolean wantsSnapshot = false;
private double squareSizeInches;
public Calibrate3dPipeline(StandardCVPipelineSettings settings) {
super(settings);
@@ -60,7 +61,7 @@ public class Calibrate3dPipeline extends CVPipeline<DriverVisionPipeline.DriverP
}
public void setSquareSize(double size) {
Core.multiply(objP_ORIG, new Scalar(new double[] { size, size, size }), objP);
this.squareSizeInches = size;
}
public void takeSnapshot() {
@@ -139,7 +140,7 @@ public class Calibrate3dPipeline extends CVPipeline<DriverVisionPipeline.DriverP
VideoMode currentVidMode = cameraCapture.getCurrentVideoMode();
Size resolution = new Size(currentVidMode.width, currentVidMode.height);
CameraCalibrationConfig cal = new CameraCalibrationConfig(resolution, cameraMatrix, distortionCoeffs);
CameraCalibrationConfig cal = new CameraCalibrationConfig(resolution, cameraMatrix, distortionCoeffs, squareSizeInches);
VisionManager.getCurrentUIVisionProcess().addCalibration(cal);

4
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/SolvePNPPipe.java
View File

@@ -33,6 +33,7 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
private List<StandardCVPipeline.TrackedTarget> poseList = new ArrayList<>();
Comparator<Point> leftRightComparator = Comparator.comparingDouble(point -> point.x);
Comparator<Point> verticalComparator = Comparator.comparingDouble(point -> point.y);
private double distanceDivisor = 1.0;
public SolvePNPPipe(StandardCVPipelineSettings settings, CameraCalibrationConfig calibration, Rotation2d tilt) {
super();
@@ -78,6 +79,7 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
distortionCoefficients.release();
settings.getDistortionCoeffsAsMat().copyTo(distortionCoefficients);
}
this.distanceDivisor = settings.squareSize;
}
@Override
@@ -235,7 +237,7 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
var targetAngle = -angle1; // radians
var targetRotation = -angle2; // radians
//noinspection UnnecessaryLocalVariable
var targetDistance = distance; // meters or whatever the calibration was in
var targetDistance = distance * 25.4 / 1000d / distanceDivisor; // meters or whatever the calibration was in
var targetLocation = new Translation2d(targetDistance * FastMath.cos(targetAngle), targetDistance * FastMath.sin(targetAngle));
target.cameraRelativePose = new Pose2d(targetLocation, new Rotation2d(targetRotation));

24
chameleon-server/src/main/java/com/chameleonvision/web/RequestHandler.java
View File

@@ -22,8 +22,10 @@ import java.util.Map;
public class RequestHandler {
private static final ObjectMapper kObjectMapper = new ObjectMapper();
public static void onGeneralSettings(Context ctx) {
ObjectMapper objectMapper = new ObjectMapper();
ObjectMapper objectMapper = kObjectMapper;
try {
Map map = objectMapper.readValue(ctx.body(), Map.class);
@@ -44,7 +46,7 @@ public class RequestHandler {
}
public static void onDuplicatePipeline(Context ctx) {
ObjectMapper objectMapper = new ObjectMapper();
ObjectMapper objectMapper = kObjectMapper;
try {
Map newPipelineData = objectMapper.readValue(ctx.body(), Map.class);
@@ -81,7 +83,7 @@ public class RequestHandler {
}
public static void onCameraSettings(Context ctx) {
ObjectMapper objectMapper = new ObjectMapper();
ObjectMapper objectMapper = kObjectMapper;
try {
Map camSettings = objectMapper.readValue(ctx.body(), Map.class);
@@ -112,7 +114,7 @@ public class RequestHandler {
public static void onCalibrationStart(Context ctx) throws JsonProcessingException {
PipelineManager pipeManager = VisionManager.getCurrentUIVisionProcess().pipelineManager;
ObjectMapper objectMapper = new ObjectMapper();
ObjectMapper objectMapper = kObjectMapper;
var data = objectMapper.readValue(ctx.body(), Map.class);
int resolutionIndex = (Integer) data.get("resolution");
double squareSize;
@@ -122,7 +124,7 @@ public class RequestHandler {
squareSize = (Integer) data.get("squareSize");
}
// convert from mm to meters
pipeManager.calib3dPipe.setSquareSize(squareSize / 1000d);
pipeManager.calib3dPipe.setSquareSize(squareSize);
VisionManager.getCurrentUIVisionProcess().pipelineManager.calib3dPipe.settings.videoModeIndex = resolutionIndex;
VisionManager.getCurrentUIVisionProcess().pipelineManager.setCalibrationMode(true);
VisionManager.getCurrentUIVisionProcess().getCamera().setVideoMode(resolutionIndex);
@@ -143,6 +145,16 @@ public class RequestHandler {
public static void onCalibrationEnding(Context ctx) throws JsonProcessingException {
PipelineManager pipeManager = VisionManager.getCurrentUIVisionProcess().pipelineManager;
var data = kObjectMapper.readValue(ctx.body(), Map.class);
double squareSize;
try {
squareSize = (Double) data.get("squareSize");
} catch (Exception e) {
squareSize = (Integer) data.get("squareSize");
}
pipeManager.calib3dPipe.setSquareSize(squareSize);
System.out.println("Finishing Cal");
if (pipeManager.calib3dPipe.hasEnoughSnapshots()) {
if (pipeManager.calib3dPipe.tryCalibration()) {
@@ -158,7 +170,7 @@ public class RequestHandler {
public static void onPnpModel(Context ctx) throws JsonProcessingException {
System.out.println(ctx.body());
ObjectMapper objectMapper = new ObjectMapper();
ObjectMapper objectMapper = kObjectMapper;
List points = objectMapper.readValue(ctx.body(), List.class);
System.out.println(points);
}