diff --git a/build.gradle b/build.gradle index 3560db4..76c9bc3 100644 --- a/build.gradle +++ b/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) { diff --git a/src/main/java/org/photonvision/mrcal/MrCalJNI.java b/src/main/java/org/photonvision/mrcal/MrCalJNI.java index cb3659c..e9f3eaa 100644 --- a/src/main/java/org/photonvision/mrcal/MrCalJNI.java +++ b/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,13 +37,17 @@ public static class MrCalResult { public double warp_x; public double warp_y; public int Noutliers; + public List optimizedPoses; + public List 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; @@ -47,6 +55,26 @@ public MrCalResult( 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 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); } } diff --git a/src/mrcal_jni.cpp b/src/mrcal_jni.cpp index 43e19e5..2e281ea 100644 --- a/src/mrcal_jni.cpp +++ b/src/mrcal_jni.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -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 +std::string jni_make_method_sig(A retval, Ts &&...args) { + std::ostringstream oss; + (oss << "(" << ... << std::forward(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, "", + 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, "", "(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(total_frames_rt_toref.data()), + total_frames_rt_toref.size() * sizeof(mrcal_pose_t) / sizeof(double)); + + std::vector 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; } diff --git a/src/mrcal_wrapper.cpp b/src/mrcal_wrapper.cpp index 5badd83..2d88d09 100644 --- a/src/mrcal_wrapper.cpp +++ b/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);