Cal checkerboard object coords update (#181)

* WIP monkeying around with adding new targets.

* Added testcase to replicate large extrinsics result from calibration.

* Tweaked calibration pipline to return image with chessboard corner detection overlaid.

* Removed "bad" images from the cal, but that didn't seem to help...

* Added test logic to output the undistorted Mat for evaluating the quality of the calibration.

* Tweaked generation of chessboard points to be in a square pattern, not parallelogram.

* Spotless and removed bespoke test target in prep for PR.

* Revised to a double-for loop for less complex-looking logic

* #thanks spotless

al;ksgfjh akljghf ;lakdfdhg ksadfgh klasdfjhg kasdfjghj aklsjg two spaces

* Boop

* Reverting my changes in Calibrate3dPipeline.java to make this merge better with other PR's

* Derp changed the wrong one

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: ""

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

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

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

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());
+    }
 }