Add Charuco Support. (#5)

PhotonVision · May 5, 2024 · 24c0320 · 24c0320
1 parent b903a09
commit 24c0320
Showing 1 changed file with 24 additions and 9 deletions.
diff --git a/src/main/java/org/photonvision/mrcal/MrCalJNI.java b/src/main/java/org/photonvision/mrcal/MrCalJNI.java
@@ -17,11 +17,10 @@
 
 package org.photonvision.mrcal;
 
-import java.io.IOException;
-import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.List;
 
+import org.opencv.core.MatOfFloat;
 import org.opencv.core.MatOfPoint2f;
 
 public class MrCalJNI {
@@ -37,6 +36,7 @@ public static class MrCalResult {
         public MrCalResult(boolean success) {
             this.success = success;
         }
+
         public MrCalResult(
                 boolean success, double[] intrinsics, double rms_error, double[] residuals, double warp_x,
                 double warp_y, int Noutliers) {
@@ -62,30 +62,45 @@ public static native MrCalResult mrcal_calibrate_camera(
             int boardWidth, int boardHeight, double boardSpacing,
             int imageWidth, int imageHeight, double focalLen);
 
-    public static native boolean undistort_mrcal(long srcMat, long dstMat, long cameraMat, long distCoeffsMat, int lensModelOrdinal, int order, int Nx, int Ny, int fov_x_deg);
+    public static native boolean undistort_mrcal(long srcMat, long dstMat, long cameraMat, long distCoeffsMat,
+            int lensModelOrdinal, int order, int Nx, int Ny, int fov_x_deg);
 
     public static MrCalResult calibrateCamera(
             List<MatOfPoint2f> board_corners,
+            List<MatOfFloat> board_corner_levels,
             int boardWidth, int boardHeight, double boardSpacing,
             int imageWidth, int imageHeight, double focalLen) {
         double[] observations = new double[boardWidth * boardHeight * 3 * board_corners.size()];
 
+        if (!(board_corners.size() == board_corner_levels.size() && board_corners.size() == boardWidth * boardHeight)) {
+            return new MrCalResult(false);
+        }
+
         int i = 0;
-        for (var board : board_corners) {
+        for (int b = 0; b < board_corners.size(); b++) {
+            var board = board_corners.get(b);
+            var levels = board_corner_levels.get(b).toArray();
             var corners = board.toArray();
+
+            if (!(corners.length == levels.length && corners.length == boardWidth * boardHeight)) {
+                return new MrCalResult(false);
+            }
+
             // Assume that we're correct in terms of row/column major-ness (lol)
-            for (var c : corners) {
-                float level = 1.0f; // if we have mrgingham, use level from that. Otherwise, hard-coded to 1
+            for (int c = 0; c < corners.length; c++) {
+
+                var corner = corners[c];
+                float level = levels[c];
 
-                observations[i * 3 + 0] = c.x;
-                observations[i * 3 + 1] = c.y;
+                observations[i * 3 + 0] = corner.x;
+                observations[i * 3 + 1] = corner.y;
                 observations[i * 3 + 2] = level;
 
                 i += 1;
             }
         }
 
-        if (i * 3 != observations.length) {
+        if (i * 3 != observations.length){
             return new MrCalResult(false);
         }