Rebase from 'upstream'

ros2-gbp · May 13, 2024 · 4106bdf · 4106bdf
1 parent de7613d
commit 4106bdf
Show file tree

Hide file tree

Showing 15 changed files with 165 additions and 55 deletions.
diff --git a/.github/workflows/cmake-ubuntu.yml b/.github/workflows/cmake-ubuntu.yml
@@ -0,0 +1,62 @@
+name: CMake (Ubuntu)
+
+on:
+  push:
+    branches: [ "master" ]
+  pull_request:
+    branches: [ "master" ]
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+
+    strategy:
+      matrix:
+        version: ["20.04", "22.04", "24.04"]
+        c_compiler: [gcc, clang]
+        shared_libs: ['ON', 'OFF']
+        # manual selection for the latest, non-default, compilers
+        include:
+        - version: "24.04"
+          c_compiler: gcc-14
+        - version: "24.04"
+          c_compiler: clang-18
+
+    container:
+      image: ubuntu:${{ matrix.version }}
+
+    env:
+      DEBIAN_FRONTEND: noninteractive
+
+    steps:
+    - name: install dependencies
+      run: |
+        apt update
+        apt install -y --no-install-recommends cmake ninja-build ${{ matrix.c_compiler }}
+        apt install -y --no-install-recommends python3-dev python3-numpy
+
+    - uses: actions/checkout@v4
+
+    - name: Set reusable strings
+      id: strings
+      shell: bash
+      run: |
+        echo "build-output-dir=$GITHUB_WORKSPACE/build" >> "$GITHUB_OUTPUT"
+
+    - name: Configure CMake
+      run: >
+        cmake -B ${{ steps.strings.outputs.build-output-dir }}
+        -G Ninja
+        -D CMAKE_C_COMPILER=${{ matrix.c_compiler }}
+        -D CMAKE_BUILD_TYPE=Release
+        -D BUILD_SHARED_LIBS=${{ matrix.shared_libs }}
+        -D BUILD_TESTING=ON
+        -S $GITHUB_WORKSPACE
+
+    - name: Build & Install
+      run: cmake --build ${{ steps.strings.outputs.build-output-dir }} --target install
+
+    - name: Test
+      working-directory: ${{ steps.strings.outputs.build-output-dir }}/test
+      run: |
+        ctest --no-tests=error --output-on-failure --verbose
diff --git a/.github/workflows/colcon-workspace.yml b/.github/workflows/colcon-workspace.yml
@@ -11,7 +11,6 @@ jobs:
 
     strategy:
       matrix:
-        ros_distribution: [noetic, humble]
         include:
           - docker_image: ubuntu:20.04
             ros_distribution: noetic
@@ -21,6 +20,10 @@ jobs:
             ros_distribution: humble
             ros_version: 2
 
+          - docker_image: ubuntu:24.04
+            ros_distribution: jazzy
+            ros_version: 2
+
     container:
       image: ${{ matrix.docker_image }}
 
@@ -58,14 +61,16 @@ jobs:
 
     strategy:
       matrix:
-        ros_distribution: [noetic, humble]
         include:
           - ros_distribution: noetic
             ros_version: 1
 
           - ros_distribution: humble
             ros_version: 2
 
+          - ros_distribution: jazzy
+            ros_version: 2
+
     steps:
       - uses: actions/checkout@v4
         with:
@@ -96,7 +101,7 @@ jobs:
 
     strategy:
       matrix:
-        ros_distribution: [humble]
+        ros_distribution: [humble, jazzy]
 
     steps:
       - uses: actions/checkout@v4

diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.16)
-project(apriltag VERSION 3.4.0 LANGUAGES C)
+project(apriltag VERSION 3.4.1 LANGUAGES C)
 
 if(POLICY CMP0077)
     cmake_policy(SET CMP0077 NEW)

diff --git a/apriltag.c b/apriltag.c
@@ -445,7 +445,9 @@ static matd_t* homography_compute2(double c[4][4]) {
             }
         }
 
-        assert(max_val_idx >= 0);
+        if (max_val_idx < 0) {
+            return NULL;
+        }
 
         if (max_val < epsilon) {
             debug_print("WRN: Matrix is singular.\n");

diff --git a/apriltag_pywrap.c b/apriltag_pywrap.c
@@ -75,6 +75,8 @@ typedef struct {
 static PyObject *
 apriltag_new(PyTypeObject *type, PyObject *args, PyObject *kwargs)
 {
+    errno = 0;
+
     bool success = false;
 
     apriltag_py_t* self = (apriltag_py_t*)type->tp_alloc(type, 0);
@@ -200,6 +202,8 @@ static void apriltag_dealloc(apriltag_py_t* self)
 static PyObject* apriltag_detect(apriltag_py_t* self,
                                  PyObject* args)
 {
+    errno = 0;
+
     PyObject*      result           = NULL;
     PyArrayObject* xy_c             = NULL;
     PyArrayObject* xy_lb_rb_rt_lt   = NULL;

diff --git a/common/g2d.c b/common/g2d.c
@@ -321,7 +321,7 @@ int g2d_polygon_contains_point(const zarray_t *poly, double q[2])
     int psz = zarray_size(poly);
     assert(psz > 0);
 
-    int last_quadrant;
+    int last_quadrant = 0;
     int quad_acc = 0;
 
     for (int i = 0; i <= psz; i++) {

diff --git a/common/image_u8.c b/common/image_u8.c
@@ -265,6 +265,9 @@ void image_u8_draw_annulus(image_u8_t *im, float x0, float y0, float r0, float r
 void image_u8_draw_line(image_u8_t *im, float x0, float y0, float x1, float y1, int v, int width)
 {
     double dist = sqrtf((y1-y0)*(y1-y0) + (x1-x0)*(x1-x0));
+    if (dist == 0) {
+        return;
+    }
     double delta = 0.5 / dist;
 
     // terrible line drawing code

diff --git a/common/zmaxheap.c b/common/zmaxheap.c
@@ -361,7 +361,7 @@ void zmaxheap_test()
 {
     int cap = 10000;
     int sz = 0;
-    int32_t *vals = calloc(sizeof(int32_t), cap);
+    int32_t *vals = calloc(cap, sizeof(int32_t));
 
     zmaxheap_t *heap = zmaxheap_create(sizeof(int32_t));
 

diff --git a/package.xml b/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>apriltag</name>
-  <version>3.4.0</version>
+  <version>3.4.1</version>
   <description>AprilTag detector library</description>
 
   <maintainer email="mkrogius@umich.edu">Max Krogius</maintainer>

diff --git a/test/data/33369213973_9d9bb4cc96_c.txt b/test/data/33369213973_9d9bb4cc96_c.txt
@@ -1,9 +1,9 @@
-0, 0, (277.8802 327.5163), (250.1780 329.6940), (251.7135 357.2946), (279.7680 354.8845)
-1, 0, (571.4235 361.6797), (543.6642 364.9229), (544.4681 395.1039), (572.3226 391.6599)
-2, 0, (760.7551 428.9775), (726.5153 428.5690), (727.0422 461.5150), (761.2695 462.2295)
-3, 0, (512.2699 371.3797), (511.7118 342.6652), (485.4774 344.9952), (486.1231 374.7085)
-4, 0, (640.0231 425.9307), (640.9352 458.5056), (669.8774 462.4900), (669.6273 429.5844)
-5, 0, (353.0279 346.9656), (329.3977 344.6678), (330.0048 372.9047), (354.1879 375.4345)
-6, 0, (465.4157 341.5291), (466.7475 370.4995), (480.0401 375.3805), (479.3255 346.8523)
-7, 0, (448.3790 335.3048), (421.9637 338.3846), (422.9550 366.6558), (449.2383 363.5178)
-8, 0, (414.5614 338.8829), (403.2398 333.4245), (404.6997 361.6877), (415.0851 366.5417)
+0, (277.8802 327.5163), (250.1780 329.6940), (251.7135 357.2946), (279.7680 354.8845)
+0, (353.0279 346.9656), (329.3977 344.6678), (330.0048 372.9047), (354.1879 375.4345)
+0, (414.5614 338.8829), (403.2398 333.4245), (404.6997 361.6877), (415.0851 366.5417)
+0, (448.3790 335.3048), (421.9637 338.3846), (422.9550 366.6558), (449.2383 363.5178)
+0, (465.4157 341.5291), (466.7475 370.4995), (480.0401 375.3805), (479.3255 346.8523)
+0, (512.2699 371.3797), (511.7118 342.6652), (485.4774 344.9952), (486.1231 374.7085)
+0, (571.4235 361.6797), (543.6642 364.9229), (544.4681 395.1039), (572.3226 391.6599)
+0, (640.0231 425.9307), (640.9352 458.5056), (669.8774 462.4900), (669.6273 429.5844)
+0, (760.7551 428.9775), (726.5153 428.5690), (727.0422 461.5150), (761.2695 462.2295)
diff --git a/test/data/34085369442_304b6bafd9_c.txt b/test/data/34085369442_304b6bafd9_c.txt
@@ -1,18 +1,18 @@
-0, 0, (712.8496 321.3379), (695.6104 319.2373), (695.0875 337.7314), (711.6552 339.5857)
-1, 0, (137.1803 350.0015), (137.0391 331.3253), (119.5017 332.1772), (119.3629 350.9057)
-2, 0, (400.1603 282.9774), (400.0085 299.7743), (416.9421 299.6274), (416.6747 283.0110)
-3, 0, (733.9197 326.1527), (751.1302 325.8460), (751.3755 307.5261), (733.9084 307.7999)
-4, 0, (416.5878 352.7041), (431.3354 354.3437), (431.1941 335.7852), (416.3476 334.0768)
-5, 0, (472.2987 271.9427), (472.0169 288.8806), (484.8894 287.0572), (485.0777 270.9519)
-6, 0, (318.5974 389.4719), (300.7015 391.3979), (300.9274 411.2381), (318.8591 409.5875)
-7, 0, (311.2344 343.5884), (328.5275 343.2667), (328.5633 324.8743), (310.5821 324.8871)
-8, 0, (136.6730 406.5262), (136.3904 386.6153), (118.5666 385.0555), (118.4830 405.8007)
-9, 0, (677.7889 302.6428), (677.6320 285.3052), (666.0012 282.7271), (665.6072 300.6304)
-10, 0, (467.2973 288.1754), (467.2424 271.4062), (457.5093 269.1866), (457.6929 286.1717)
-11, 0, (543.4706 350.8587), (543.2719 333.1554), (525.7717 331.5527), (525.3212 350.7736)
-12, 0, (49.6552 357.7258), (63.3771 355.7744), (63.1037 337.2454), (49.7023 338.9751)
-13, 0, (215.9719 413.1197), (235.2605 413.4054), (236.1417 393.1873), (216.0083 392.9312)
-14, 0, (615.2866 276.6579), (615.1252 293.3296), (629.6708 292.3475), (631.6648 274.3273)
-15, 0, (357.4018 346.3447), (373.2110 347.5034), (373.4671 329.2181), (357.1758 328.1275)
-16, 0, (55.9971 395.5411), (75.6599 395.5399), (76.0009 375.6914), (56.1971 375.6695)
-17, 0, (214.3428 345.1931), (230.8877 343.9739), (231.1031 325.5886), (214.2705 326.9748)
+0, (49.6552 357.7258), (63.3771 355.7744), (63.1037 337.2454), (49.7023 338.9751)
+0, (55.9971 395.5411), (75.6599 395.5399), (76.0009 375.6914), (56.1971 375.6695)
+0, (136.6730 406.5262), (136.3904 386.6153), (118.5666 385.0555), (118.4830 405.8007)
+0, (137.1803 350.0015), (137.0391 331.3253), (119.5017 332.1772), (119.3629 350.9057)
+0, (214.3428 345.1931), (230.8877 343.9739), (231.1031 325.5886), (214.2705 326.9748)
+0, (215.9719 413.1197), (235.2605 413.4054), (236.1417 393.1873), (216.0083 392.9312)
+0, (311.2344 343.5884), (328.5275 343.2667), (328.5633 324.8743), (310.5821 324.8871)
+0, (318.5974 389.4719), (300.7015 391.3979), (300.9274 411.2381), (318.8591 409.5875)
+0, (357.4018 346.3447), (373.2110 347.5034), (373.4671 329.2181), (357.1758 328.1275)
+0, (400.1603 282.9774), (400.0085 299.7743), (416.9421 299.6274), (416.6747 283.0110)
+0, (416.5878 352.7041), (431.3354 354.3437), (431.1941 335.7852), (416.3476 334.0768)
+0, (467.2973 288.1754), (467.2424 271.4062), (457.5093 269.1866), (457.6929 286.1717)
+0, (472.2987 271.9427), (472.0169 288.8806), (484.8894 287.0572), (485.0777 270.9519)
+0, (543.4706 350.8587), (543.2719 333.1554), (525.7717 331.5527), (525.3212 350.7736)
+0, (615.2866 276.6579), (615.1252 293.3296), (629.6708 292.3475), (631.6648 274.3273)
+0, (677.7889 302.6428), (677.6320 285.3052), (666.0012 282.7271), (665.6072 300.6304)
+0, (712.8496 321.3379), (695.6104 319.2373), (695.0875 337.7314), (711.6552 339.5857)
+0, (733.9197 326.1527), (751.1302 325.8460), (751.3755 307.5261), (733.9084 307.7999)
diff --git a/test/data/34139872896_defdb2f8d9_c.txt b/test/data/34139872896_defdb2f8d9_c.txt
@@ -1,7 +1,7 @@
-0, 0, (450.7940 281.6134), (445.1442 246.4043), (403.9372 242.6193), (409.3882 277.7529)
-1, 0, (751.6141 416.2980), (708.4382 420.6972), (709.8007 466.8162), (753.4771 462.5127)
-2, 0, (422.5576 450.2083), (421.3539 405.6422), (376.8648 407.7634), (378.0238 452.7328)
-3, 0, (329.0142 399.5628), (285.4985 402.8493), (286.6385 446.9015), (330.8413 443.2896)
-4, 0, (585.3918 383.6006), (587.3239 427.6843), (608.2718 435.5553), (606.5024 391.1288)
-5, 0, (695.4645 419.9278), (676.2806 411.6397), (677.7034 456.7264), (697.6416 466.1422)
-6, 0, (658.1703 429.9575), (657.1567 385.4371), (616.7877 389.5853), (618.0186 434.5197)
+0, (329.0142 399.5628), (285.4985 402.8493), (286.6385 446.9015), (330.8413 443.2896)
+0, (422.5576 450.2083), (421.3539 405.6422), (376.8648 407.7634), (378.0238 452.7328)
+0, (450.7940 281.6134), (445.1442 246.4043), (403.9372 242.6193), (409.3882 277.7529)
+0, (585.3918 383.6006), (587.3239 427.6843), (608.2718 435.5553), (606.5024 391.1288)
+0, (658.1703 429.9575), (657.1567 385.4371), (616.7877 389.5853), (618.0186 434.5197)
+0, (695.4645 419.9278), (676.2806 411.6397), (677.7034 456.7264), (697.6416 466.1422)
+0, (751.6141 416.2980), (708.4382 420.6972), (709.8007 466.8162), (753.4771 462.5127)
diff --git a/test/getline.c b/test/getline.c
@@ -6,7 +6,7 @@
 /* The original code is public domain -- Will Hartung 4/9/09 */
 /* Modifications, public domain as well, by Antti Haapala, 11/10/17 - Switched to getc on 5/23/19 */
 
-ssize_t getline(char **lineptr, size_t *n, FILE *stream) {
+ssize_t apriltag_test_getline(char **lineptr, size_t *n, FILE *stream) {
     size_t pos;
     int c;
 

diff --git a/test/getline.h b/test/getline.h
@@ -6,4 +6,4 @@
 
 typedef intptr_t ssize_t;
 
-ssize_t getline(char **lineptr, size_t *n, FILE *stream);
+ssize_t apriltag_test_getline(char **lineptr, size_t *n, FILE *stream);
diff --git a/test/test_detection.c b/test/test_detection.c
@@ -2,6 +2,7 @@
 #include <apriltag.h>
 #include <tag36h11.h>
 #include <common/pjpeg.h>
+#include <math.h>
 
 #include "getline.h"
 
@@ -37,12 +38,31 @@ format(const char* fmt, ...)
 }
 
 int
-detection_corner_compare_function(const void *_a, const void *_b)
+detection_compare_function(const apriltag_detection_t *a, const apriltag_detection_t *b)
 {
-    apriltag_detection_t *a = *(apriltag_detection_t**) _a;
-    apriltag_detection_t *b = *(apriltag_detection_t**) _b;
+    if (a->id == b->id) {
+        return 0;
+    }
+
+    for (int e = 0; e<4; e++) {
+        for (int c = 0; c<2; c++) {
+            const double d = a->p[e][c] - b->p[e][c];
+            if (fabs(d) > 1e-1) {
+                return copysign(1, d);
+            }
+        }
+    }
+
+    return 0;
+}
 
-    return memcmp(a->p, b->p, sizeof(a));
+int
+detection_array_element_compare_function(const void *_a, const void *_b)
+{
+    const apriltag_detection_t * const a = *(apriltag_detection_t**) _a;
+    const apriltag_detection_t * const b = *(apriltag_detection_t**) _b;
+
+    return detection_compare_function(a, b);
 }
 
 int
@@ -70,28 +90,29 @@ main(int argc, char *argv[])
     apriltag_family_t *tf = tag36h11_create();
     apriltag_detector_add_family(td, tf);
 
-    const char fmt[] = "%i, %i, (%.4lf %.4lf), (%.4lf %.4lf), (%.4lf %.4lf), (%.4lf %.4lf)";
+    const char fmt_det[] = "%i, (%.4lf %.4lf), (%.4lf %.4lf), (%.4lf %.4lf), (%.4lf %.4lf)";
+    const char fmt_ref_parse[] = "%i, (%lf %lf), (%lf %lf), (%lf %lf), (%lf %lf)";
 
     bool ok = true;
 
     zarray_t *detections = apriltag_detector_detect(td, im);
 
     // sort detections by detected corners for deterministic sorting order
-    zarray_sort(detections, detection_corner_compare_function);
+    zarray_sort(detections, detection_array_element_compare_function);
 
     int i = 0;
     for (; i < zarray_size(detections); i++) {
         apriltag_detection_t *det;
         zarray_get(detections, i, &det);
 
-        char* const det_fmt = format(fmt,
-            i, det->id,
+        char* const det_fmt = format(fmt_det,
+            det->id,
             det->p[0][0], det->p[0][1], det->p[1][0], det->p[1][1],
             det->p[2][0], det->p[2][1], det->p[3][0], det->p[3][1]);
 
         char* line = NULL;
         size_t len = 0;
-        const ssize_t nread = getline(&line, &len, fp);
+        const ssize_t nread = apriltag_test_getline(&line, &len, fp);
         if (nread == -1) {
             free(line);
             return EXIT_FAILURE;
@@ -100,8 +121,21 @@ main(int argc, char *argv[])
         printf("Got:      %s\n", det_fmt);
         printf("Expected: %s\n", line);
 
-        // compare strings without the newline character (\n)
-        if (strncmp(det_fmt, line, nread-1) != 0) {
+        // parse reference detection
+        apriltag_detection_t ref;
+        const int nparsed = sscanf(
+            line, fmt_ref_parse,
+            &ref.id,
+            &ref.p[0][0], &ref.p[0][1], &ref.p[1][0], &ref.p[1][1],
+            &ref.p[2][0], &ref.p[2][1], &ref.p[3][0], &ref.p[3][1]);
+
+        (void) nparsed;
+        assert(nparsed == 9);
+
+        // compare detections
+        const bool equ = detection_compare_function(det, &ref) == 0;
+
+        if (!equ || det->id != ref.id) {
             fprintf(stderr, "Mismatch.\nGot:\n  %s\nExpected:\n  %s\n", det_fmt, line);
             ok = false;
         }