diff --git a/AprilTagJNI.cpp b/AprilTagJNI.cpp index a1c7041b..561b18e2 100644 --- a/AprilTagJNI.cpp +++ b/AprilTagJNI.cpp @@ -13,6 +13,7 @@ #include #include +#include struct DetectorState { @@ -28,7 +29,8 @@ extern "C" { JNIEXPORT jlong JNICALL Java_org_photonvision_vision_apriltag_AprilTagJNI_AprilTag_1Create(JNIEnv *env, - jclass cls, jstring jstr, jdouble decimate, jdouble blur, jint threads, jboolean debug, jboolean refine_edges) + jclass cls, jstring jstr, jdouble decimate, jdouble blur, jint threads, jboolean debug, jboolean refine_edges, jint min_cluster_pixels, + jint maxErrorBits, jint extraDecisionMargin) { // Initialize tag detector with options apriltag_family_t *tf = NULL; @@ -91,6 +93,9 @@ extern "C" td->nthreads = threads; td->debug = debug; td->refine_edges = refine_edges; + td->qtp.min_cluster_pixels = (int) min_cluster_pixels; + td->maxErrorBits = maxErrorBits; + td->extraDecisionMargin = extraDecisionMargin; env->ReleaseStringUTFChars(jstr, famname); @@ -225,12 +230,13 @@ extern "C" jdouble pose1RotMat[9] = {0}; jdouble pose2RotMat[9] = {0}; - // Row major so inner loop is rows for (int i = 0; i < 9; i++) { - if (pose1.R) + if (pose1.R) { pose1RotMat[i] = pose1.R->data[i]; - if (pose2.R) + } + if (pose2.R) { pose2RotMat[i] = pose2.R->data[i]; + } } // And translation a 3x1 vector (todo check axis order) @@ -260,12 +266,10 @@ extern "C" pose1transArr, pose1rotArr, err1, pose2transArr, pose2rotArr, err2); - // // I think this prevents us from leaking new double arrays every time + // 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 nullptr; - return ret; } @@ -274,8 +278,8 @@ extern "C" jint rows, jint cols, jboolean doPoseEstimation, jdouble tagWidthMeters, jdouble fx, jdouble fy, jdouble cx, jdouble cy, jint nIters) { - if (!pData) - { + // No image, can't do anything + if (!pData) { return nullptr; } @@ -286,11 +290,10 @@ extern "C" (uint8_t *)pData}; // Get our detector - // printf("Finding detector at idx %i\n", detectIdx); auto state = std::find_if(detectors.begin(), detectors.end(), [&](DetectorState& s) { return s.id == detectIdx; }); - if (state == detectors.end()) + if (state == detectors.end()) { return nullptr; - // printf("Found detector %llu, end %llu!\n", state, detectors.end()); + } // And run the detector on our new image zarray_t *detections = apriltag_detector_detect(state->td, &im); @@ -314,69 +317,28 @@ extern "C" // Add our detected targets to the array for (size_t i = 0; i < size; ++i) { - apriltag_detection_t *det; + apriltag_detection_t *det = {0}; zarray_get(detections, i, &det); - // printf("Got %i\n", det); if (det != nullptr) { - double err1 = 9999.0; //Should get overwritten if pose estimation is happening - double err2 = 9999.0; + 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); - - if (pose1.t && pose1.t->data) { - printf("Trans 1: "); - for (int i = 0; i < 3; i++) printf("%f ", pose1.t->data[i]); - printf("\n"); - } - if (pose2.t && pose2.t->data) { - printf("Trans 2: "); - for (int i = 0; i < 3; i++) printf("%f ", pose2.t->data[i]); - printf("\n"); - } - printf("Error 1 %f Error 2 %f\n", err1, err2); } jobject obj = MakeJObject(env, det, pose1, pose2, err1, err2); env->SetObjectArrayElement(jarr, i, obj); - // printf("Set element of array %i and idx %i to %i\n", &jarr, i, obj); } } - // Now that stuff's in our array, we can clean up native memory + // Now that stuff's in our Java-side array, we can clean up native memory apriltag_detections_destroy(detections); - - // TODO do I need to do this - // if (pose1.R) { - // printf("Rotation 1: "); - // for (int i = 0; i < 9; i++) printf("%f ", pose1.R->data[i]); - // printf("\n"); - // matd_destroy(pose1.R); - // } - // if (pose2.R) { - // printf("Rotation 2: "); - // for (int i = 0; i < 9; i++) printf("%f ", pose2.R->data[i]); - // printf("\n"); - // matd_destroy(pose2.R); - // } - // if (pose1.t) { - // printf("Trans 1: "); - // for (int i = 0; i < 3; i++) printf("%f ", pose1.t->data[i]); - // printf("\n"); - // // matd_destroy(pose1.t); - // } - // if (pose2.t) { - // printf("Trans 2: "); - // for (int i = 0; i < 3; i++) printf("%f ", pose2.t->data[i]); - // printf("\n"); - // // matd_destroy(pose2.t); - // } - - // printf("Returning %i\n", jarr); + return jarr; } @@ -389,8 +351,6 @@ extern "C" if(state == detectors.end()) return; - // printf("Destroying detector FR\n", detectIdx); - if (state->td) { apriltag_detector_destroy(state->td); @@ -403,7 +363,5 @@ extern "C" } detectors.erase(detectors.begin() + detectIdx); - // printf("New len %i elements:\n", detectors.size()); - // std::for_each(detectors.begin(), detectors.end(), [](DetectorState &s){printf("id %i\n", s.id);}); } } \ No newline at end of file diff --git a/AprilTagJNI.h b/AprilTagJNI.h index 50574cd0..bfcee045 100644 --- a/AprilTagJNI.h +++ b/AprilTagJNI.h @@ -13,7 +13,7 @@ extern "C" { * Signature: (Ljava/lang/String;DDIZZ)J */ JNIEXPORT jlong JNICALL Java_org_photonvision_vision_apriltag_AprilTagJNI_AprilTag_1Create - (JNIEnv *, jclass, jstring, jdouble, jdouble, jint, jboolean, jboolean); + (JNIEnv *, jclass, jstring, jdouble, jdouble, jint, jboolean, jboolean, jint, jint, jint); JNIEXPORT void JNICALL Java_org_photonvision_vision_apriltag_AprilTagJNI_AprilTag_1Destroy (JNIEnv *, jclass, jlong); diff --git a/apriltag.c b/apriltag.c index 6446d368..b0e39aa9 100644 --- a/apriltag.c +++ b/apriltag.c @@ -916,7 +916,11 @@ static void quad_decode_task(void *_u) float decision_margin = quad_decode(td, family, im, quad, &entry, task->im_samples); - if (decision_margin >= 0 && entry.hamming < 255) { + if (decision_margin >= 0 && entry.hamming < 255 + // Additions (Matt/Photon): Check decision margin and hamming + && entry.hamming <= td->maxErrorBits + && decision_margin >= td->extraDecisionMargin) { + apriltag_detection_t *det = calloc(1, sizeof(apriltag_detection_t)); det->family = family; diff --git a/apriltag.h b/apriltag.h index 35bc698f..d3844692 100644 --- a/apriltag.h +++ b/apriltag.h @@ -188,6 +188,11 @@ struct apriltag_detector // Used for thread safety. pthread_mutex_t mutex; + + // Photon additions (matt): extra params for max bits corrected, + // and extra decision margin + int maxErrorBits; + int extraDecisionMargin; }; // Represents the detection of a tag. These are returned to the user