Pipe optimized board pose, outliers through JNI

build.gradle

@@ -35,6 +35,10 @@ dependencies {
     testImplementation("org.junit.jupiter:junit-jupiter-api:5.8.2")
     testImplementation("org.junit.jupiter:junit-jupiter-params:5.8.2")
     testRuntimeOnly("org.junit.jupiter:junit-jupiter-engine:5.8.2")
+
+    implementation wpilibTools.deps.wpilibJava("wpimath")
+    implementation wpilibTools.deps.wpilibJava("wpiunits")
+    implementation "com.fasterxml.jackson.core:jackson-annotations:2.15.2"
 }
 
 tasks.withType(JavaCompile) {

src/main/java/org/photonvision/mrcal/MrCalJNI.java

@@ -17,13 +17,17 @@
 
 package org.photonvision.mrcal;
 
-import java.io.IOException;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.List;
 
 import org.opencv.core.MatOfPoint2f;
 
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+
 public class MrCalJNI {
     public static class MrCalResult {
         public boolean success;
@@ -33,20 +37,44 @@ public static class MrCalResult {
         public double warp_x;
         public double warp_y;
         public int Noutliers;
+        public List<Pose3d> optimizedPoses;
+        public List<boolean[]> cornersUsed;
 
         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) {
+                boolean success, int width, int height, double[] intrinsics, double[] optimized_rt_rtoref,
+                double rms_error, double[] residuals, double warp_x,
+                double warp_y, int Noutliers, boolean[] cornerUseMask) {
             this.success = success;
             this.intrinsics = intrinsics;
             this.rms_error = rms_error;
             this.residuals = residuals;
             this.warp_x = warp_x;
             this.warp_y = warp_y;
             this.Noutliers = Noutliers;
+
+            optimizedPoses = new ArrayList<>();
+            for (int i = 0; i < optimized_rt_rtoref.length; i += 6) {
+                var rot = new Rotation3d(VecBuilder.fill(
+                        optimized_rt_rtoref[i + 0],
+                        optimized_rt_rtoref[i + 1],
+                        optimized_rt_rtoref[i + 2]));
+                var t = new Translation3d(
+                        optimized_rt_rtoref[i + 3],
+                        optimized_rt_rtoref[i + 4],
+                        optimized_rt_rtoref[i + 5]);
+
+                optimizedPoses.add(new Pose3d(t, rot));
+            }
+
+            var cornersPerBoard = width * height;
+            cornersUsed = new ArrayList<>();
+            for (int cornerIdx = 0; cornerIdx < cornerUseMask.length; cornerIdx += cornersPerBoard) {
+                cornersUsed.add(Arrays.copyOfRange(cornerUseMask, cornerIdx, cornerIdx + cornersPerBoard));
+            }
         }
 
         @Override
@@ -62,7 +90,8 @@ 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,
@@ -89,6 +118,7 @@ public static MrCalResult calibrateCamera(
             return new MrCalResult(false);
         }
 
-        return mrcal_calibrate_camera(observations, boardWidth, boardHeight, boardSpacing, imageWidth, imageHeight, focalLen);
+        return mrcal_calibrate_camera(observations, boardWidth, boardHeight, boardSpacing, imageWidth, imageHeight,
+                focalLen);
     }
 }

src/mrcal_jni.cpp

@@ -20,6 +20,7 @@
 #include <algorithm>
 #include <cstdio>
 #include <span>
+#include <sstream>
 #include <stdexcept>
 #include <vector>
 
@@ -45,6 +46,22 @@ WPI_JNI_MAKEJARRAY(jdouble, Double)
 
 #undef WPI_JNI_MAKEJARRAY
 
+#define JNI_BOOL "Z"
+#define JNI_VOID "V"
+#define JNI_INT "I"
+#define JNI_DOUBLE "D"
+#define JNI_DOUBLEARR "[D"
+#define JNI_BOOLARR "[Z"
+
+#define JNI_STRINGIFY(x) #x
+
+template <typename A, typename... Ts>
+std::string jni_make_method_sig(A retval, Ts &&...args) {
+  std::ostringstream oss;
+  (oss << "(" << ... << std::forward<Ts>(args)) << ")" << retval;
+  return oss.str();
+}
+
 /**
  * Finds a class and keeps it as a global reference.
  *
@@ -80,7 +97,8 @@ class JClass {
 };
 
 // Cache MrCalResult class
-JClass detectionClass;
+static JClass detectionClass;
+static jmethodID constructor;
 
 extern "C" {
 JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM *vm, void *reserved) {
@@ -96,6 +114,15 @@ JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM *vm, void *reserved) {
     return JNI_ERR;
   }
 
+  // Find the constructor. Reference:
+  // https://www.microfocus.com/documentation/extend-acucobol/925/BKITITJAVAS027.html
+  constructor = env->GetMethodID(
+      detectionClass, "<init>",
+      jni_make_method_sig(JNI_VOID, JNI_BOOL, JNI_INT, JNI_INT, JNI_DOUBLEARR,
+                          JNI_DOUBLEARR, JNI_DOUBLE, JNI_DOUBLEARR, JNI_DOUBLE,
+                          JNI_DOUBLE, JNI_INT, JNI_BOOLARR)
+          .c_str());
+
   return JNI_VERSION_1_6;
 }
 
@@ -171,10 +198,6 @@ Java_org_photonvision_mrcal_MrCalJNI_mrcal_1calibrate_1camera
   }
   mrcal_result &stats = *statsptr;
 
-  // Find the constructor. Reference:
-  // https://www.microfocus.com/documentation/extend-acucobol/925/BKITITJAVAS027.html
-  static jmethodID constructor =
-      env->GetMethodID(detectionClass, "<init>", "(Z[DD[DDDI)V");
   if (!constructor) {
     return nullptr;
   }
@@ -192,9 +215,21 @@ Java_org_photonvision_mrcal_MrCalJNI_mrcal_1calibrate_1camera
   jdouble warp_y = stats.calobject_warp.y2;
   jint Noutliers = stats.Noutliers_board;
 
-  // Actually call the constructor (TODO)
-  auto ret = env->NewObject(detectionClass, constructor, success, intrinsics,
-                            rms_err, residuals, warp_x, warp_y, Noutliers);
+  jdoubleArray optimized_rt_toref = MakeJDoubleArray(
+      env, reinterpret_cast<double *>(total_frames_rt_toref.data()),
+      total_frames_rt_toref.size() * sizeof(mrcal_pose_t) / sizeof(double));
+
+  std::vector<jboolean> cornersUsedMask(observations.size());
+  std::transform(observations.begin(), observations.end(),
+                 cornersUsedMask.begin(),
+                 [](const auto &pt) { return (jboolean)(pt.z > 0); });
+  auto cornersUsedJarr = MakeJBooleanArray(env, cornersUsedMask.data(), cornersUsedMask.size());
+
+  // Actually call the constructor
+  auto ret =
+      env->NewObject(detectionClass, constructor, success, boardWidth,
+                     boardHeight, intrinsics, optimized_rt_toref, rms_err,
+                     residuals, warp_x, warp_y, Noutliers, cornersUsedJarr);
 
   return ret;
 }

src/mrcal_wrapper.cpp

@@ -348,11 +348,6 @@ mrcal_pose_t getSeedPose(const mrcal_point3_t *c_observations_board_pool,
   for (auto a : objectPoints)
     objectPoints3.push_back(Point3f(a.x, a.y, 0));
 
-  // for (auto& o : objectPoints) std::cout << o << std::endl;
-  // for (auto& i : imagePoints) std::cout << i << std::endl;
-  // std::cout << "cam mat\n" << cameraMatrix << std::endl;
-  // std::cout << "distortion: " << distCoeffs << std::endl;
-
   solvePnP(objectPoints3, imagePoints, cameraMatrix, distCoeffs, rvec, tvec,
            false, SOLVEPNP_ITERATIVE);