diff --git a/photon-client/src/views/PipelineViews/PnPTab.vue b/photon-client/src/views/PipelineViews/PnPTab.vue index a9912f3f9..2543e9b2b 100644 --- a/photon-client/src/views/PipelineViews/PnPTab.vue +++ b/photon-client/src/views/PipelineViews/PnPTab.vue @@ -60,7 +60,7 @@ }, data() { return { - targetList: ['2020 High Goal Outer', '2020 High Goal Inner', '2019 Dual Target', 'Power Cell (7in)'], + targetList: ['2020 High Goal Outer', '2020 High Goal Inner', '2019 Dual Target', 'Power Cell (7in)', '2016 High Goal'], //Keep in sync with TargetModel.java snackbar: { color: "Success", text: "" diff --git a/photon-server/src/main/java/org/photonvision/common/util/TestUtils.java b/photon-server/src/main/java/org/photonvision/common/util/TestUtils.java index 9ee7747dc..c7afe976a 100644 --- a/photon-server/src/main/java/org/photonvision/common/util/TestUtils.java +++ b/photon-server/src/main/java/org/photonvision/common/util/TestUtils.java @@ -182,6 +182,10 @@ public class TestUtils { return getResourcesFolderPath(false).resolve("calibrationBoardImages"); } + public static Path getSquaresBoardImagesPath() { + return getResourcesFolderPath(false).resolve("calibrationSquaresImg"); + } + public static File getHardwareConfigJson() { return getResourcesFolderPath(false) .resolve("hardware") diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java index a4530c87f..deba68f2b 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java @@ -71,13 +71,12 @@ public class FindBoardCornersPipe // dots per column if (params.type == UICalibrationData.BoardType.CHESSBOARD) { // Here we can create an NxN grid since a chessboard is rectangular - for (int i = 0; i < patternSize.height * patternSize.width; i++) { - objectPoints.push_back( - new MatOfPoint3f( - new Point3( - (double) i / patternSize.width * params.gridSize, - i % patternSize.width * params.gridSize, - 0.0f))); + for (int heightIdx = 0; heightIdx < patternSize.height; heightIdx++) { + for (int widthIdx = 0; widthIdx < patternSize.height; widthIdx++) { + double boardYCoord = heightIdx * params.gridSize; + double boardXCoord = widthIdx * params.gridSize; + objectPoints.push_back(new MatOfPoint3f(new Point3(boardXCoord, boardYCoord, 0.0))); + } } } else if (params.type == UICalibrationData.BoardType.DOTBOARD) { // Here we need to alternate the amount of dots per column since a dot board is not diff --git a/photon-server/src/main/java/org/photonvision/vision/target/TargetModel.java b/photon-server/src/main/java/org/photonvision/vision/target/TargetModel.java index 634e15fe3..f75540942 100644 --- a/photon-server/src/main/java/org/photonvision/vision/target/TargetModel.java +++ b/photon-server/src/main/java/org/photonvision/vision/target/TargetModel.java @@ -73,7 +73,15 @@ public enum TargetModel implements Releasable { Units.inchesToMeters(7) / 2, -Units.inchesToMeters(7) / 2, -Units.inchesToMeters(7) / 2)), - 0); + 0), + k2016HighGoal( + List.of( + new Point3(Units.inchesToMeters(-10), Units.inchesToMeters(12), 0), + new Point3(Units.inchesToMeters(-10), Units.inchesToMeters(0), 0), + new Point3(Units.inchesToMeters(10), Units.inchesToMeters(0), 0), + new Point3(Units.inchesToMeters(10), Units.inchesToMeters(12), 0)), + Units.inchesToMeters(6)), + ; @JsonIgnore private MatOfPoint3f realWorldTargetCoordinates; @JsonIgnore private MatOfPoint3f visualizationBoxBottom = new MatOfPoint3f(); diff --git a/photon-server/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java b/photon-server/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java index 354892a84..77539910d 100644 --- a/photon-server/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java +++ b/photon-server/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java @@ -32,6 +32,7 @@ import org.junit.jupiter.api.Test; import org.opencv.core.Mat; import org.opencv.core.Size; import org.opencv.imgcodecs.Imgcodecs; +import org.opencv.imgproc.Imgproc; import org.photonvision.common.util.TestUtils; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameStaticProperties; @@ -97,7 +98,7 @@ public class Calibrate3dPipeTest { new Frame( new CVMat(Imgcodecs.imread(file.getAbsolutePath())), new FrameStaticProperties(640, 480, 60, new Rotation2d(), null))); - // TestUtils.showImage(output.outputFrame.image.getMat()); + // TestUtils.showImage(output.outputFrame.image.getMat()); } assertTrue( @@ -128,4 +129,56 @@ public class Calibrate3dPipeTest { System.out.println( "Mean: " + Arrays.stream(calibration3dPipeline.perViewErrors()).average().toString()); } + + @Test + public void calibrateSquaresTest() { + + File dir = new File(TestUtils.getSquaresBoardImagesPath().toAbsolutePath().toString()); + File[] directoryListing = dir.listFiles(); + + Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline(20); + calibration3dPipeline.getSettings().boardType = UICalibrationData.BoardType.CHESSBOARD; + calibration3dPipeline.getSettings().resolution = new Size(320, 240); + + for (var file : directoryListing) { + if (file.isFile()) { + calibration3dPipeline.takeSnapshot(); + var output = + calibration3dPipeline.run( + new Frame( + new CVMat(Imgcodecs.imread(file.getAbsolutePath())), + new FrameStaticProperties(320, 240, 67, new Rotation2d(), null))); + + // TestUtils.showImage(output.outputFrame.image.getMat(), file.getName()); + } + } + + assertTrue( + calibration3dPipeline.foundCornersList.stream() + .map(Triple::getRight) + .allMatch(it -> it.width() > 0 && it.height() > 0)); + + var cal = calibration3dPipeline.tryCalibration(); + calibration3dPipeline.finishCalibration(); + + for (var file : directoryListing) { + if (file.isFile()) { + Mat raw = Imgcodecs.imread(file.getAbsolutePath()); + Mat undistorted = new Mat(new Size(600, 600), raw.type()); + Imgproc.undistort( + raw, undistorted, cal.cameraIntrinsics.getAsMat(), cal.cameraExtrinsics.getAsMat()); + + TestUtils.showImage(undistorted, "undistorted " + file.getName(), 1); + } + } + + assertNotNull(cal); + assertNotNull(cal.perViewErrors); + System.out.println("Per View Errors: " + Arrays.toString(cal.perViewErrors)); + System.out.println("Camera Intrinsics : " + cal.cameraIntrinsics.toString()); + System.out.println("Camera Extrinsics : " + cal.cameraExtrinsics.toString()); + System.out.println("Standard Deviation: " + cal.standardDeviation); + System.out.println( + "Mean: " + Arrays.stream(calibration3dPipeline.perViewErrors()).average().toString()); + } } diff --git a/photon-server/src/test/resources/calibrationSquaresImg/bad/img21.jpg b/photon-server/src/test/resources/calibrationSquaresImg/bad/img21.jpg new file mode 100644 index 000000000..3e4e926ac Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/bad/img21.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/bad/img23.jpg b/photon-server/src/test/resources/calibrationSquaresImg/bad/img23.jpg new file mode 100644 index 000000000..9badd2fc9 Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/bad/img23.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/bad/img24.jpg b/photon-server/src/test/resources/calibrationSquaresImg/bad/img24.jpg new file mode 100644 index 000000000..b8784deb2 Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/bad/img24.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/bad/img9.jpg b/photon-server/src/test/resources/calibrationSquaresImg/bad/img9.jpg new file mode 100644 index 000000000..3eafeb000 Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/bad/img9.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img1.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img1.jpg new file mode 100644 index 000000000..343d5abc8 Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img1.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img10.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img10.jpg new file mode 100644 index 000000000..5ccb4e571 Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img10.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img11.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img11.jpg new file mode 100644 index 000000000..b8140f063 Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img11.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img12.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img12.jpg new file mode 100644 index 000000000..d0ee2e5c6 Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img12.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img13.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img13.jpg new file mode 100644 index 000000000..accbf3981 Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img13.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img14.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img14.jpg new file mode 100644 index 000000000..3e084b350 Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img14.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img15.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img15.jpg new file mode 100644 index 000000000..926816385 Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img15.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img16.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img16.jpg new file mode 100644 index 000000000..bf65792df Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img16.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img17.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img17.jpg new file mode 100644 index 000000000..f604ccb5a Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img17.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img18.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img18.jpg new file mode 100644 index 000000000..2cc3118fa Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img18.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img19.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img19.jpg new file mode 100644 index 000000000..7a82159af Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img19.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img2.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img2.jpg new file mode 100644 index 000000000..b28a91e9e Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img2.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img20.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img20.jpg new file mode 100644 index 000000000..5aa9d5c24 Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img20.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img22.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img22.jpg new file mode 100644 index 000000000..b6f384051 Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img22.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img25.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img25.jpg new file mode 100644 index 000000000..8050f3d0f Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img25.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img26.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img26.jpg new file mode 100644 index 000000000..ec28fdac7 Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img26.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img3.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img3.jpg new file mode 100644 index 000000000..7ed355300 Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img3.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img4.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img4.jpg new file mode 100644 index 000000000..a29edef9e Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img4.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img5.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img5.jpg new file mode 100644 index 000000000..0d04fc8fc Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img5.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img6.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img6.jpg new file mode 100644 index 000000000..1806b513c Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img6.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img7.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img7.jpg new file mode 100644 index 000000000..9ab415586 Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img7.jpg differ diff --git a/photon-server/src/test/resources/calibrationSquaresImg/img8.jpg b/photon-server/src/test/resources/calibrationSquaresImg/img8.jpg new file mode 100644 index 000000000..f48300375 Binary files /dev/null and b/photon-server/src/test/resources/calibrationSquaresImg/img8.jpg differ