diff --git a/src/mrcal_jni.cpp b/src/mrcal_jni.cpp index b8ecfb5..6ed40e6 100644 --- a/src/mrcal_jni.cpp +++ b/src/mrcal_jni.cpp @@ -19,12 +19,11 @@ #include #include +#include #include #include #include -#include - #include "mrcal_wrapper.h" // JClass helper from wpilib @@ -103,15 +102,23 @@ JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM *vm, void *reserved) { } // extern "C" -static std::string what(const std::exception_ptr &eptr = std::current_exception()) -{ - if (!eptr) { throw std::bad_exception(); } +static std::string +what(const std::exception_ptr &eptr = std::current_exception()) { + if (!eptr) { + throw std::bad_exception(); + } - try { std::rethrow_exception(eptr); } - catch (const std::exception &e) { return e.what() ; } - catch (const std::string &e) { return e ; } - catch (const char *e) { return e ; } - catch (...) { return "who knows"; } + try { + std::rethrow_exception(eptr); + } catch (const std::exception &e) { + return e.what(); + } catch (const std::string &e) { + return e; + } catch (const char *e) { + return e; + } catch (...) { + return "who knows"; + } } /* @@ -126,99 +133,96 @@ Java_org_photonvision_mrcal_MrCalJNI_mrcal_1calibrate_1camera jdouble focalLenGuessMM) { try { - - - // Pull out arrays. We rely on data being packed and aligned to make this - // work! Observations should be [x, y, level] - std::span observations{ - reinterpret_cast( - env->GetDoubleArrayElements(observations_board, 0)), - env->GetArrayLength(observations_board) / 3lu}; - - size_t points_in_board = boardWidth * boardHeight; - if (observations.size() % points_in_board != 0) { - jclass exception_class = env->FindClass("java/lang/Exception"); - if (exception_class && env) { - (env)->ExceptionClear(); - env->ThrowNew(exception_class, - "Observation list length does not match board size!"); - return {}; - } else { - // ???? - std::cerr << "Observation list length does not match board size!\n"; + // Pull out arrays. We rely on data being packed and aligned to make this + // work! Observations should be [x, y, level] + std::span observations{ + reinterpret_cast( + env->GetDoubleArrayElements(observations_board, 0)), + env->GetArrayLength(observations_board) / 3lu}; + + size_t points_in_board = boardWidth * boardHeight; + if (observations.size() % points_in_board != 0) { + jclass exception_class = env->FindClass("java/lang/Exception"); + if (exception_class && env) { + (env)->ExceptionClear(); + env->ThrowNew(exception_class, + "Observation list length does not match board size!"); + return {}; + } else { + // ???? + std::cerr << "Observation list length does not match board size!\n"; + } } - } - - size_t boards_observed = observations.size() / points_in_board; - const auto boardSize = cv::Size{boardWidth, boardHeight}; - const auto imagerSize = cv::Size{imageWidth, imageHeight}; + size_t boards_observed = observations.size() / points_in_board; - // down big list of observations/extrinsic guesses (one per board object) - std::vector total_frames_rt_toref; + const auto boardSize = cv::Size{boardWidth, boardHeight}; + const auto imagerSize = cv::Size{imageWidth, imageHeight}; - for (size_t i = 0; i < boards_observed; i++) { - auto seed_pose = - getSeedPose(&(*observations.begin()) + (i * points_in_board), boardSize, - imagerSize, boardSpacing, focalLenGuessMM); - // std::printf("Seed pose %lu: r %f %f %f t %f %f %f\n", i, seed_pose.r.x, - // seed_pose.r.y, seed_pose.r.z, seed_pose.t.x, seed_pose.t.y, - // seed_pose.t.z); + // down big list of observations/extrinsic guesses (one per board object) + std::vector total_frames_rt_toref; - // Add to seed poses - total_frames_rt_toref.push_back(seed_pose); - } + for (size_t i = 0; i < boards_observed; i++) { + auto seed_pose = + getSeedPose(&(*observations.begin()) + (i * points_in_board), + boardSize, imagerSize, boardSpacing, focalLenGuessMM); + // std::printf("Seed pose %lu: r %f %f %f t %f %f %f\n", i, seed_pose.r.x, + // seed_pose.r.y, seed_pose.r.z, seed_pose.t.x, seed_pose.t.y, + // seed_pose.t.z); - // Convert detection level to weights - for (auto &o : observations) { - double &level = o.z; - if (level < 0) { - o.z = -1; - } else { - o.z = std::pow(0.5, level); + // Add to seed poses + total_frames_rt_toref.push_back(seed_pose); } - } - auto statsptr = mrcal_main(observations, total_frames_rt_toref, boardSize, - static_cast(boardSpacing), imagerSize, - focalLenGuessMM); - if (!statsptr) { - return nullptr; - } - 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; - } + // Convert detection level to weights + for (auto &o : observations) { + double &level = o.z; + if (level < 0) { + o.z = -1; + } else { + o.z = std::pow(0.5, level); + } + } - size_t Nintrinsics = stats.intrinsics.size(); - size_t Nresid = stats.residuals.size(); + auto statsptr = mrcal_main(observations, total_frames_rt_toref, boardSize, + static_cast(boardSpacing), imagerSize, + focalLenGuessMM); + if (!statsptr) { + return nullptr; + } + 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; + } - jdoubleArray intrinsics = - MakeJDoubleArray(env, stats.intrinsics.data(), Nintrinsics); - jdoubleArray residuals = - MakeJDoubleArray(env, stats.residuals.data(), Nresid); - jboolean success = stats.success; - jdouble rms_err = stats.rms_error; - jdouble warp_x = stats.calobject_warp.x2; - jdouble warp_y = stats.calobject_warp.y2; - jint Noutliers = stats.Noutliers_board; + size_t Nintrinsics = stats.intrinsics.size(); + size_t Nresid = stats.residuals.size(); - // Actually call the constructor (TODO) - auto ret = env->NewObject(detectionClass, constructor, success, intrinsics, - rms_err, residuals, warp_x, warp_y, Noutliers); + jdoubleArray intrinsics = + MakeJDoubleArray(env, stats.intrinsics.data(), Nintrinsics); + jdoubleArray residuals = + MakeJDoubleArray(env, stats.residuals.data(), Nresid); + jboolean success = stats.success; + jdouble rms_err = stats.rms_error; + jdouble warp_x = stats.calobject_warp.x2; + jdouble warp_y = stats.calobject_warp.y2; + jint Noutliers = stats.Noutliers_board; - return ret; + // Actually call the constructor (TODO) + auto ret = env->NewObject(detectionClass, constructor, success, intrinsics, + rms_err, residuals, warp_x, warp_y, Noutliers); + return ret; } catch (...) { std::cerr << "Calibration exception: " << what() << std::endl; static char buff[512]; - strcpy(buff, what().c_str()); + std::strcpy(buff, what().c_str()); env->ThrowNew(env->FindClass("java/lang/Exception"), buff); } } diff --git a/src/mrcal_wrapper.cpp b/src/mrcal_wrapper.cpp index 5badd83..8970741 100644 --- a/src/mrcal_wrapper.cpp +++ b/src/mrcal_wrapper.cpp @@ -334,9 +334,8 @@ mrcal_pose_t getSeedPose(const mrcal_point3_t *c_observations_board_pool, &model, intrinsics); std::transform(mrcal_imagepts.begin(), mrcal_imagepts.end(), - imagePoints.begin(), [](const auto &pt) { - return Point2d{pt.x, pt.y}; - }); + imagePoints.begin(), + [](const auto &pt) { return Point2d{pt.x, pt.y}; }); } // Initial guess at intrinsics