From 9559bb360eb8dae7357ea39e5abb7649c248ec6c Mon Sep 17 00:00:00 2001 From: Matt Date: Tue, 31 Dec 2019 16:38:52 -0800 Subject: [PATCH] standardize on calibration in inches and add square size as a divisor This should do the same thing as calibrating in the correct units but it doesnt like meeeeee --- .../src/views/SettingsViewes/Cameras.vue | 4 ++-- .../config/CameraCalibrationConfig.java | 8 +++++-- .../pipeline/impl/Calibrate3dPipeline.java | 5 ++-- .../vision/pipeline/pipes/SolvePNPPipe.java | 4 +++- .../chameleonvision/web/RequestHandler.java | 24 ++++++++++++++----- 5 files changed, 32 insertions(+), 13 deletions(-) diff --git a/chameleon-client/src/views/SettingsViewes/Cameras.vue b/chameleon-client/src/views/SettingsViewes/Cameras.vue index 405607be1..9e158e76a 100644 --- a/chameleon-client/src/views/SettingsViewes/Cameras.vue +++ b/chameleon-client/src/views/SettingsViewes/Cameras.vue @@ -17,7 +17,7 @@ - + @@ -74,7 +74,7 @@ text: "Cancel Calibration", color: "red" }, - squareSize: 2.54, + squareSize: 1.0, snapshotAmount: 0, hasEnough: false, snack: false diff --git a/chameleon-server/src/main/java/com/chameleonvision/config/CameraCalibrationConfig.java b/chameleon-server/src/main/java/com/chameleonvision/config/CameraCalibrationConfig.java index e90b98c98..2dbed87b2 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/config/CameraCalibrationConfig.java +++ b/chameleon-server/src/main/java/com/chameleonvision/config/CameraCalibrationConfig.java @@ -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 diff --git a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/Calibrate3dPipeline.java b/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/Calibrate3dPipeline.java index 19bf20cda..fbba9856c 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/Calibrate3dPipeline.java +++ b/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/Calibrate3dPipeline.java @@ -40,6 +40,7 @@ public class Calibrate3dPipeline extends CVPipeline private List poseList = new ArrayList<>(); Comparator leftRightComparator = Comparator.comparingDouble(point -> point.x); Comparator 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 distortionCoefficients.release(); settings.getDistortionCoeffsAsMat().copyTo(distortionCoefficients); } + this.distanceDivisor = settings.squareSize; } @Override @@ -235,7 +237,7 @@ public class SolvePNPPipe implements Pipe 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)); diff --git a/chameleon-server/src/main/java/com/chameleonvision/web/RequestHandler.java b/chameleon-server/src/main/java/com/chameleonvision/web/RequestHandler.java index 75c89f6a1..7b52cc403 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/web/RequestHandler.java +++ b/chameleon-server/src/main/java/com/chameleonvision/web/RequestHandler.java @@ -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); }