From 5e74ff26d83c969c2e2bfb6d0ef3831ed3e9868d Mon Sep 17 00:00:00 2001 From: Thad House Date: Wed, 30 Nov 2022 00:16:29 -0800 Subject: [PATCH] [apriltag, build] Update native utils, add apriltag impl and JNI (#4733) Co-authored-by: Peter Johnson --- apriltag/CMakeLists.txt | 105 +++++- apriltag/apriltag-config.cmake.in | 7 + apriltag/build.gradle | 20 +- .../wpi/first/apriltag/jni/AprilTagJNI.java | 89 +++++ .../first/apriltag/jni/DetectionResult.java | 224 ++++++++++++ .../src/main/native/cpp/jni/AprilTagJNI.cpp | 320 ++++++++++++++++++ buildSrc/build.gradle | 2 +- cscore/build.gradle | 4 +- datalogtool/build.gradle | 3 +- glass/build.gradle | 8 +- outlineviewer/build.gradle | 4 +- roborioteamnumbersetter/build.gradle | 3 +- shared/apriltaglib.gradle | 13 + shared/config.gradle | 5 +- shared/imgui.gradle | 13 + simulation/halsim_gui/build.gradle | 5 +- wpigui/build.gradle | 5 +- 17 files changed, 794 insertions(+), 36 deletions(-) create mode 100644 apriltag/apriltag-config.cmake.in create mode 100644 apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java create mode 100644 apriltag/src/main/java/edu/wpi/first/apriltag/jni/DetectionResult.java create mode 100644 apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp create mode 100644 shared/apriltaglib.gradle create mode 100644 shared/imgui.gradle diff --git a/apriltag/CMakeLists.txt b/apriltag/CMakeLists.txt index cfd9615013..d3b642c853 100644 --- a/apriltag/CMakeLists.txt +++ b/apriltag/CMakeLists.txt @@ -2,37 +2,114 @@ project(apriltag) include(CompileWarnings) include(GenResources) +include(FetchContent) -if (WITH_JAVA) - find_package(Java REQUIRED) - include(UseJava) +FetchContent_Declare( + apriltaglib + GIT_REPOSITORY https://github.com/photonvision/apriltag.git + GIT_TAG 404435205b62c3981569dc837e0051d829181ab3 +) - file(GLOB JACKSON_JARS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar") - - set(CMAKE_JAVA_INCLUDE_PATH apriltag.jar ${JACKSON_JARS}) - - file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java) - file(GLOB_RECURSE JAVA_RESOURCES src/main/native/resources/*.json) - add_jar(apriltag_jar SOURCES ${JAVA_SOURCES} RESOURCES NAMESPACE "edu/wpi/first/apriltag" ${JAVA_RESOURCES} INCLUDE_JARS wpimath_jar OUTPUT_NAME apriltag) +# Don't use apriltag's CMakeLists.txt due to conflicting naming and JNI +FetchContent_GetProperties(apriltaglib) +if(NOT apriltaglib_POPULATED) + FetchContent_Populate(apriltaglib) endif() +aux_source_directory(${apriltaglib_SOURCE_DIR}/common APRILTAGLIB_COMMON_SRC) +file(GLOB TAG_FILES ${apriltaglib_SOURCE_DIR}/tag*.c) +set(APRILTAGLIB_SRCS ${apriltaglib_SOURCE_DIR}/apriltag.c ${apriltaglib_SOURCE_DIR}/apriltag_pose.c ${apriltaglib_SOURCE_DIR}/apriltag_quad_thresh.c) + +file(GLOB apriltag_jni_src src/main/native/cpp/jni/AprilTagJNI.cpp) + +if (WITH_JAVA) + find_package(Java REQUIRED) + find_package(JNI REQUIRED) + include(UseJava) + set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked") + + set(CMAKE_JNI_TARGET true) + + file(GLOB EJML_JARS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/*.jar") + file(GLOB JACKSON_JARS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar") + find_file(OPENCV_JAR_FILE + NAMES opencv-${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.jar + PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin ${OpenCV_INSTALL_PATH}/share/java + NO_DEFAULT_PATH) + + set(CMAKE_JAVA_INCLUDE_PATH apriltag.jar ${EJML_JARS} ${JACKSON_JARS}) + + file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java) + file(GLOB_RECURSE JAVA_RESOURCES src/main/native/resources/*.json) + add_jar(apriltag_jar + SOURCES ${JAVA_SOURCES} + RESOURCES NAMESPACE "edu/wpi/first/apriltag" ${JAVA_RESOURCES} + INCLUDE_JARS wpimath_jar ${EJML_JARS} wpiutil_jar ${OPENCV_JAR_FILE} + OUTPUT_NAME apriltag + GENERATE_NATIVE_HEADERS apriltag_jni_headers) + + get_property(APRILTAG_JAR_FILE TARGET apriltag_jar PROPERTY JAR_FILE) + install(FILES ${APRILTAG_JAR_FILE} DESTINATION "${java_lib_dest}") + + set_property(TARGET apriltag_jar PROPERTY FOLDER "java") + + add_library(apriltagjni ${apriltag_jni_src}) + wpilib_target_warnings(apriltagjni) + target_link_libraries(apriltagjni PUBLIC apriltag) + + set_property(TARGET apriltagjni PROPERTY FOLDER "libraries") + + target_link_libraries(apriltagjni PRIVATE apriltag_jni_headers) + add_dependencies(apriltagjni apriltag_jar) + + if (MSVC) + install(TARGETS apriltagjni RUNTIME DESTINATION "${jni_lib_dest}" COMPONENT Runtime) + endif() + + install(TARGETS apriltagjni EXPORT apriltagjni DESTINATION "${main_lib_dest}") + +endif() GENERATE_RESOURCES(src/main/native/resources/edu/wpi/first/apriltag generated/main/cpp APRILTAG frc apriltag_resources_src) -file(GLOB_RECURSE apriltag_native_src src/main/native/cpp/*.cpp) +file(GLOB apriltag_native_src src/main/native/cpp/*.cpp) -add_library(apriltag ${apriltag_native_src} ${apriltag_resources_src}) +add_library(apriltag ${apriltag_native_src} ${apriltag_resources_src} ${APRILTAGLIB_SRCS} ${APRILTAGLIB_COMMON_SRC} ${TAG_FILES}) set_target_properties(apriltag PROPERTIES DEBUG_POSTFIX "d") set_property(TARGET apriltag PROPERTY FOLDER "libraries") target_compile_features(apriltag PUBLIC cxx_std_20) wpilib_target_warnings(apriltag) +# disable warnings that apriltaglib can't handle +if (MSVC) + target_compile_options(apriltag PRIVATE /wd4005 /wd4018 /wd4098 /wd4200 /wd4244 /wd4996) +else() + target_compile_options(apriltag PRIVATE -Wno-newline-eof -Wno-unused-variable -Wno-sign-compare -Wno-pedantic -Wno-shift-negative-value) +endif() + target_link_libraries(apriltag wpimath) target_include_directories(apriltag PUBLIC - $ - $) + $ + $ + $) +install(TARGETS apriltag EXPORT apriltag DESTINATION "${main_lib_dest}") +install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/apriltag") + +if (WITH_JAVA AND MSVC) + install(TARGETS apriltag RUNTIME DESTINATION "${jni_lib_dest}" COMPONENT Runtime) +endif() + +if (WITH_FLAT_INSTALL) + set (apriltag_config_dir ${wpilib_dest}) +else() + set (apriltag_config_dir share/apriltag) +endif() + +configure_file(apriltag-config.cmake.in ${WPILIB_BINARY_DIR}/apriltag-config.cmake ) +install(FILES ${WPILIB_BINARY_DIR}/apriltag-config.cmake DESTINATION ${apriltag_config_dir}) +install(EXPORT apriltag DESTINATION ${apriltag_config_dir}) if (WITH_TESTS) wpilib_add_test(apriltag src/test/native/cpp) diff --git a/apriltag/apriltag-config.cmake.in b/apriltag/apriltag-config.cmake.in new file mode 100644 index 0000000000..7477a0eff6 --- /dev/null +++ b/apriltag/apriltag-config.cmake.in @@ -0,0 +1,7 @@ +include(CMakeFindDependencyMacro) +@FILENAME_DEP_REPLACE@ +@WPIMATH_DEP_REPLACE@ +@WPIUTIL_DEP_REPLACE@ + +@FILENAME_DEP_REPLACE@ +include(${SELF_DIR}/apriltag.cmake) diff --git a/apriltag/build.gradle b/apriltag/build.gradle index d10366b75e..c0c1c07478 100644 --- a/apriltag/build.gradle +++ b/apriltag/build.gradle @@ -3,13 +3,14 @@ apply from: "${rootDir}/shared/resources.gradle" ext { nativeName = 'apriltag' devMain = 'edu.wpi.first.apriltag.DevMain' + useJava = true def generateTask = createGenerateResourcesTask('main', 'APRILTAG', 'frc', project) tasks.withType(CppCompile) { dependsOn generateTask } - extraSetup = { + splitSetup = { it.sources { resourcesCpp(CppSourceSet) { source { @@ -23,7 +24,9 @@ ext { evaluationDependsOn(':wpimath') -apply from: "${rootDir}/shared/javacpp/setupBuild.gradle" +apply from: "${rootDir}/shared/jni/setupBuild.gradle" +apply from: "${rootDir}/shared/apriltaglib.gradle" +apply from: "${rootDir}/shared/opencv.gradle" dependencies { implementation project(':wpimath') @@ -47,12 +50,15 @@ model { } it.cppCompiler.define 'WPILIB_EXPORTS' - lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' - lib project: ':wpimath', library: 'wpimath', linkage: 'shared' - - if ((it instanceof NativeExecutableBinarySpec || it instanceof GoogleTestTestSuiteBinarySpec) && it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) { - nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries') + if (it.component.name == "${nativeName}JNI") { + lib project: ':wpimath', library: 'wpimath', linkage: 'static' + lib project: ':wpiutil', library: 'wpiutil', linkage: 'static' + } else { + lib project: ':wpimath', library: 'wpimath', linkage: 'shared' + lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' } + + nativeUtils.useRequiredLibrary(it, 'apriltaglib') } } tasks { diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java b/apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java new file mode 100644 index 0000000000..eb67ad47cf --- /dev/null +++ b/apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java @@ -0,0 +1,89 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.apriltag.jni; + +import edu.wpi.first.util.RuntimeLoader; +import java.io.IOException; +import java.util.concurrent.atomic.AtomicBoolean; +import org.opencv.core.Mat; + +public class AprilTagJNI { + static boolean libraryLoaded = false; + + static RuntimeLoader loader = null; + + public static class Helper { + private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true); + + public static boolean getExtractOnStaticLoad() { + return extractOnStaticLoad.get(); + } + + public static void setExtractOnStaticLoad(boolean load) { + extractOnStaticLoad.set(load); + } + } + + static { + if (Helper.getExtractOnStaticLoad()) { + try { + loader = + new RuntimeLoader<>( + "apriltagjni", RuntimeLoader.getDefaultExtractionRoot(), AprilTagJNI.class); + loader.loadLibrary(); + } catch (IOException ex) { + ex.printStackTrace(); + System.exit(1); + } + libraryLoaded = true; + } + } + + // Returns a pointer to a apriltag_detector_t + public static native long aprilTagCreate( + String fam, double decimate, double blur, int threads, boolean debug, boolean refine_edges); + + // Destroy and free a previously created detector. + public static native void aprilTagDestroy(long detector); + + private static native Object[] aprilTagDetectInternal( + long detector, + long imgAddr, + int rows, + int cols, + boolean doPoseEstimation, + double tagWidth, + double fx, + double fy, + double cx, + double cy, + int nIters); + + // Detect targets given a GRAY frame. Returns a pointer toa zarray + public static DetectionResult[] aprilTagDetect( + long detector, + Mat img, + boolean doPoseEstimation, + double tagWidth, + double fx, + double fy, + double cx, + double cy, + int nIters) { + return (DetectionResult[]) + aprilTagDetectInternal( + detector, + img.dataAddr(), + img.rows(), + img.cols(), + doPoseEstimation, + tagWidth, + fx, + fy, + cx, + cy, + nIters); + } +} diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/jni/DetectionResult.java b/apriltag/src/main/java/edu/wpi/first/apriltag/jni/DetectionResult.java new file mode 100644 index 0000000000..219a167487 --- /dev/null +++ b/apriltag/src/main/java/edu/wpi/first/apriltag/jni/DetectionResult.java @@ -0,0 +1,224 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.apriltag.jni; + +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N3; +import java.util.Arrays; +import org.ejml.data.DMatrixRMaj; +import org.ejml.dense.row.factory.DecompositionFactory_DDRM; +import org.ejml.simple.SimpleMatrix; + +public class DetectionResult { + public int getId() { + return m_id; + } + + public int getHamming() { + return m_hamming; + } + + public float getDecisionMargin() { + return m_decisionMargin; + } + + public void setDecisionMargin(float decisionMargin) { + this.m_decisionMargin = decisionMargin; + } + + @SuppressWarnings("PMD.MethodReturnsInternalArray") + public double[] getHomography() { + return m_homography; + } + + @SuppressWarnings("PMD.ArrayIsStoredDirectly") + public void setHomography(double[] homography) { + this.m_homography = homography; + } + + public double getCenterX() { + return m_centerX; + } + + public void setCenterX(double centerX) { + this.m_centerX = centerX; + } + + public double getCenterY() { + return m_centerY; + } + + public void setCenterY(double centerY) { + this.m_centerY = centerY; + } + + @SuppressWarnings("PMD.MethodReturnsInternalArray") + public double[] getCorners() { + return m_corners; + } + + @SuppressWarnings("PMD.ArrayIsStoredDirectly") + public void setCorners(double[] corners) { + this.m_corners = corners; + } + + public double getError1() { + return m_error1; + } + + public double getError2() { + return m_error2; + } + + public Transform3d getPoseResult1() { + return m_poseResult1; + } + + public Transform3d getPoseResult2() { + return m_poseResult2; + } + + private final int m_id; + private final int m_hamming; + private float m_decisionMargin; + private double[] m_homography; + private double m_centerX; + private double m_centerY; + private double[] m_corners; + + private final Transform3d m_poseResult1; + private final double m_error1; + private final Transform3d m_poseResult2; + private final double m_error2; + + /** + * Constructs a new detection result. Used from JNI. + * + * @param id id + * @param hamming hamming + * @param decisionMargin dm + * @param homography homography + * @param centerX centerX + * @param centerY centerY + * @param corners corners + * @param pose1TransArr pose1TransArr + * @param pose1RotArr pose1RotArr + * @param err1 err1 + * @param pose2TransArr pose2TransArr + * @param pose2RotArr pose2RotArr + * @param err2 err2 + */ + @SuppressWarnings("PMD.ArrayIsStoredDirectly") + public DetectionResult( + int id, + int hamming, + float decisionMargin, + double[] homography, + double centerX, + double centerY, + double[] corners, + double[] pose1TransArr, + double[] pose1RotArr, + double err1, + double[] pose2TransArr, + double[] pose2RotArr, + double err2) { + this.m_id = id; + this.m_hamming = hamming; + this.m_decisionMargin = decisionMargin; + this.m_homography = homography; + this.m_centerX = centerX; + this.m_centerY = centerY; + this.m_corners = corners; + + this.m_error1 = err1; + this.m_poseResult1 = + new Transform3d( + new Translation3d(pose1TransArr[0], pose1TransArr[1], pose1TransArr[2]), + new Rotation3d( + orthogonalizeRotationMatrix( + new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose1RotArr)))); + this.m_error2 = err2; + this.m_poseResult2 = + new Transform3d( + new Translation3d(pose2TransArr[0], pose2TransArr[1], pose2TransArr[2]), + new Rotation3d( + orthogonalizeRotationMatrix( + new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose2RotArr)))); + } + + /** + * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be + * ambiguous. + */ + public double getPoseAmbiguity() { + var min = Math.min(m_error1, m_error2); + var max = Math.max(m_error1, m_error2); + + if (max > 0) { + return min / max; + } else { + return -1; + } + } + + @Override + public String toString() { + return "DetectionResult [centerX=" + + m_centerX + + ", centerY=" + + m_centerY + + ", corners=" + + Arrays.toString(m_corners) + + ", decisionMargin=" + + m_decisionMargin + + ", error1=" + + m_error1 + + ", error2=" + + m_error2 + + ", hamming=" + + m_hamming + + ", homography=" + + Arrays.toString(m_homography) + + ", id=" + + m_id + + ", poseResult1=" + + m_poseResult1 + + ", poseResult2=" + + m_poseResult2 + + "]"; + } + + private static Matrix orthogonalizeRotationMatrix(Matrix input) { + var a = DecompositionFactory_DDRM.qr(3, 3); + if (!a.decompose(input.getStorage().getDDRM())) { + // best we can do is return the input + return input; + } + + // Grab results (thanks for this _great_ api, EJML) + var Q = new DMatrixRMaj(3, 3); + var R = new DMatrixRMaj(3, 3); + a.getQ(Q, false); + a.getR(R, false); + + // Fix signs in R if they're < 0 so it's close to an identity matrix + // (our QR decomposition implementation sometimes flips the signs of columns) + for (int colR = 0; colR < 3; ++colR) { + if (R.get(colR, colR) < 0) { + for (int rowQ = 0; rowQ < 3; ++rowQ) { + Q.set(rowQ, colR, -Q.get(rowQ, colR)); + } + } + } + + return new Matrix<>(new SimpleMatrix(Q)); + } +} diff --git a/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp b/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp new file mode 100644 index 0000000000..9e270e3a0c --- /dev/null +++ b/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp @@ -0,0 +1,320 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include "edu_wpi_first_apriltag_jni_AprilTagJNI.h" + +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable : 4200) +#endif + +#if defined(__GNUC__) +#pragma GCC diagnostic ignored "-Wpedantic" +#endif +#include "apriltag.h" +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#include "tag36h11.h" +#include "tag25h9.h" +#include "tag16h5.h" +#include "tagCircle21h7.h" +#include "tagCircle49h12.h" +#include "tagCustom48h12.h" +#include "tagStandard41h12.h" +#include "tagStandard52h13.h" +#include "apriltag_pose.h" + +#include +#include +#include + +using namespace wpi::java; + +struct DetectorState { + int id; + apriltag_detector_t* td; + apriltag_family_t* tf; + void (*tf_destroy)(apriltag_family_t*); +}; + +static std::vector detectors; + +extern "C" { + +/* + * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Method: aprilTagCreate + * Signature: (Ljava/lang/String;DDIZZ)J + */ +JNIEXPORT jlong JNICALL +Java_edu_wpi_first_apriltag_jni_AprilTagJNI_aprilTagCreate + (JNIEnv* env, jclass cls, jstring jstr, jdouble decimate, jdouble blur, + jint threads, jboolean debug, jboolean refine_edges) +{ + // Initialize tag detector with options + apriltag_family_t* tf = nullptr; + // const char *famname = fam; + const char* famname = env->GetStringUTFChars(jstr, nullptr); + + void (*tf_destroy_func)(apriltag_family_t*); + + if (!strcmp(famname, "tag36h11")) { + tf = tag36h11_create(); + tf_destroy_func = tag36h11_destroy; + } else if (!strcmp(famname, "tag25h9")) { + tf = tag25h9_create(); + tf_destroy_func = tag25h9_destroy; + } else if (!strcmp(famname, "tag16h5")) { + tf = tag16h5_create(); + tf_destroy_func = tag16h5_destroy; + } else if (!strcmp(famname, "tagCircle21h7")) { + tf = tagCircle21h7_create(); + tf_destroy_func = tagCircle21h7_destroy; + } else if (!strcmp(famname, "tagCircle49h12")) { + tf = tagCircle49h12_create(); + tf_destroy_func = tagCircle49h12_destroy; + } else if (!strcmp(famname, "tagStandard41h12")) { + tf = tagStandard41h12_create(); + tf_destroy_func = tagStandard41h12_destroy; + } else if (!strcmp(famname, "tagStandard52h13")) { + tf = tagStandard52h13_create(); + tf_destroy_func = tagStandard52h13_destroy; + } else if (!strcmp(famname, "tagCustom48h12")) { + tf = tagCustom48h12_create(); + tf_destroy_func = tagCustom48h12_destroy; + } else { + std::printf("Unrecognized tag family name. Use e.g. \"tag36h11\".\n"); + env->ReleaseStringUTFChars(jstr, famname); + return 0; + } + + apriltag_detector_t* td = apriltag_detector_create(); + apriltag_detector_add_family(td, tf); + td->quad_decimate = static_cast(decimate); + td->quad_sigma = static_cast(blur); + td->nthreads = threads; + td->debug = debug; + td->refine_edges = refine_edges; + + env->ReleaseStringUTFChars(jstr, famname); + + // std::printf("Looking for max\n"); + auto max = std::max_element(detectors.begin(), detectors.end(), + [](DetectorState& a, DetectorState& b) { + return a.id < b.id; + }); // detectors.size(); + int index = 0; + if (max != detectors.end()) + index = max->id + 1; + detectors.push_back({index, td, tf, tf_destroy_func}); + std::printf("Created detector at idx %i\n", index); + return (jlong)index; +} + +static JClass detectionClass; + +JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) { + JNIEnv* env; + if (vm->GetEnv(reinterpret_cast(&env), JNI_VERSION_1_6) != JNI_OK) { + return JNI_ERR; + } + + detectionClass = JClass(env, "edu/wpi/first/apriltag/jni/DetectionResult"); + + if (!detectionClass) { + std::printf("Couldn't find class!"); + return JNI_ERR; + } + + return JNI_VERSION_1_6; +} + +static jobject MakeJObject(JNIEnv* env, const apriltag_detection_t* detect, + apriltag_pose_t& pose1, apriltag_pose_t& pose2, + double error1, double error2) { + // Constructor signature must match Java! I = int, F = float, [D = double + // array + static jmethodID constructor = + env->GetMethodID(detectionClass, "", "(IIF[DDD[D[D[DD[D[DD)V"); + + if (!constructor) { + return nullptr; + } + + if (!detect) { + return nullptr; + } + + // We have to copy the homography matrix and coners into jdoubles + jdouble h[9]; // = new jdouble[9]{}; + for (int i = 0; i < 9; i++) { + h[i] = detect->H->data[i]; + } + + jdouble corners[8]; // = new jdouble[8]{}; + for (int i = 0; i < 4; i++) { + corners[i * 2] = detect->p[i][0]; + corners[i * 2 + 1] = detect->p[i][1]; + } + + jdoubleArray harr = MakeJDoubleArray(env, {h, 9}); + jdoubleArray carr = MakeJDoubleArray(env, {corners, 8}); + + // The rotation of the target is encoded as a 3 by 3 rotation matrix, we'll + // convert to a row-major array + jdouble pose1RotMat[9] = {0}; + jdouble pose2RotMat[9] = {0}; + + for (int i = 0; i < 9; i++) { + if (pose1.R) { + pose1RotMat[i] = pose1.R->data[i]; + } + if (pose2.R) { + pose2RotMat[i] = pose2.R->data[i]; + } + } + + // And translation a 3x1 vector (todo check axis order) + jdouble pose1Trans[3] = {0}; + jdouble pose2Trans[3] = {0}; + for (int i = 0; i < 3; i++) { + if (pose1.t) { + pose1Trans[i] = pose1.t->data[i]; + } + if (pose2.t) { + pose2Trans[i] = pose2.t->data[i]; + } + } + + jdoubleArray pose1rotArr = MakeJDoubleArray(env, {pose1RotMat, 9}); + jdoubleArray pose2rotArr = MakeJDoubleArray(env, {pose2RotMat, 9}); + jdoubleArray pose1transArr = MakeJDoubleArray(env, {pose1Trans, 3}); + jdoubleArray pose2transArr = MakeJDoubleArray(env, {pose2Trans, 3}); + jdouble err1 = error1; + jdouble err2 = error2; + + // Actually call the constructor + auto ret = env->NewObject( + detectionClass, constructor, (jint)detect->id, (jint)detect->hamming, + (jfloat)detect->decision_margin, harr, (jdouble)detect->c[0], + (jdouble)detect->c[1], carr, pose1transArr, pose1rotArr, err1, + pose2transArr, pose2rotArr, err2); + + // TODO we don't seem to need this... or at least, it doesnt leak rn + // env->ReleaseDoubleArrayElements(harr, h, 0); + // env->ReleaseDoubleArrayElements(carr, corners, 0); + + return ret; +} + +/* + * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Method: aprilTagDetectInternal + * Signature: (JJIIZDDDDDI)[Ljava/lang/Object; + */ +JNIEXPORT jobjectArray JNICALL +Java_edu_wpi_first_apriltag_jni_AprilTagJNI_aprilTagDetectInternal + (JNIEnv* env, jclass cls, jlong detectIdx, jlong pData, jint rows, jint cols, + jboolean doPoseEstimation, jdouble tagWidthMeters, jdouble fx, jdouble fy, + jdouble cx, jdouble cy, jint nIters) +{ + // No image, can't do anything + if (!pData) { + return nullptr; + } + + // Make an image_u8_t header for the Mat data + image_u8_t im = {static_cast(cols), static_cast(rows), + static_cast(cols), + reinterpret_cast(pData)}; + + // Get our detector + auto state = + std::find_if(detectors.begin(), detectors.end(), + [&](DetectorState& s) { return s.id == detectIdx; }); + if (state == detectors.end()) { + return nullptr; + } + + // And run the detector on our new image + zarray_t* detections = apriltag_detector_detect(state->td, &im); + + int size = zarray_size(detections); + + // Object array to return to Java + jobjectArray jarr = env->NewObjectArray(size, detectionClass, nullptr); + if (!jarr) { + std::printf("Couldn't make array\n"); + return nullptr; + } + + // Global pose + apriltag_pose_t pose1; + std::memset(&pose1, 0, sizeof(pose1)); + + apriltag_pose_t pose2; + std::memset(&pose2, 0, sizeof(pose2)); + + // std::printf("Created array %llu! Got %i targets!\n", &jarr, size); + // Add our detected targets to the array + for (int i = 0; i < size; ++i) { + apriltag_detection_t* det = nullptr; + zarray_get(detections, i, &det); + + if (det != nullptr) { + double err1 = + HUGE_VAL; // Should get overwritten if pose estimation is happening + double err2 = HUGE_VAL; + if (doPoseEstimation) { + // Feed results to the pose estimator + apriltag_detection_info_t info{det, tagWidthMeters, fx, fy, cx, cy}; + estimate_tag_pose_orthogonal_iteration(&info, &err1, &pose1, &err2, + &pose2, nIters); + } + + jobject obj = MakeJObject(env, det, pose1, pose2, err1, err2); + + env->SetObjectArrayElement(jarr, i, obj); + } + } + + // Now that stuff's in our Java-side array, we can clean up native memory + apriltag_detections_destroy(detections); + + return jarr; +} + +/* + * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Method: aprilTagDestroy + * Signature: (J)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_apriltag_jni_AprilTagJNI_aprilTagDestroy + (JNIEnv* env, jclass clazz, jlong detectIdx) +{ + auto state = + std::find_if(detectors.begin(), detectors.end(), + [&](DetectorState& s) { return s.id == detectIdx; }); + + if (state == detectors.end()) { + return; + } + + if (state->td) { + apriltag_detector_destroy(state->td); + state->td = nullptr; + } + if (state->tf) { + state->tf_destroy(state->tf); + state->tf = nullptr; + } + + detectors.erase(detectors.begin() + detectIdx); +} +} // extern "C" diff --git a/buildSrc/build.gradle b/buildSrc/build.gradle index 706a75a3ad..12e04d19ac 100644 --- a/buildSrc/build.gradle +++ b/buildSrc/build.gradle @@ -9,5 +9,5 @@ repositories { } } dependencies { - implementation "edu.wpi.first:native-utils:2023.8.2" + implementation "edu.wpi.first:native-utils:2023.9.0" } diff --git a/cscore/build.gradle b/cscore/build.gradle index 9b63b618af..50c70266c2 100644 --- a/cscore/build.gradle +++ b/cscore/build.gradle @@ -181,6 +181,8 @@ nativeUtils.exportsConfigs { } } +apply from: "${rootDir}/shared/imgui.gradle" + model { components { examplesMap.each { key, value -> @@ -193,7 +195,7 @@ model { lib project: ':wpinet', library: 'wpinet', linkage: 'shared' lib project: ':wpigui', library: 'wpigui', linkage: 'static' lib library: 'cscore', linkage: 'shared' - nativeUtils.useRequiredLibrary(it, 'imgui_static') + nativeUtils.useRequiredLibrary(it, 'imgui') if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) { it.buildable = false return diff --git a/datalogtool/build.gradle b/datalogtool/build.gradle index 9d89ce5ddf..46452a9662 100644 --- a/datalogtool/build.gradle +++ b/datalogtool/build.gradle @@ -25,6 +25,7 @@ if (!project.hasProperty('onlylinuxathena')) { def wpilibVersionFileOutput = file("$buildDir/generated/main/cpp/WPILibVersion.cpp") apply from: "${rootDir}/shared/libssh.gradle" + apply from: "${rootDir}/shared/imgui.gradle" task generateCppVersion() { description = 'Generates the wpilib version class' @@ -103,7 +104,7 @@ if (!project.hasProperty('onlylinuxathena')) { lib project: ':glass', library: 'glass', linkage: 'static' lib project: ':wpiutil', library: 'wpiutil', linkage: 'static' lib project: ':wpigui', library: 'wpigui', linkage: 'static' - nativeUtils.useRequiredLibrary(it, 'imgui_static', 'libssh') + nativeUtils.useRequiredLibrary(it, 'imgui', 'libssh') if (it.targetPlatform.operatingSystem.isWindows()) { it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib' it.linker.args << 'ws2_32.lib' << 'advapi32.lib' << 'crypt32.lib' << 'user32.lib' diff --git a/glass/build.gradle b/glass/build.gradle index 1d890118e9..47b943c45e 100644 --- a/glass/build.gradle +++ b/glass/build.gradle @@ -24,6 +24,8 @@ if (!project.hasProperty('onlylinuxathena')) { def wpilibVersionFileInput = file("src/app/generate/WPILibVersion.cpp.in") def wpilibVersionFileOutput = file("$buildDir/generated/app/cpp/WPILibVersion.cpp") + apply from: "${rootDir}/shared/imgui.gradle" + task generateCppVersion() { description = 'Generates the wpilib version class' group = 'WPILib' @@ -111,7 +113,7 @@ if (!project.hasProperty('onlylinuxathena')) { lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' lib project: ':wpimath', library: 'wpimath', linkage: 'shared' lib project: ':wpigui', library: 'wpigui', linkage: 'static' - nativeUtils.useRequiredLibrary(it, 'imgui_static') + nativeUtils.useRequiredLibrary(it, 'imgui') } appendDebugPathToBinaries(binaries) } @@ -142,7 +144,7 @@ if (!project.hasProperty('onlylinuxathena')) { lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' lib project: ':wpimath', library: 'wpimath', linkage: 'shared' lib project: ':wpigui', library: 'wpigui', linkage: 'static' - nativeUtils.useRequiredLibrary(it, 'imgui_static') + nativeUtils.useRequiredLibrary(it, 'imgui') } appendDebugPathToBinaries(binaries) } @@ -182,7 +184,7 @@ if (!project.hasProperty('onlylinuxathena')) { lib project: ':wpimath', library: 'wpimath', linkage: 'static' lib project: ':wpigui', library: 'wpigui', linkage: 'static' nativeUtils.useRequiredLibrary(it, 'opencv_static') - nativeUtils.useRequiredLibrary(it, 'imgui_static') + nativeUtils.useRequiredLibrary(it, 'imgui') if (it.targetPlatform.operatingSystem.isWindows()) { it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib' it.linker.args << '/DELAYLOAD:MF.dll' << '/DELAYLOAD:MFReadWrite.dll' << '/DELAYLOAD:MFPlat.dll' << '/delay:nobind' diff --git a/outlineviewer/build.gradle b/outlineviewer/build.gradle index 466bed5db5..bb4a4cad01 100644 --- a/outlineviewer/build.gradle +++ b/outlineviewer/build.gradle @@ -24,6 +24,8 @@ if (!project.hasProperty('onlylinuxathena')) { def wpilibVersionFileInput = file("src/main/generate/WPILibVersion.cpp.in") def wpilibVersionFileOutput = file("$buildDir/generated/main/cpp/WPILibVersion.cpp") + apply from: "${rootDir}/shared/imgui.gradle" + task generateCppVersion() { description = 'Generates the wpilib version class' group = 'WPILib' @@ -103,7 +105,7 @@ if (!project.hasProperty('onlylinuxathena')) { lib project: ':wpinet', library: 'wpinet', linkage: 'static' lib project: ':wpiutil', library: 'wpiutil', linkage: 'static' lib project: ':wpigui', library: 'wpigui', linkage: 'static' - nativeUtils.useRequiredLibrary(it, 'imgui_static') + nativeUtils.useRequiredLibrary(it, 'imgui') if (it.targetPlatform.operatingSystem.isWindows()) { it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib' } else if (it.targetPlatform.operatingSystem.isMacOsX()) { diff --git a/roborioteamnumbersetter/build.gradle b/roborioteamnumbersetter/build.gradle index 5099e9edd0..8a7070eb7e 100644 --- a/roborioteamnumbersetter/build.gradle +++ b/roborioteamnumbersetter/build.gradle @@ -25,6 +25,7 @@ if (!project.hasProperty('onlylinuxathena')) { def wpilibVersionFileOutput = file("$buildDir/generated/main/cpp/WPILibVersion.cpp") apply from: "${rootDir}/shared/libssh.gradle" + apply from: "${rootDir}/shared/imgui.gradle" task generateCppVersion() { description = 'Generates the wpilib version class' @@ -104,7 +105,7 @@ if (!project.hasProperty('onlylinuxathena')) { lib project: ':wpinet', library: 'wpinet', linkage: 'static' lib project: ':wpiutil', library: 'wpiutil', linkage: 'static' lib project: ':wpigui', library: 'wpigui', linkage: 'static' - nativeUtils.useRequiredLibrary(it, 'imgui_static', 'libssh') + nativeUtils.useRequiredLibrary(it, 'imgui', 'libssh') if (it.targetPlatform.operatingSystem.isWindows()) { it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib' it.linker.args << 'ws2_32.lib' << 'advapi32.lib' << 'crypt32.lib' << 'user32.lib' diff --git a/shared/apriltaglib.gradle b/shared/apriltaglib.gradle new file mode 100644 index 0000000000..3cfddf6842 --- /dev/null +++ b/shared/apriltaglib.gradle @@ -0,0 +1,13 @@ +nativeUtils { + nativeDependencyContainer { + apriltaglib(getNativeDependencyTypeClass('WPIStaticMavenDependency')) { + groupId = "edu.wpi.first.thirdparty.frc2023" + artifactId = "apriltaglib" + headerClassifier = "headers" + sourceClassifier = "sources" + ext = "zip" + version = '3.2.0-1' + targetPlatforms.addAll(nativeUtils.wpi.platforms.allPlatforms) + } + } +} diff --git a/shared/config.gradle b/shared/config.gradle index 295d470655..9ccebeca69 100644 --- a/shared/config.gradle +++ b/shared/config.gradle @@ -13,12 +13,9 @@ nativeUtils.withCrossLinuxArm64() nativeUtils { wpi { configureDependencies { - wpiVersion = "-1" niLibVersion = "2023.1.0" opencvVersion = "4.6.0-3" googleTestVersion = "1.11.0-4" - imguiVersion = "1.88-12" - wpimathVersion = "-1" } } } @@ -37,7 +34,7 @@ nativeUtils.platformConfigs.each { if (it.name.contains('osx')) { it.linker.args << '-Wl,-rpath,\'@loader_path\'' it.linker.args << "-headerpad_max_install_names" - } else { + } else if (it.name.contains('linux')) { it.linker.args << '-Wl,-rpath,\'$ORIGIN\'' } } diff --git a/shared/imgui.gradle b/shared/imgui.gradle new file mode 100644 index 0000000000..1e35c6e835 --- /dev/null +++ b/shared/imgui.gradle @@ -0,0 +1,13 @@ +nativeUtils { + nativeDependencyContainer { + imgui(getNativeDependencyTypeClass('WPIStaticMavenDependency')) { + groupId = "edu.wpi.first.thirdparty.frc2023" + artifactId = "imgui" + headerClassifier = "headers" + sourceClassifier = "sources" + ext = "zip" + version = '1.88-12' + targetPlatforms.addAll(nativeUtils.wpi.platforms.allPlatforms) + } + } +} diff --git a/simulation/halsim_gui/build.gradle b/simulation/halsim_gui/build.gradle index e11d7fb121..a8608e38e5 100644 --- a/simulation/halsim_gui/build.gradle +++ b/simulation/halsim_gui/build.gradle @@ -18,6 +18,9 @@ if (!project.hasProperty('onlylinuxathena')) { apply from: "${rootDir}/shared/plugins/setupBuild.gradle" + + apply from: "${rootDir}/shared/imgui.gradle" + model { binaries { all { @@ -28,7 +31,7 @@ if (!project.hasProperty('onlylinuxathena')) { project(':ntcore').addNtcoreDependency(it, 'shared') lib project: ':wpinet', library: 'wpinet', linkage: 'shared' lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' - nativeUtils.useRequiredLibrary(it, 'imgui_static') + nativeUtils.useRequiredLibrary(it, 'imgui') if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) { it.buildable = false return diff --git a/wpigui/build.gradle b/wpigui/build.gradle index 7b28843938..fb08198a8b 100644 --- a/wpigui/build.gradle +++ b/wpigui/build.gradle @@ -14,6 +14,7 @@ if (!project.hasProperty('onlylinuxathena')) { } apply from: "${rootDir}/shared/config.gradle" + apply from: "${rootDir}/shared/imgui.gradle" nativeUtils.exportsConfigs { wpigui { @@ -48,7 +49,7 @@ if (!project.hasProperty('onlylinuxathena')) { } } binaries.all { - nativeUtils.useRequiredLibrary(it, 'imgui_static') + nativeUtils.useRequiredLibrary(it, 'imgui') if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) { it.buildable = false return @@ -114,7 +115,7 @@ if (!project.hasProperty('onlylinuxathena')) { } binaries.all { lib library: 'wpigui' - nativeUtils.useRequiredLibrary(it, 'imgui_static') + nativeUtils.useRequiredLibrary(it, 'imgui') } } }