Show board outliers in calibration info card

build.gradle

@@ -32,7 +32,7 @@ ext {
     photonGlDriverLibVersion = "dev-v2023.1.0-9-g75fc678"
     rknnVersion = "dev-v2024.0.0-64-gc0836a6"
     frcYear = "2024"
-    mrcalVersion = "dev-v2024.0.0-7-gc976aaa";
+    mrcalVersion = "dev-v2024.0.0-29-g1536857";
 
 
     pubVersion = versionString

photon-client/src/components/cameras/CameraCalibrationInfoCard.vue

@@ -228,10 +228,7 @@ const calibrationImageURL = (index: number) =>
               <td>Board warp, X/Y</td>
               <td>
                 {{
-                  useCameraSettingsStore()
-                    .getCalibrationCoeffs(props.videoFormat.resolution)
-                    ?.calobjectWarp?.map((it) => (it * 1000).toFixed(2) + " mm")
-                    .join(" / ")
+                  currentCalibrationCoeffs?.calobjectWarp?.map((it) => (it * 1000).toFixed(2) + " mm").join(" / ")
                 }}
               </td>
             </tr>

photon-client/src/types/SettingTypes.ts

@@ -129,7 +129,7 @@ export interface BoardObservation {
   locationInImageSpace: CvPoint[];
   reprojectionErrors: CvPoint[];
   optimisedCameraToObject: Pose3d;
-  includeObservationInCalibration: boolean;
+  cornersUsed: boolean[];
   snapshotName: string;
   snapshotData: JsonImageMat;
 }
@@ -256,7 +256,7 @@ export const PlaceholderCameraSettings: CameraSettings = {
             { x: 2, y: 1 },
             { x: 3, y: 1 }
           ],
-          includeObservationInCalibration: false,
+          cornersUsed: [true, true, true, false, false, true],
           snapshotName: "img0.png",
           snapshotData: { rows: 480, cols: 640, type: CvType.CV_8U, data: "" }
         }

photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java

@@ -18,12 +18,21 @@
 package org.photonvision.vision.calibration;
 
 import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnore;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.geometry.Pose3d;
+import java.awt.Color;
+import java.util.Arrays;
 import java.util.List;
+import org.opencv.core.Mat;
 import org.opencv.core.Point;
 import org.opencv.core.Point3;
+import org.opencv.core.Scalar;
+import org.opencv.imgproc.Imgproc;
+import org.photonvision.common.util.ColorHelper;
 
+@JsonIgnoreProperties(ignoreUnknown = true)
 public final class BoardObservation implements Cloneable {
     // Expected feature 3d location in the camera frame
     @JsonProperty("locationInObjectSpace")
@@ -42,8 +51,8 @@ public final class BoardObservation implements Cloneable {
     public Pose3d optimisedCameraToObject;
 
     // If we should use this observation when re-calculating camera calibration
-    @JsonProperty("includeObservationInCalibration")
-    public boolean includeObservationInCalibration;
+    @JsonProperty("cornersUsed")
+    public boolean[] cornersUsed;
 
     @JsonProperty("snapshotName")
     public String snapshotName;
@@ -57,16 +66,22 @@ public BoardObservation(
             @JsonProperty("locationInImageSpace") List<Point> locationInImageSpace,
             @JsonProperty("reprojectionErrors") List<Point> reprojectionErrors,
             @JsonProperty("optimisedCameraToObject") Pose3d optimisedCameraToObject,
-            @JsonProperty("includeObservationInCalibration") boolean includeObservationInCalibration,
+            @JsonProperty("cornersUsed") boolean[] cornersUsed,
             @JsonProperty("snapshotName") String snapshotName,
             @JsonProperty("snapshotData") JsonImageMat snapshotData) {
         this.locationInObjectSpace = locationInObjectSpace;
         this.locationInImageSpace = locationInImageSpace;
         this.reprojectionErrors = reprojectionErrors;
         this.optimisedCameraToObject = optimisedCameraToObject;
-        this.includeObservationInCalibration = includeObservationInCalibration;
         this.snapshotName = snapshotName;
         this.snapshotData = snapshotData;
+
+        // legacy migration -- we assume all points are inliers
+        if (cornersUsed == null) {
+            cornersUsed = new boolean[locationInObjectSpace.size()];
+            Arrays.fill(cornersUsed, true);
+        }
+        this.cornersUsed = cornersUsed;
     }
 
     @Override
@@ -79,8 +94,8 @@ public String toString() {
                 + reprojectionErrors
                 + ", optimisedCameraToObject="
                 + optimisedCameraToObject
-                + ", includeObservationInCalibration="
-                + includeObservationInCalibration
+                + ", cornersUsed="
+                + cornersUsed
                 + ", snapshotName="
                 + snapshotName
                 + ", snapshotData="
@@ -97,4 +112,28 @@ public BoardObservation clone() {
             return null;
         }
     }
+
+    @JsonIgnore
+    public Mat getAnnotatedImage() {
+        var image = snapshotData.getAsMat().clone();
+        var diag = Math.hypot(image.width(), image.height());
+        int thickness = (int) Math.max(diag * 1.0 / 600.0, 1);
+        int r = (int) Math.max(diag * 4.0 / 500.0, 3);
+        var r2 = r / Math.sqrt(2);
+        for (int i = 0; i < this.locationInImageSpace.size(); i++) {
+            Scalar color;
+            if (cornersUsed[i]) {
+                color = ColorHelper.colorToScalar(Color.green);
+            } else {
+                color = ColorHelper.colorToScalar(Color.red);
+            }
+            var c = locationInImageSpace.get(i);
+            Imgproc.circle(image, c, r, color, thickness);
+            Imgproc.line(
+                    image, new Point(c.x - r2, c.y - r2), new Point(c.x + r2, c.y + r2), color, thickness);
+            Imgproc.line(
+                    image, new Point(c.x + r2, c.y - r2), new Point(c.x - r2, c.y + r2), color, thickness);
+        }
+        return image;
+    }
 }

photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java

@@ -144,8 +144,19 @@ protected CameraCalibrationCoefficients calibrateOpenCV(
         JsonMatOfDouble cameraMatrixMat = JsonMatOfDouble.fromMat(cameraMatrix);
         JsonMatOfDouble distortionCoefficientsMat = JsonMatOfDouble.fromMat(distortionCoefficients);
 
+        // Opencv is lame, so we can only assume all points are inliers
+        var inliners =
+                objPoints.stream()
+                        .map(
+                                it -> {
+                                    var array = new boolean[it.rows() * it.cols()];
+                                    Arrays.fill(array, true);
+                                    return array;
+                                })
+                        .collect(Collectors.toList());
+
         var observations =
-                createObservations(in, cameraMatrix, distortionCoefficients, rvecs, tvecs, null);
+                createObservations(in, cameraMatrix, distortionCoefficients, rvecs, tvecs, inliners, null);
 
         cameraMatrix.release();
         distortionCoefficients.release();
@@ -205,20 +216,17 @@ protected CameraCalibrationCoefficients calibrateMrcal(
         JsonMatOfDouble distortionCoefficientsMat =
                 new JsonMatOfDouble(1, 8, CvType.CV_64FC1, Arrays.copyOfRange(result.intrinsics, 4, 12));
 
-        // Calculate optimized board poses manually. We get this for free from mrcal too, but that's not
-        // JNIed (yet)
+        // Pull optimised camera to board poses out from the JNI
         List<Mat> rvecs = new ArrayList<>();
         List<Mat> tvecs = new ArrayList<>();
-        for (var o : in) {
-            var rvec = new Mat();
-            var tvec = new Mat();
-            Calib3d.solvePnP(
-                    o.objectPoints,
-                    o.imagePoints,
-                    cameraMatrixMat.getAsMat(),
-                    distortionCoefficientsMat.getAsMatOfDouble(),
-                    rvec,
-                    tvec);
+        for (var o : result.optimizedPoses) {
+            var rvec = new MatOfDouble();
+            var tvec = new MatOfDouble();
+
+            rvec.fromArray(o.getRotation().getAxis().times(o.getRotation().getAngle()).getData());
+            tvec.fromArray(
+                    o.getTranslation().getX(), o.getTranslation().getY(), o.getTranslation().getZ());
+
             rvecs.add(rvec);
             tvecs.add(tvec);
         }
@@ -230,6 +238,7 @@ protected CameraCalibrationCoefficients calibrateMrcal(
                         distortionCoefficientsMat.getAsMatOfDouble(),
                         rvecs,
                         tvecs,
+                        result.cornersUsed,
                         new double[] {result.warp_x, result.warp_y});
 
         rvecs.forEach(Mat::release);
@@ -252,18 +261,19 @@ private List<BoardObservation> createObservations(
             MatOfDouble distortionCoefficients_,
             List<Mat> rvecs,
             List<Mat> tvecs,
+            List<boolean[]> cornersUsed,
             double[] calobject_warp) {
         List<Mat> objPoints = in.stream().map(it -> it.objectPoints).collect(Collectors.toList());
         List<Mat> imgPts = in.stream().map(it -> it.imagePoints).collect(Collectors.toList());
 
         // For each observation, calc reprojection error
         Mat jac_temp = new Mat();
         List<BoardObservation> observations = new ArrayList<>();
-        for (int i = 0; i < objPoints.size(); i++) {
+        for (int snapshotId = 0; snapshotId < objPoints.size(); snapshotId++) {
             MatOfPoint3f i_objPtsNative = new MatOfPoint3f();
-            objPoints.get(i).copyTo(i_objPtsNative);
+            objPoints.get(snapshotId).copyTo(i_objPtsNative);
             var i_objPts = i_objPtsNative.toList();
-            var i_imgPts = ((MatOfPoint2f) imgPts.get(i)).toList();
+            var i_imgPts = ((MatOfPoint2f) imgPts.get(snapshotId)).toList();
 
             // Apply warp, if set
             if (calobject_warp != null && calobject_warp.length == 2) {
@@ -291,8 +301,8 @@ private List<BoardObservation> createObservations(
             try {
                 Calib3d.projectPoints(
                         i_objPtsNative,
-                        rvecs.get(i),
-                        tvecs.get(i),
+                        rvecs.get(snapshotId),
+                        tvecs.get(snapshotId),
                         cameraMatrix_,
                         distortionCoefficients_,
                         img_pts_reprojected,
@@ -313,16 +323,22 @@ private List<BoardObservation> createObservations(
                 reprojectionError.add(error);
             }
 
-            var camToBoard = MathUtils.opencvRTtoPose3d(rvecs.get(i), tvecs.get(i));
+            var camToBoard = MathUtils.opencvRTtoPose3d(rvecs.get(snapshotId), tvecs.get(snapshotId));
 
             JsonImageMat image = null;
-            var inputImage = in.get(i).inputImage;
+            var inputImage = in.get(snapshotId).inputImage;
             if (inputImage != null) {
                 image = new JsonImageMat(inputImage);
             }
             observations.add(
                     new BoardObservation(
-                            i_objPts, i_imgPts, reprojectionError, camToBoard, true, "img" + i + ".png", image));
+                            i_objPts,
+                            i_imgPts,
+                            reprojectionError,
+                            camToBoard,
+                            cornersUsed.get(snapshotId),
+                            "img" + snapshotId + ".png",
+                            image));
         }
         jac_temp.release();
 

photon-server/src/main/java/org/photonvision/server/RequestHandler.java

@@ -609,16 +609,16 @@ public static void onCalibrationSnapshotRequest(Context ctx) {
             return;
         }
 
+        var mat = calList.observations.get(observationIdx).getAnnotatedImage();
+
         // encode as jpeg to save even more space. reduces size of a 1280p image from 300k to 25k
         var jpegBytes = new MatOfByte();
-        Imgcodecs.imencode(
-                ".jpg",
-                calList.observations.get(observationIdx).snapshotData.getAsMat(),
-                jpegBytes,
-                new MatOfInt(Imgcodecs.IMWRITE_JPEG_QUALITY, 60));
+        Imgcodecs.imencode(".jpg", mat, jpegBytes, new MatOfInt(Imgcodecs.IMWRITE_JPEG_QUALITY, 60));
 
         ctx.result(jpegBytes.toArray());
+
         jpegBytes.release();
+        mat.release();
 
         ctx.status(200);
     }