Add ArUco families support, deep-copy utilities, and pose ambiguity handling

README.md

@@ -72,7 +72,8 @@ Some heuristics for when to choose other tag families:
 1. If you need more tags, use tagStandard52h13
 2. If you need to maximize the use of space on a small circular object, use tagCircle49h12 (or tagCircle21h7).
 3. If you want to make a recursive tag use tagCustom48h12.
-4. If you want compatibility with the ArUcO detector use tag36h11
+4. If you need ArUco support, use the native ArUco families (e.g., `tagAruco4x4_50`, `tagAruco5x5_100`, `tagAruco6x6_250`, `tagAruco7x7_1000`, etc.). These are now fully integrated and optimized.
+4. If you want compatibility with the legacy ArUco detector use tag36h11
 
 If none of these fit your needs, generate your own custom tag family [here](https://github.com/AprilRobotics/apriltag-generation).
 
@@ -193,6 +194,28 @@ Note: The tag size should not be measured from the outside of the tag. The tag s
 ### Coordinate System
 The coordinate system has the origin at the camera center. The z-axis points from the camera center out the camera lens. The x-axis is to the right in the image taken by the camera, and y is down. The tag's coordinate frame is centered at the center of the tag. From the viewer's perspective, the x-axis is to the right, y-axis down, and z-axis is into the tag.
 
+### Handling Pose Ambiguity
+Planar targets often result in two possible pose solutions with similar errors (the "ambiguity" problem). To retrieve the alternative solution, you can use:
+
+```c
+apriltag_pose_t pose1, pose2;
+double err1 = estimate_tag_pose(&info, &pose1);
+double err2;
+
+// v and p are the object and image points used during the iteration
+get_second_solution(v, p, &pose1, &pose2, nIters, &err2);
+```
+
+This is particularly useful when temporal filtering or additional constraints are used to disambiguate the tag's orientation.
+
+Utility Functions
+=================
+AprilTag 3 now includes helper functions for deep-copying structures, which is essential for multi-threaded applications or when you need to store detections beyond the detector's lifecycle.
+
+* `apriltag_detector_copy(td)`: Creates a clone of the detector configuration.
+* `apriltag_detections_copy(detections)`: Returns a new `zarray_t` with deep copies of all `apriltag_detection_t` objects.
+* `apriltag_detection_copy(src, dst)`: Performs a deep copy of a single detection into an existing structure.
+
 Debugging
 =========
 

apriltag.c

@@ -1469,6 +1469,69 @@ void apriltag_detections_destroy(zarray_t *detections)
     zarray_destroy(detections);
 }
 
+void apriltag_detection_copy(apriltag_detection_t* src, apriltag_detection_t* dst)
+{
+    assert(src != NULL);
+    assert(dst != NULL);
+
+    if (dst->H) {
+        matd_destroy(dst->H);
+    }
+    dst->H = matd_copy(src->H);
+
+    dst->c[0] = src->c[0];
+    dst->c[1] = src->c[1];
+
+    for (int i = 0; i < 4; i++) {
+        for (int j = 0; j < 2; j++) {
+            dst->p[i][j] = src->p[i][j];
+        }
+    }
+
+    dst->id = src->id;
+    dst->family = src->family;
+    dst->hamming = src->hamming;
+    dst->decision_margin = src->decision_margin;
+}
+
+zarray_t* apriltag_detections_copy(zarray_t* detections)
+{
+    zarray_t* detections_copy = zarray_create(sizeof(apriltag_detection_t*));
+    for (int i = 0; i < zarray_size(detections); i++) {
+        apriltag_detection_t* det;
+        zarray_get(detections, i, &det);
+
+        apriltag_detection_t* det_copy = (apriltag_detection_t*)calloc(1, sizeof(apriltag_detection_t));
+        apriltag_detection_copy(det, det_copy);
+        zarray_add(detections_copy, &det_copy);
+    }
+
+    return detections_copy;
+}
+
+apriltag_detector_t *apriltag_detector_copy(apriltag_detector_t *td)
+{
+    apriltag_detector_t *td_copy = apriltag_detector_create();
+
+    td_copy->nthreads = td->nthreads;
+    td_copy->quad_decimate = td->quad_decimate;
+    td_copy->quad_sigma = td->quad_sigma;
+
+    td_copy->qtp.max_nmaxima = td->qtp.max_nmaxima ;
+    td_copy->qtp.min_cluster_pixels = td->qtp.min_cluster_pixels;
+
+    td_copy->qtp.max_line_fit_mse = td->qtp.max_line_fit_mse;
+    td_copy->qtp.cos_critical_rad = td->qtp.cos_critical_rad;
+    td_copy->qtp.deglitch = td->qtp.deglitch;
+    td_copy->qtp.min_white_black_diff = td->qtp.min_white_black_diff;
+
+    td_copy->refine_edges = td->refine_edges;
+    td_copy->decode_sharpening = td->decode_sharpening;
+    td_copy->debug = td->debug;
+
+    return td_copy;
+}
+
 image_u8_t *apriltag_to_image(apriltag_family_t *fam, uint32_t idx)
 {
     assert(fam != NULL);

apriltag.h

@@ -267,6 +267,15 @@ void apriltag_detection_destroy(apriltag_detection_t *det);
 // destroys the array AND the detections within it.
 void apriltag_detections_destroy(zarray_t *detections);
 
+// Performs a deep copy of an AprilTag detection structure from a source to a destination.
+void apriltag_detection_copy(apriltag_detection_t* src, apriltag_detection_t* dst);
+
+// Creates a complete deep copy of a list of AprilTag detections.
+zarray_t* apriltag_detections_copy(zarray_t* detections);
+
+// Clones an AprilTag detector configuration into a new instance.
+apriltag_detector_t *apriltag_detector_copy(apriltag_detector_t *td);
+
 // Renders the apriltag.
 // Caller is responsible for calling image_u8_destroy on the image
 image_u8_t *apriltag_to_image(apriltag_family_t *fam, uint32_t idx);

apriltag_pose.c

@@ -545,3 +545,15 @@ double estimate_tag_pose(apriltag_detection_info_t* info, apriltag_pose_t* pose)
         return err2;
     }
 }
+
+void get_second_solution(matd_t* v[4], matd_t* p[4], apriltag_pose_t* solution1, apriltag_pose_t* solution2, int nIters, double* err2)
+{
+    solution2->R = fix_pose_ambiguities(v, p, solution1->t, solution1->R, 4);
+    if (solution2->R) {
+        solution2->t = matd_create(3, 1);
+        *err2 = orthogonal_iteration(v, p, &solution2->t, &solution2->R, 4, nIters);
+    }
+    else {
+        *err2 = HUGE_VAL;
+    }
+}

apriltag_pose.h

@@ -74,6 +74,8 @@ void estimate_tag_pose_orthogonal_iteration(
  */
 double estimate_tag_pose(apriltag_detection_info_t* info, apriltag_pose_t* pose);
 
+void get_second_solution(matd_t* v[4], matd_t* p[4], apriltag_pose_t* solution1, apriltag_pose_t* solution2, int nIters, double* err2);
+
 #ifdef __cplusplus
 }
 #endif

apriltag_py_type.docstring

@@ -51,6 +51,24 @@ The constructor takes a number of arguments:
   - "tagStandard41h12"
   - "tagStandard52h13"
   - "tagCustom48h12"
+  - "tagAruco4x4_50"
+  - "tagAruco4x4_100"
+  - "tagAruco4x4_250"
+  - "tagAruco4x4_1000"
+  - "tagAruco5x5_50"
+  - "tagAruco5x5_100"
+  - "tagAruco5x5_250"
+  - "tagAruco5x5_1000"
+  - "tagAruco6x6_50"
+  - "tagAruco6x6_100"
+  - "tagAruco6x6_250"
+  - "tagAruco6x6_1000"
+  - "tagAruco7x7_50"
+  - "tagAruco7x7_100"
+  - "tagAruco7x7_250"
+  - "tagAruco7x7_1000"
+  - "tagArucoMIP36h12"
+
 
 All the other arguments are optional:
 

apriltag_pywrap.c

@@ -20,6 +20,23 @@
 #include "tagCustom48h12.h"
 #include "tagStandard41h12.h"
 #include "tagStandard52h13.h"
+#include "tagAruco4x4_50.h"
+#include "tagAruco4x4_100.h"
+#include "tagAruco4x4_250.h"
+#include "tagAruco4x4_1000.h"
+#include "tagAruco5x5_50.h"
+#include "tagAruco5x5_100.h"
+#include "tagAruco5x5_250.h"
+#include "tagAruco5x5_1000.h"
+#include "tagAruco6x6_50.h"
+#include "tagAruco6x6_100.h"
+#include "tagAruco6x6_250.h"
+#include "tagAruco6x6_1000.h"
+#include "tagAruco7x7_50.h"
+#include "tagAruco7x7_100.h"
+#include "tagAruco7x7_250.h"
+#include "tagAruco7x7_1000.h"
+#include "tagArucoMIP36h12.h"
 
 
 #define SUPPORTED_TAG_FAMILIES(_)           \
@@ -31,7 +48,24 @@
     _(tagCircle49h12)                       \
     _(tagStandard41h12)                     \
     _(tagStandard52h13)                     \
-    _(tagCustom48h12)
+    _(tagCustom48h12)                       \
+    _(tagAruco4x4_50)                       \
+    _(tagAruco4x4_100)                      \
+    _(tagAruco4x4_250)                      \
+    _(tagAruco4x4_1000)                     \
+    _(tagAruco5x5_50)                       \
+    _(tagAruco5x5_100)                      \
+    _(tagAruco5x5_250)                      \
+    _(tagAruco5x5_1000)                     \
+    _(tagAruco6x6_50)                       \
+    _(tagAruco6x6_100)                      \
+    _(tagAruco6x6_250)                      \
+    _(tagAruco6x6_1000)                     \
+    _(tagAruco7x7_50)                       \
+    _(tagAruco7x7_100)                      \
+    _(tagAruco7x7_250)                      \
+    _(tagAruco7x7_1000)                     \
+    _(tagArucoMIP36h12)
 
 #define TAG_CREATE_FAMILY(name) \
     else if (0 == strcmp(family, #name)) self->tf = name ## _create();

example/apriltag_demo.c

@@ -50,6 +50,23 @@ either expressed or implied, of the Regents of The University of Michigan.
 #include "tagCustom48h12.h"
 #include "tagStandard41h12.h"
 #include "tagStandard52h13.h"
+#include "tagAruco4x4_50.h"
+#include "tagAruco4x4_100.h"
+#include "tagAruco4x4_250.h"
+#include "tagAruco4x4_1000.h"
+#include "tagAruco5x5_50.h"
+#include "tagAruco5x5_100.h"
+#include "tagAruco5x5_250.h"
+#include "tagAruco5x5_1000.h"
+#include "tagAruco6x6_50.h"
+#include "tagAruco6x6_100.h"
+#include "tagAruco6x6_250.h"
+#include "tagAruco6x6_1000.h"
+#include "tagAruco7x7_50.h"
+#include "tagAruco7x7_100.h"
+#include "tagAruco7x7_250.h"
+#include "tagAruco7x7_1000.h"
+#include "tagArucoMIP36h12.h"
 
 #include "common/getopt.h"
 #include "common/image_u8.h"
@@ -99,6 +116,40 @@ int main(int argc, char *argv[])
         tf = tagStandard52h13_create();
     } else if (!strcmp(famname, "tagCustom48h12")) {
         tf = tagCustom48h12_create();
+    } else if (!strcmp(famname, "tagAruco4x4_50")) {
+        tf = tagAruco4x4_50_create();
+    } else if (!strcmp(famname, "tagAruco4x4_100")) {
+        tf = tagAruco4x4_100_create();
+    } else if (!strcmp(famname, "tagAruco4x4_250")) {
+        tf = tagAruco4x4_250_create();
+    } else if (!strcmp(famname, "tagAruco4x4_1000")) {
+        tf = tagAruco4x4_1000_create();
+    } else if (!strcmp(famname, "tagAruco5x5_50")) {
+        tf = tagAruco5x5_50_create();
+    } else if (!strcmp(famname, "tagAruco5x5_100")) {
+        tf = tagAruco5x5_100_create();
+    } else if (!strcmp(famname, "tagAruco5x5_250")) {
+        tf = tagAruco5x5_250_create();
+    } else if (!strcmp(famname, "tagAruco5x5_1000")) {
+        tf = tagAruco5x5_1000_create();
+    } else if (!strcmp(famname, "tagAruco6x6_50")) {
+        tf = tagAruco6x6_50_create();
+    } else if (!strcmp(famname, "tagAruco6x6_100")) {
+        tf = tagAruco6x6_100_create();
+    } else if (!strcmp(famname, "tagAruco6x6_250")) {
+        tf = tagAruco6x6_250_create();
+    } else if (!strcmp(famname, "tagAruco6x6_1000")) {
+        tf = tagAruco6x6_1000_create();
+    } else if (!strcmp(famname, "tagAruco7x7_50")) {
+        tf = tagAruco7x7_50_create();
+    } else if (!strcmp(famname, "tagAruco7x7_100")) {
+        tf = tagAruco7x7_100_create();
+    } else if (!strcmp(famname, "tagAruco7x7_250")) {
+        tf = tagAruco7x7_250_create();
+    } else if (!strcmp(famname, "tagAruco7x7_1000")) {
+        tf = tagAruco7x7_1000_create();
+    } else if (!strcmp(famname, "tagArucoMIP36h12")) {
+        tf = tagArucoMIP36h12_create();
     } else {
         printf("Unrecognized tag family name. Use e.g. \"tag36h11\".\n");
         exit(-1);
@@ -276,6 +327,40 @@ int main(int argc, char *argv[])
         tagStandard52h13_destroy(tf);
     } else if (!strcmp(famname, "tagCustom48h12")) {
         tagCustom48h12_destroy(tf);
+    }else if (!strcmp(famname, "tagAruco4x4_50")) {
+        tagAruco4x4_50_destroy(tf);
+    } else if (!strcmp(famname, "tagAruco4x4_100")) {
+        tagAruco4x4_100_destroy(tf);
+    } else if (!strcmp(famname, "tagAruco4x4_250")) {
+        tagAruco4x4_250_destroy(tf);
+    } else if (!strcmp(famname, "tagAruco4x4_1000")) {
+        tagAruco4x4_1000_destroy(tf);
+    } else if (!strcmp(famname, "tagAruco5x5_50")) {
+        tagAruco5x5_50_destroy(tf);
+    } else if (!strcmp(famname, "tagAruco5x5_100")) {
+        tagAruco5x5_100_destroy(tf);
+    } else if (!strcmp(famname, "tagAruco5x5_250")) {
+        tagAruco5x5_250_destroy(tf);
+    } else if (!strcmp(famname, "tagAruco5x5_1000")) {
+        tagAruco5x5_1000_destroy(tf);
+    } else if (!strcmp(famname, "tagAruco6x6_50")) {
+        tagAruco6x6_50_destroy(tf);
+    } else if (!strcmp(famname, "tagAruco6x6_100")) {
+        tagAruco6x6_100_destroy(tf);
+    } else if (!strcmp(famname, "tagAruco6x6_250")) {
+        tagAruco6x6_250_destroy(tf);
+    } else if (!strcmp(famname, "tagAruco6x6_1000")) {
+        tagAruco6x6_1000_destroy(tf);
+    } else if (!strcmp(famname, "tagAruco7x7_50")) {
+        tagAruco7x7_50_destroy(tf);
+    } else if (!strcmp(famname, "tagAruco7x7_100")) {
+        tagAruco7x7_100_destroy(tf);
+    } else if (!strcmp(famname, "tagAruco7x7_250")) {
+        tagAruco7x7_250_destroy(tf);
+    } else if (!strcmp(famname, "tagAruco7x7_1000")) {
+        tagAruco7x7_1000_destroy(tf);
+    } else if (!strcmp(famname, "tagArucoMIP36h12")) {
+        tagArucoMIP36h12_destroy(tf);
     }
 
     getopt_destroy(getopt);