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

Update box for 2020

Browse Source
This commit is contained in:
Matt
2020-01-05 22:29:41 -08:00
parent 46e66b9a3b
commit 1aac80c63a
1 changed files with 19 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/DrawSolvePNPPipe.java
View File

@@ -23,7 +23,8 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
public DrawSolvePNPPipe(CameraCalibrationConfig settings) {
setConfig(settings);
setObjectBox(14.5, 6, 2);
// setObjectBox(14.5, 6, 2);
set2020Box();
}
public void setObjectBox(double targetWidth, double targetHeight, double targetDepth) {
@@ -42,6 +43,20 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
);
}
public void set2020Box() {
boxCornerMat.release();
boxCornerMat = new MatOfPoint3f(
new Point3(-16.25, 0, 0),
new Point3(-9.819867, -17, 0),
new Point3(9.819867, -17, 0),
new Point3(16.25, 0, 0),
new Point3(-16.25, 0, -6),
new Point3(-9.819867, -17, -6),
new Point3(9.819867, -17, -6),
new Point3(16.25, 0, -6)
);
}
private Mat cameraMatrix = new Mat();
private MatOfDouble distortionCoefficients = new MatOfDouble();
@@ -58,13 +73,15 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
this.distortionCoefficients = distortionMatrix_;
}
MatOfPoint2f imagePoints = new MatOfPoint2f();
@Override
public Pair<Mat, Long> run(Pair<Mat, List<StandardCVPipeline.TrackedTarget>> targets) {
long processStartNanos = System.nanoTime();
var image = targets.getLeft();
for(var it : targets.getRight()) {
MatOfPoint2f imagePoints = new MatOfPoint2f();
try {
Calib3d.projectPoints(boxCornerMat, it.rVector, it.tVector, this.cameraMatrix, this.distortionCoefficients, imagePoints, new Mat() , 0);
} catch (Exception e) {