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

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