diff --git a/Makefile b/Makefile index 6909e562..8f44eed7 100644 --- a/Makefile +++ b/Makefile @@ -70,7 +70,7 @@ clean: rm -f folk *.o vendor/tracy/public/TracyClient.o vendor/c11-queues/*.o distclean: clean make -C vendor/jimtcl distclean - make -C vendor/apriltag clean + rm -rf vendor/apriltag/build remote-clean: sync ssh $(FOLK_REMOTE_NODE) -- 'cd folk; make clean' @@ -81,15 +81,24 @@ deps: cd vendor/jimtcl && ./configure CFLAGS='-g -fno-omit-frame-pointer'; \ fi make -C vendor/jimtcl - make -C vendor/apriltag libapriltag.so + + cmake -B vendor/apriltag/build -S vendor/apriltag -DCMAKE_BUILD_TYPE=Release -DBUILD_SHARED_LIBS=ON + cmake --build vendor/apriltag/build + if [ ! -f vendor/wslay/Makefile ]; then \ cd vendor/wslay && autoreconf -i && automake && autoconf && ./configure; \ fi make -C vendor/wslay + if [ "$$(uname)" = "Darwin" ]; then \ - install_name_tool -id @executable_path/vendor/apriltag/libapriltag.so vendor/apriltag/libapriltag.so; \ + install_name_tool -id @executable_path/vendor/apriltag/build/libapriltag.dylib vendor/apriltag/build/libapriltag.dylib; \ + cd vendor/apriltag/build && ln -sf libapriltag.dylib libapriltag.so; \ install_name_tool -id @executable_path/vendor/wslay/lib/.libs/libwslay.0.dylib vendor/wslay/lib/.libs/libwslay.0.dylib; \ fi + if [ "$$(uname)" = "Linux" ]; then \ + patchelf --set-soname "$$(pwd)/vendor/apriltag/build/libapriltag.so.3" vendor/apriltag/build/libapriltag.so.3; \ + patchelf --set-soname "$$(pwd)/vendor/wslay/lib/.libs/libwslay.so.0" vendor/wslay/lib/.libs/libwslay.so.0 || true; \ + fi kill-folk: sudo systemctl stop folk diff --git a/README.md b/README.md index c9608c38..6070c13d 100644 --- a/README.md +++ b/README.md @@ -66,7 +66,7 @@ if flashing from a Mac] -- Ubuntu doesn't have a good kernel for Pi 5) `folk@folk-WHATEVER.local` by name, `sudo apt install avahi-daemon` and then on your laptop: `ssh-copy-id folk@folk-WHATEVER.local` -1. Install dependencies: `sudo apt install rsync git libturbojpeg0-dev libpng-dev libdrm-dev pkg-config v4l-utils vulkan-tools libvulkan-dev libvulkan1 meson libgbm-dev glslc vulkan-validationlayers ghostscript console-data kbd psmisc zlib1g-dev libssl-dev automake libtool autoconf-archive` +1. Install dependencies: `sudo apt install rsync git cmake patchelf libturbojpeg0-dev libpng-dev libdrm-dev pkg-config v4l-utils vulkan-tools libvulkan-dev libvulkan1 meson libgbm-dev glslc vulkan-validationlayers ghostscript console-data kbd psmisc zlib1g-dev libssl-dev automake libtool autoconf-archive` (When prompted while installing `console-data` for `Policy for handling keymaps` type `3` (meaning `3. Keep kernel keymap`) and diff --git a/builtin-programs/apriltags.folk b/builtin-programs/apriltags.folk index bbf08dd8..3cdceb8a 100644 --- a/builtin-programs/apriltags.folk +++ b/builtin-programs/apriltags.folk @@ -5,7 +5,7 @@ fn makeAprilTagDetector {TAG_FAMILY QUAD_DECIMATE NTHREADS} { $cc extend $imageLib $cc cflags -I./vendor/apriltag - $cc endcflags ./vendor/apriltag/libapriltag.so + $cc endcflags ./vendor/apriltag/build/libapriltag.so $cc include $cc include <$TAG_FAMILY.h> $cc include @@ -18,7 +18,7 @@ fn makeAprilTagDetector {TAG_FAMILY QUAD_DECIMATE NTHREADS} { $cc proc detectInit {} void { td = apriltag_detector_create(); tf = ${TAG_FAMILY}_create(); - apriltag_detector_add_family_bits(td, tf, 1); + apriltag_detector_add_family_bits(td, tf, 3); td->quad_decimate = ${QUAD_DECIMATE}; td->nthreads = ${NTHREADS}; } diff --git a/builtin-programs/calibrate/matlib.folk b/builtin-programs/calibrate/matlib.folk index 79289644..583794c8 100644 --- a/builtin-programs/calibrate/matlib.folk +++ b/builtin-programs/calibrate/matlib.folk @@ -245,7 +245,7 @@ $cc proc applyHomography {matd_t* H double[2] xy} Jim_Obj* { $cc proc matdMul {matd_t* a matd_t* b} matd_t* { return matd_multiply(a, b); } -$cc endcflags ./vendor/apriltag/libapriltag.so +$cc endcflags ./vendor/apriltag/build/libapriltag.so set matLib [$cc compile] Claim the calibration matLib is $matLib diff --git a/builtin-programs/calibrate/refine.folk b/builtin-programs/calibrate/refine.folk index 1e46a6a9..7f701176 100644 --- a/builtin-programs/calibrate/refine.folk +++ b/builtin-programs/calibrate/refine.folk @@ -61,7 +61,7 @@ set NUM_TAGS_IN_MODEL 20 set cc [C] $cc cflags -I./vendor/cmpfit -I./vendor/apriltag -Wall -Werror -$cc endcflags ./vendor/apriltag/libapriltag.so +$cc endcflags ./vendor/apriltag/build/libapriltag.so $cc include $cc include diff --git a/builtin-programs/print.folk b/builtin-programs/print.folk index 4b6b6c82..7c10acf6 100644 --- a/builtin-programs/print.folk +++ b/builtin-programs/print.folk @@ -23,7 +23,7 @@ When the image library is /imageLib/ &\ set cc [C] $cc extend $imageLib $cc cflags -I./vendor/apriltag -Wall -Werror -$cc endcflags ./vendor/apriltag/libapriltag.so +$cc endcflags ./vendor/apriltag/build/libapriltag.so $cc code { #include diff --git a/builtin-programs/tags-to-quads.folk b/builtin-programs/tags-to-quads.folk index 495962a6..4f9f124e 100644 --- a/builtin-programs/tags-to-quads.folk +++ b/builtin-programs/tags-to-quads.folk @@ -70,7 +70,7 @@ $cc proc rescaleAndUndistort {Intrinsics intr out[1] = y*intr.fy + intr.cy; } $cc cflags -I./vendor/apriltag -$cc endcflags ./vendor/apriltag/libapriltag.so +$cc endcflags ./vendor/apriltag/build/libapriltag.so $cc include $cc include @@ -78,7 +78,10 @@ $cc include $cc include $cc code { -#define MATD_VAR(name, nr, nc) matd_t* name = alloca(sizeof(matd_t) + ((nr)*(nc)*sizeof(double))); name->nrows = nr; name->ncols = nc; + #define MATD_VAR(name, nr, nc) \ + matd_t* name = alloca(sizeof(matd_t)); \ + name->data = alloca((nr)*(nc)*sizeof(double)); \ + name->nrows = nr; name->ncols = nc; } $cc proc pseudoInverse {matd_t* a} matd_t* { matd_svd_t usv = matd_svd(a); diff --git a/vendor/README.md b/vendor/README.md index 7d394eca..23e92330 100644 --- a/vendor/README.md +++ b/vendor/README.md @@ -12,3 +12,4 @@ Some of these packages have been modified by us. - `vk_mem_alloc` is from - idiomorph is from +- apriltag is from diff --git a/vendor/apriltag/.github/workflows/bloom.yml b/vendor/apriltag/.github/workflows/bloom.yml new file mode 100644 index 00000000..f74f0c0b --- /dev/null +++ b/vendor/apriltag/.github/workflows/bloom.yml @@ -0,0 +1,53 @@ +name: bloom + +on: [push, pull_request] + +jobs: + build_linux: + name: "Ubuntu (${{ matrix.ros_distribution }})" + + runs-on: ubuntu-latest + + strategy: + matrix: + include: + - docker_image: ubuntu:20.04 + ros_distribution: noetic + + - docker_image: ubuntu:22.04 + ros_distribution: humble + + - docker_image: ubuntu:24.04 + ros_distribution: jazzy + + container: + image: ${{ matrix.docker_image }} + + env: + DEBIAN_FRONTEND: noninteractive + + steps: + - name: install core dependencies + run: | + apt update + apt install -y --no-install-recommends git ca-certificates + + - uses: actions/checkout@v4 + + - uses: ros-tooling/setup-ros@v0.7 + + - name: install build tool dependencies + run: | + apt install -y --no-install-recommends devscripts equivs python3-bloom + + - name: bloom + run: | + rosdep update + bloom-generate rosdebian --ros-distro ${{ matrix.ros_distribution }} + mk-build-deps + apt install -y --no-install-recommends ./ros-${{ matrix.ros_distribution }}-apriltag-build-deps_*_all.deb + dpkg-buildpackage -b + + - name: install bloomed packages + run: | + apt install -y --no-install-recommends ../ros-${{ matrix.ros_distribution }}-apriltag_*.deb ../ros-${{ matrix.ros_distribution }}-apriltag-dbgsym_*.ddeb diff --git a/vendor/apriltag/.github/workflows/cmake-multi-platform.yml b/vendor/apriltag/.github/workflows/cmake-multi-platform.yml index 5f735016..1a0f6aa6 100644 --- a/vendor/apriltag/.github/workflows/cmake-multi-platform.yml +++ b/vendor/apriltag/.github/workflows/cmake-multi-platform.yml @@ -1,6 +1,4 @@ -# This starter workflow is for a CMake project running on multiple platforms. There is a different starter workflow if you just want a single platform. -# See: https://github.com/actions/starter-workflows/blob/main/ci/cmake-single-platform.yml -name: CMake on multiple platforms +name: CMake on: push: @@ -13,19 +11,11 @@ jobs: runs-on: ${{ matrix.os }} strategy: - # Set fail-fast to false to ensure that feedback is delivered for all matrix combinations. Consider changing this to true when your workflow is stable. - fail-fast: false - - # Set up a matrix to run the following 3 configurations: - # 1. - # 2. - # 3. - # - # To add more build types (Release, Debug, RelWithDebInfo, etc.) customize the build_type list. matrix: os: [ubuntu-latest, windows-latest, macos-latest] build_type: [Release] c_compiler: [gcc, clang, cl] + shared_libs: ['ON', 'OFF'] include: - os: windows-latest c_compiler: cl @@ -45,20 +35,18 @@ jobs: - os: macos-latest c_compiler: clang cpp_compiler: clang++ - - os: macos-latest - c_compiler: gcc - cpp_compiler: g++ exclude: - os: ubuntu-latest c_compiler: cl - os: macos-latest c_compiler: cl + - os: macos-latest + c_compiler: gcc steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Set reusable strings - # Turn repeated input strings (such as the build output directory) into step outputs. These step outputs can be used throughout the workflow file. id: strings shell: bash run: | @@ -68,23 +56,46 @@ jobs: - uses: ilammy/msvc-dev-cmd@v1 + - uses: actions/setup-python@v5 + with: + python-version: '3.10' + + - run: pip install numpy + - name: Configure CMake - # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. - # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type run: > cmake -B ${{ steps.strings.outputs.build-output-dir }} -G Ninja - -DCMAKE_CXX_COMPILER=${{ matrix.cpp_compiler }} - -DCMAKE_C_COMPILER=${{ matrix.c_compiler }} - -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} + -D CMAKE_CXX_COMPILER=${{ matrix.cpp_compiler }} + -D CMAKE_C_COMPILER=${{ matrix.c_compiler }} + -D CMAKE_BUILD_TYPE=${{ matrix.build_type }} + -D BUILD_SHARED_LIBS=${{ matrix.shared_libs }} + -D BUILD_TESTING=ON -S ${{ github.workspace }} - name: Build - # Build your program with the given configuration. Note that --config is needed because the default Windows generator is a multi-config generator (Visual Studio generator). run: cmake --build ${{ steps.strings.outputs.build-output-dir }} --config ${{ matrix.build_type }} + - name: Install (Windows) + if: matrix.os == 'windows-latest' + run: Start-Process -Verb RunAs -FilePath cmake "--build ${{ steps.strings.outputs.build-output-dir }} --config ${{ matrix.build_type }} --target install" + + - name: Install (sudo) + if: matrix.os != 'windows-latest' + run: sudo cmake --build ${{ steps.strings.outputs.build-output-dir }} --config ${{ matrix.build_type }} --target install + + - name: add DLL to test folder + if: matrix.os == 'windows-latest' + working-directory: ${{ steps.strings.outputs.build-output-dir }} + run: | + cp *apriltag.dll test/ + - name: Test working-directory: ${{ steps.strings.outputs.build-output-dir }} - # Execute tests defined by the CMake configuration. Note that --build-config is needed because the default Windows generator is a multi-config generator (Visual Studio generator). - # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail - run: ctest --build-config ${{ matrix.build_type }} + run: | + ctest --build-config ${{ matrix.build_type }} --no-tests=error --output-on-failure --verbose + + - name: Test Python Module Import + working-directory: ${{ steps.strings.outputs.build-output-dir }} + run: | + python3 -c "import apriltag; apriltag.apriltag(family='tag36h11')" diff --git a/vendor/apriltag/.github/workflows/cmake-ubuntu.yml b/vendor/apriltag/.github/workflows/cmake-ubuntu.yml new file mode 100644 index 00000000..15d8a545 --- /dev/null +++ b/vendor/apriltag/.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 }} + run: | + ctest --no-tests=error --output-on-failure --verbose diff --git a/vendor/apriltag/.github/workflows/colcon-workspace.yml b/vendor/apriltag/.github/workflows/colcon-workspace.yml index ca37f4b3..a067d97d 100644 --- a/vendor/apriltag/.github/workflows/colcon-workspace.yml +++ b/vendor/apriltag/.github/workflows/colcon-workspace.yml @@ -5,14 +5,12 @@ on: [push, pull_request] jobs: # build on Ubuntu docker images build_linux: - name: "Ubuntu (${{ matrix.ros_distribution }}, shared: ${{ matrix.cmake_shared_libs }})" + name: "${{ matrix.docker_image }} (${{ matrix.ros_distribution }})" runs-on: ubuntu-latest strategy: matrix: - ros_distribution: [noetic, humble] - cmake_shared_libs: ['ON', 'OFF'] include: - docker_image: ubuntu:20.04 ros_distribution: noetic @@ -22,95 +20,56 @@ jobs: ros_distribution: humble ros_version: 2 - container: - image: ${{ matrix.docker_image }} - - steps: - - name: install core dependencies - run: | - apt update - apt install --no-install-recommends -y git ca-certificates - - - uses: actions/checkout@v4 - - - name: Setup ROS environment - uses: ros-tooling/setup-ros@v0.7 - - - name: ROS 1 CI Action - if: ${{ matrix.ros_version == 1 }} - uses: ros-tooling/action-ros-ci@v0.3 - with: - package-name: apriltag - target-ros1-distro: ${{ matrix.ros_distribution }} - extra-cmake-args: "-DBUILD_SHARED_LIBS=${{ matrix.cmake_shared_libs }}" - - - name: ROS 2 CI Action - if: ${{ matrix.ros_version == 2 }} - uses: ros-tooling/action-ros-ci@v0.3 - with: - package-name: apriltag - target-ros2-distro: ${{ matrix.ros_distribution }} - extra-cmake-args: "-DBUILD_SHARED_LIBS=${{ matrix.cmake_shared_libs }}" - - - # build on Windows native - build_windows: - name: "Windows (${{ matrix.ros_distribution }}, shared: ${{ matrix.cmake_shared_libs }})" - - runs-on: windows-2019 + - docker_image: ubuntu:24.04 + ros_distribution: jazzy + ros_version: 2 - strategy: - matrix: - ros_distribution: [noetic, humble] - cmake_shared_libs: ['OFF'] - include: - - ros_distribution: noetic - ros_version: 1 + - docker_image: almalinux:8 + ros_distribution: humble + ros_version: 2 - - ros_distribution: humble + - docker_image: almalinux:9 + ros_distribution: jazzy ros_version: 2 + container: + image: ${{ matrix.docker_image }} + steps: - - uses: actions/checkout@v4 - with: - submodules: recursive + - uses: actions/checkout@v5 - name: Setup ROS environment uses: ros-tooling/setup-ros@v0.7 - name: ROS 1 CI Action if: ${{ matrix.ros_version == 1 }} - uses: ros-tooling/action-ros-ci@v0.3 + uses: ros-tooling/action-ros-ci@v0.4 with: package-name: apriltag target-ros1-distro: ${{ matrix.ros_distribution }} - extra-cmake-args: "-DBUILD_SHARED_LIBS=${{ matrix.cmake_shared_libs }}" - name: ROS 2 CI Action if: ${{ matrix.ros_version == 2 }} - uses: ros-tooling/action-ros-ci@v0.3 + uses: ros-tooling/action-ros-ci@v0.4 with: package-name: apriltag target-ros2-distro: ${{ matrix.ros_distribution }} - extra-cmake-args: "-DBUILD_SHARED_LIBS=${{ matrix.cmake_shared_libs }}" + # build on macOS native build_macos: - name: "macOS (${{ matrix.ros_distribution }}, shared: ${{ matrix.cmake_shared_libs }})" + name: "macOS (${{ matrix.ros_distribution }})" runs-on: macos-latest strategy: matrix: - ros_distribution: [humble] - cmake_shared_libs: ['OFF'] + ros_distribution: [humble, jazzy] steps: - - uses: actions/checkout@v4 - with: - submodules: recursive + - uses: actions/checkout@v5 - - uses: actions/setup-python@v4 + - uses: actions/setup-python@v5 with: python-version: "3.10" @@ -118,8 +77,7 @@ jobs: uses: ros-tooling/setup-ros@v0.7 - name: ROS 2 CI Action - uses: ros-tooling/action-ros-ci@v0.3 + uses: ros-tooling/action-ros-ci@v0.4 with: package-name: apriltag target-ros2-distro: ${{ matrix.ros_distribution }} - extra-cmake-args: "-DBUILD_SHARED_LIBS=${{ matrix.cmake_shared_libs }}" diff --git a/vendor/apriltag/.github/workflows/cross-compilation.yml b/vendor/apriltag/.github/workflows/cross-compilation.yml new file mode 100644 index 00000000..c9d08ac0 --- /dev/null +++ b/vendor/apriltag/.github/workflows/cross-compilation.yml @@ -0,0 +1,102 @@ +name: Cross compilation + +on: + push: + branches: [ "master" ] + pull_request: + branches: [ "master" ] + workflow_dispatch: + +jobs: + # Use mingw to cross compile apriltag and its tests on linux + build: + runs-on: ubuntu-latest + + strategy: + matrix: + shared_libs: ['ON','OFF'] + + steps: + # Install required packages to build for windows + - name: install dependencies + run: | + sudo apt update + sudo apt install -y --no-install-recommends cmake ninja-build gcc-mingw-w64-x86-64 + + # Get the sources + - uses: actions/checkout@v4 + + # Save the "build/" folder path + - name: Set reusable strings + id: strings + shell: bash + run: | + echo "build-output-dir=$GITHUB_WORKSPACE/build" >> "$GITHUB_OUTPUT" + + # Configure cmake + # CMAKE_SYSTEM_NAME=Windows is needed else it will produce libapriltag.so instead of .dll + - name: Configure CMake + run: > + cmake -B ${{ steps.strings.outputs.build-output-dir }} + -G Ninja + -D CMAKE_SYSTEM_NAME=Windows + -D CMAKE_C_COMPILER=/usr/bin/x86_64-w64-mingw32-gcc + -D CMAKE_BUILD_TYPE=Release + -D BUILD_SHARED_LIBS=${{ matrix.shared_libs }} + -D BUILD_TESTING=ON + -S $GITHUB_WORKSPACE + + # Build apriltag + - name: Build + run: cmake --build ${{ steps.strings.outputs.build-output-dir }} + + # In case of shared lib, copy the .dll file in the test directory + - name: add DLL to test folder + if: matrix.shared_libs == 'ON' + working-directory: ${{ steps.strings.outputs.build-output-dir }} + run: | + cp *apriltag.dll test/ + + # Upload the "build/" directory as artifact + - name: Upload build artifact + uses: actions/upload-artifact@v4 + with: + name: cross-compilation-build-sharedlib-${{ matrix.shared_libs }} + path: ${{ steps.strings.outputs.build-output-dir }} + + # On windows, retrieve the cross compiled build and run the tests + test: + needs: build + runs-on: windows-latest + + strategy: + matrix: + shared_libs: ['ON','OFF'] + + steps: + - uses: actions/checkout@v4 + + # Save the "build/" folder path + - name: Set reusable strings + id: strings + shell: bash + run: | + echo "build-output-dir=$GITHUB_WORKSPACE/build" >> "$GITHUB_OUTPUT" + + # Download the "build/" directory + - name: Download build artifact + uses: actions/download-artifact@v4 + with: + name: cross-compilation-build-sharedlib-${{ matrix.shared_libs }} + path: ${{ steps.strings.outputs.build-output-dir }} + + # Change the tests pathes so they match the windows ones instead of the linux ones + - name: Change test pathes + run: | + (Get-Content ${{ steps.strings.outputs.build-output-dir }}/test/CTestTestfile.cmake) -replace '/home/runner/work/apriltag/apriltag', $PWD.Path.Replace('\', '/') | Set-Content ${{ steps.strings.outputs.build-output-dir }}/test/CTestTestfile.cmake + + # Run the tests + - name: Test + working-directory: ${{ steps.strings.outputs.build-output-dir }} + run: | + ctest --no-tests=error --output-on-failure --verbose diff --git a/vendor/apriltag/CMake/vtkEncodeString.cmake b/vendor/apriltag/CMake/vtkEncodeString.cmake new file mode 100644 index 00000000..25a332d2 --- /dev/null +++ b/vendor/apriltag/CMake/vtkEncodeString.cmake @@ -0,0 +1,279 @@ +#[==[ + +Copyright (c) 1993-2015 Ken Martin, Will Schroeder, Bill Lorensen +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither name of Ken Martin, Will Schroeder, or Bill Lorensen nor the names + of any contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#]==] + +#[==[ +@file vtkEncodeString.cmake + +This module contains the @ref vtk_encode_string function which may be used to +turn a file into a C string. This is primarily used within a program so that +the content does not need to be retrieved from the filesystem at runtime, but +can still be developed as a standalone file. +#]==] + +set(_vtkEncodeString_script_file "${CMAKE_CURRENT_LIST_FILE}") + +#[==[ +@brief Encode a file as a C string at build time + +Adds a rule to turn a file into a C string. Note that any Unicode characters +will not be replaced with escaping, so it is recommended to avoid their usage +in the input. + +~~~ +vtk_encode_string( + INPUT + [NAME ] + [EXPORT_SYMBOL ] + [EXPORT_HEADER
] + [HEADER_OUTPUT ] + [SOURCE_OUTPUT ] + + [ABI_MANGLE_SYMBOL_BEGIN ] + [ABI_MANGLE_SYMBOL_END ] + [ABI_MANGLE_HEADER
] + + [BINARY] [NUL_TERMINATE]) +~~~ + +The only required variable is `INPUT`, however, it is likely that at least one +of `HEADER_OUTPUT` or `SOURCE_OUTPUT` will be required to add them to a +library. + + * `INPUT`: (Required) The path to the file to be embedded. If a relative path + is given, it will be interpreted as being relative to + `CMAKE_CURRENT_SOURCE_DIR`. + * `NAME`: This is the base name of the files that will be generated as well + as the variable name for the C string. It defaults to the basename of the + input without extensions. + * `EXPORT_SYMBOL`: The symbol to use for exporting the variable. By default, + it will not be exported. If set, `EXPORT_HEADER` must also be set. + * `EXPORT_HEADER`: The header to include for providing the given export + symbol. If set, `EXPORT_SYMBOL` should also be set. + * `HEADER_OUTPUT`: The variable to store the generated header path. + * `SOURCE_OUTPUT`: The variable to store the generated source path. + * `BINARY`: If given, the data will be written as an array of `unsigned char` + bytes. + * `NUL_TERMINATE`: If given, the binary data will be `NUL`-terminated. Only + makes sense with the `BINARY` flag. This is intended to be used if + embedding a file as a C string exceeds compiler limits on string literals + in various compilers. + * `ABI_MANGLE_SYMBOL_BEGIN`: Open a mangling namespace with the given symbol. + If given, `ABI_MANGLE_SYMBOL_END` and `ABI_MANGLE_HEADER` must also be set. + * `ABI_MANGLE_SYMBOL_END`: Close a mangling namespace with the given symbol. + If given, `ABI_MANGLE_SYMBOL_BEGIN` and `ABI_MANGLE_HEADER` must also be set. + * `ABI_MANGLE_HEADER`: The header which provides the ABI mangling symbols. + If given, `ABI_MANGLE_SYMBOL_BEGIN` and `ABI_MANGLE_SYMBOL_END` must also + be set. +#]==] +function (vtk_encode_string) + cmake_parse_arguments(PARSE_ARGV 0 _vtk_encode_string + "BINARY;NUL_TERMINATE" + "INPUT;NAME;EXPORT_SYMBOL;EXPORT_HEADER;HEADER_OUTPUT;SOURCE_OUTPUT;ABI_MANGLE_SYMBOL_BEGIN;ABI_MANGLE_SYMBOL_END;ABI_MANGLE_HEADER" + "") + + if (_vtk_encode_string_UNPARSED_ARGUMENTS) + message(FATAL_ERROR + "Unrecognized arguments to vtk_encode_string: " + "${_vtk_encode_string_UNPARSED_ARGUMENTS}") + endif () + + if (NOT DEFINED _vtk_encode_string_INPUT) + message(FATAL_ERROR + "Missing `INPUT` for vtk_encode_string.") + endif () + + if (NOT DEFINED _vtk_encode_string_NAME) + get_filename_component(_vtk_encode_string_NAME + "${_vtk_encode_string_INPUT}" NAME_WE) + endif () + + if (DEFINED _vtk_encode_string_EXPORT_SYMBOL AND + NOT DEFINED _vtk_encode_string_EXPORT_HEADER) + message(FATAL_ERROR + "Missing `EXPORT_HEADER` when using `EXPORT_SYMBOL`.") + endif () + + if (DEFINED _vtk_encode_string_EXPORT_HEADER AND + NOT DEFINED _vtk_encode_string_EXPORT_SYMBOL) + message(WARNING + "Missing `EXPORT_SYMBOL` when using `EXPORT_HEADER`.") + endif () + + if (DEFINED _vtk_encode_string_ABI_MANGLE_SYMBOL_BEGIN AND + (NOT DEFINED _vtk_encode_string_ABI_MANGLE_SYMBOL_END OR + NOT DEFINED _vtk_encode_string_ABI_MANGLE_HEADER)) + message(WARNING + "Missing `ABI_MANGLE_SYMBOL_END` or `ABI_MANGLE_HEADER` when using " + "`ABI_MANGLE_SYMBOL_BEGIN`.") + endif () + + if (DEFINED _vtk_encode_string_ABI_MANGLE_SYMBOL_END AND + (NOT DEFINED _vtk_encode_string_ABI_MANGLE_SYMBOL_BEGIN OR + NOT DEFINED _vtk_encode_string_ABI_MANGLE_HEADER)) + message(WARNING + "Missing `ABI_MANGLE_SYMBOL_BEGIN` or `ABI_MANGLE_HEADER` when using " + "`ABI_MANGLE_SYMBOL_END`.") + endif () + + if (DEFINED _vtk_encode_string_ABI_MANGLE_HEADER AND + (NOT DEFINED _vtk_encode_string_ABI_MANGLE_SYMBOL_BEGIN OR + NOT DEFINED _vtk_encode_string_ABI_MANGLE_SYMBOL_END)) + message(WARNING + "Missing `ABI_MANGLE_SYMBOL_BEGIN` or `ABI_MANGLE_SYMBOL_END` when " + "using `ABI_MANGLE_HEADER`.") + endif () + + if (NOT _vtk_encode_string_BINARY AND _vtk_encode_string_NUL_TERMINATE) + message(FATAL_ERROR + "The `NUL_TERMINATE` flag only makes sense with the `BINARY` flag.") + endif () + + set(_vtk_encode_string_header + "${CMAKE_CURRENT_BINARY_DIR}/${_vtk_encode_string_NAME}.h") + set(_vtk_encode_string_source + "${CMAKE_CURRENT_BINARY_DIR}/${_vtk_encode_string_NAME}.cxx") + + if (IS_ABSOLUTE "${_vtk_encode_string_INPUT}") + set(_vtk_encode_string_input + "${_vtk_encode_string_INPUT}") + else () + set(_vtk_encode_string_input + "${CMAKE_CURRENT_SOURCE_DIR}/${_vtk_encode_string_INPUT}") + endif () + + set(_vtk_encode_string_depends_args) + if (CMAKE_VERSION VERSION_GREATER_EQUAL "3.27") + list(APPEND _vtk_encode_string_depends_args + DEPENDS_EXPLICIT_ONLY) + endif () + + add_custom_command( + OUTPUT ${_vtk_encode_string_header} + ${_vtk_encode_string_source} + DEPENDS "${_vtkEncodeString_script_file}" + "${_vtk_encode_string_input}" + COMMAND "${CMAKE_COMMAND}" + "-Dsource_dir=${CMAKE_CURRENT_SOURCE_DIR}" + "-Dbinary_dir=${CMAKE_CURRENT_BINARY_DIR}" + "-Dsource_file=${_vtk_encode_string_input}" + "-Doutput_name=${_vtk_encode_string_NAME}" + "-Dexport_symbol=${_vtk_encode_string_EXPORT_SYMBOL}" + "-Dexport_header=${_vtk_encode_string_EXPORT_HEADER}" + "-Dabi_mangle_symbol_begin=${_vtk_encode_string_ABI_MANGLE_SYMBOL_BEGIN}" + "-Dabi_mangle_symbol_end=${_vtk_encode_string_ABI_MANGLE_SYMBOL_END}" + "-Dabi_mangle_header=${_vtk_encode_string_ABI_MANGLE_HEADER}" + "-Dbinary=${_vtk_encode_string_BINARY}" + "-Dnul_terminate=${_vtk_encode_string_NUL_TERMINATE}" + "-D_vtk_encode_string_run=ON" + -P "${_vtkEncodeString_script_file}" + ${_vtk_encode_string_depends_args}) + + if (DEFINED _vtk_encode_string_SOURCE_OUTPUT) + set("${_vtk_encode_string_SOURCE_OUTPUT}" + "${_vtk_encode_string_source}" + PARENT_SCOPE) + endif () + + if (DEFINED _vtk_encode_string_HEADER_OUTPUT) + set("${_vtk_encode_string_HEADER_OUTPUT}" + "${_vtk_encode_string_header}" + PARENT_SCOPE) + endif () +endfunction () + +if (_vtk_encode_string_run AND CMAKE_SCRIPT_MODE_FILE) + set(output_source "${binary_dir}/${output_name}.h") + + set(license_topfile "// SPDX-FileCopyrightText: Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen\n// SPDX-License-Identifier: BSD-3-Clause\n") + file(WRITE "${output_source}" ${license_topfile}) + + if (IS_ABSOLUTE "${source_file}") + set(source_file_full "${source_file}") + else () + set(source_file_full "${source_dir}/${source_file}") + endif () + set(hex_arg) + if (binary) + set(hex_arg HEX) + endif () + file(READ "${source_file_full}" original_content ${hex_arg}) + + if (binary) + if (nul_terminate) + string(APPEND original_content "00") + endif () + string(LENGTH "${original_content}" output_size) + math(EXPR output_size "${output_size} / 2") + + file(APPEND "${output_source}" + "#include \"${output_name}.h\"\n\n") + if (abi_mangle_symbol_begin) + file(APPEND "${output_source}" + "${abi_mangle_symbol_begin}\n\n") + endif () + file(APPEND "${output_source}" + "const unsigned char ${output_name}[${output_size}] = {\n") + string(REGEX REPLACE "\([0-9a-f][0-9a-f]\)" ",0x\\1" escaped_content "${original_content}") + # Hard line wrap the file. + string(REGEX REPLACE "\(..........................................................................,\)" "\\1\n" escaped_content "${escaped_content}") + # Remove the leading comma. + string(REGEX REPLACE "^," "" escaped_content "${escaped_content}") + file(APPEND "${output_source}" + "${escaped_content}\n") + file(APPEND "${output_source}" + "};\n") + if (abi_mangle_symbol_end) + file(APPEND "${output_source}" + "${abi_mangle_symbol_end}\n") + endif () + else () + # Escape literal backslashes. + string(REPLACE "\\" "\\\\" escaped_content "${original_content}") + # Escape literal double quotes. + string(REPLACE "\"" "\\\"" escaped_content "${escaped_content}") + # Turn newlines into newlines in the C string. + string(REPLACE "\n" "\\n\"\n\"" escaped_content "${escaped_content}") + + if (abi_mangle_symbol_begin) + file(APPEND "${output_source}" + "${abi_mangle_symbol_begin}\n\n") + endif () + file(APPEND "${output_source}" + "const char ${output_name}[] =\n") + file(APPEND "${output_source}" + "\"${escaped_content}\";\n") + if (abi_mangle_symbol_end) + file(APPEND "${output_source}" + "\n${abi_mangle_symbol_end}\n") + endif () + endif () +endif () diff --git a/vendor/apriltag/CMakeLists.txt b/vendor/apriltag/CMakeLists.txt index f8ff30c9..5b238324 100644 --- a/vendor/apriltag/CMakeLists.txt +++ b/vendor/apriltag/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(apriltag VERSION 3.3.0 LANGUAGES C CXX) +project(apriltag VERSION 3.4.5 LANGUAGES C) if(POLICY CMP0077) cmake_policy(SET CMP0077 NEW) @@ -34,6 +34,10 @@ set(default_build_type "Release") SET(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) +if(WIN32) + add_compile_definitions(WIN32_LEAN_AND_MEAN) +endif() + if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) message(STATUS "Setting build type to '${default_build_type}' as none was specified.") set(CMAKE_BUILD_TYPE "${default_build_type}" CACHE STRING "Choose the type of build." FORCE) @@ -42,13 +46,21 @@ if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) endif() if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_C_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Werror) - # add_compile_options(-Wpedantic) + add_compile_options(-Wall -Wextra) + add_compile_options(-Wpedantic) + if(CMAKE_C_COMPILER_ID MATCHES "Clang") + add_compile_options( + -Wno-gnu-zero-variadic-macro-arguments + -Wno-strict-prototypes + -Wno-static-in-inline + ) + endif() add_compile_options(-Wno-shift-negative-value) -endif() - -if(CMAKE_C_COMPILER_ID MATCHES "Clang" AND NOT CMAKE_C_COMPILER_ID MATCHES "AppleClang" AND NOT CMAKE_C_SIMULATE_ID MATCHES "MSVC") - add_link_options("-Wl,-z,relro,-z,now,-z,defs") + if(${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.24.0") + set(CMAKE_COMPILE_WARNING_AS_ERROR ON) + else() + add_compile_options(-Werror) + endif() endif() if(CMAKE_C_COMPILER_ID MATCHES "Clang" AND CMAKE_C_SIMULATE_ID MATCHES "MSVC") @@ -65,6 +77,11 @@ set(APRILTAG_SRCS apriltag.c apriltag_pose.c apriltag_quad_thresh.c) # Library file(GLOB TAG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/tag*.c) add_library(${PROJECT_NAME} ${APRILTAG_SRCS} ${COMMON_SRC} ${TAG_FILES}) +set_property(TARGET ${PROJECT_NAME} PROPERTY POSITION_INDEPENDENT_CODE ON) + +if(CMAKE_C_COMPILER_ID MATCHES "Clang" AND NOT APPLE AND NOT CMAKE_C_SIMULATE_ID MATCHES "MSVC") + target_link_options(${PROJECT_NAME} PRIVATE "-Wl,-z,relro,-z,now,-z,defs") +endif() if (MSVC) add_compile_definitions("_CRT_SECURE_NO_WARNINGS") @@ -79,6 +96,8 @@ endif() set_target_properties(${PROJECT_NAME} PROPERTIES SOVERSION 3 VERSION ${PROJECT_VERSION}) set_target_properties(${PROJECT_NAME} PROPERTIES DEBUG_POSTFIX "d") +set_property(TARGET ${PROJECT_NAME} PROPERTY C_STANDARD 99) + include(GNUInstallDirs) target_include_directories(${PROJECT_NAME} PUBLIC @@ -101,7 +120,7 @@ set(generated_dir "${CMAKE_CURRENT_BINARY_DIR}/generated") set(version_config "${generated_dir}/${PROJECT_NAME}ConfigVersion.cmake") set(project_config "${generated_dir}/${PROJECT_NAME}Config.cmake") set(targets_export_name "${PROJECT_NAME}Targets") -set(config_install_dir "share/${PROJECT_NAME}/cmake") +set(config_install_dir "${CMAKE_INSTALL_LIBDIR}/${PROJECT_NAME}/cmake") # Include module with fuction 'write_basic_package_version_file' include(CMakePackageConfigHelpers) @@ -141,52 +160,48 @@ export(TARGETS apriltag # install pkgconfig file configure_file(${PROJECT_NAME}.pc.in ${PROJECT_NAME}.pc @ONLY) -install(FILES "${CMAKE_BINARY_DIR}/${PROJECT_NAME}.pc" +install(FILES "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.pc" DESTINATION "${CMAKE_INSTALL_LIBDIR}/pkgconfig") # Python wrapper -include(CMakeDependentOption) -cmake_dependent_option(BUILD_PYTHON_WRAPPER "Builds Python wrapper" ON BUILD_SHARED_LIBS OFF) - -if(BUILD_PYTHON_WRAPPER) - SET(Python_ADDITIONAL_VERSIONS 3) - find_package(PythonLibs) - execute_process(COMMAND which python3 OUTPUT_QUIET RESULT_VARIABLE Python3_NOT_FOUND) - execute_process(COMMAND python3 -c "import numpy" RESULT_VARIABLE Numpy_NOT_FOUND) -endif(BUILD_PYTHON_WRAPPER) - -if (NOT Python3_NOT_FOUND AND NOT Numpy_NOT_FOUND AND PYTHONLIBS_FOUND AND BUILD_PYTHON_WRAPPER) - # TODO deal with both python2/3 - execute_process(COMMAND python3 ${CMAKE_CURRENT_SOURCE_DIR}/python_build_flags.py OUTPUT_VARIABLE PY_OUT) - set(PY_VARS CFLAGS LDFLAGS LINKER EXT_SUFFIX) - cmake_parse_arguments(PY "" "${PY_VARS}" "" ${PY_OUT}) - separate_arguments(PY_CFLAGS) - list(REMOVE_ITEM PY_CFLAGS -flto) - separate_arguments(PY_LDFLAGS) - - foreach(X detect py_type) - add_custom_command(OUTPUT ${PROJECT_BINARY_DIR}/apriltag_${X}.docstring.h - COMMAND < ${CMAKE_CURRENT_SOURCE_DIR}/apriltag_${X}.docstring sed 's/\"/\\\\\"/g\; s/^/\"/\; s/$$/\\\\n\"/\;' > apriltag_${X}.docstring.h - WORKING_DIRECTORY ${PROJECT_BINARY_DIR}) - endforeach() +option(BUILD_PYTHON_WRAPPER "Builds Python wrapper" ON) - add_custom_command(OUTPUT apriltag_pywrap.o - COMMAND ${CMAKE_C_COMPILER} ${PY_CFLAGS} -I${PROJECT_BINARY_DIR} -c -o apriltag_pywrap.o ${CMAKE_CURRENT_SOURCE_DIR}/apriltag_pywrap.c - DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/apriltag_pywrap.c ${PROJECT_BINARY_DIR}/apriltag_detect.docstring.h ${PROJECT_BINARY_DIR}/apriltag_py_type.docstring.h) +find_package(Python3 QUIET COMPONENTS Development NumPy) - add_custom_target(apriltag${PY_EXT_SUFFIX} - ${PY_LINKER} ${PY_LDFLAGS} -Wl,-rpath,$ORIGIN apriltag_pywrap.o $ -o apriltag${PY_EXT_SUFFIX} - DEPENDS ${PROJECT_NAME} apriltag_pywrap.o - VERBATIM) +if(BUILD_PYTHON_WRAPPER AND Python3_Development_FOUND AND Python3_NumPy_FOUND) - add_custom_target(apriltag_python ALL - DEPENDS apriltag${PY_EXT_SUFFIX}) + include(CMake/vtkEncodeString.cmake) -execute_process(COMMAND python3 -m site --user-site OUTPUT_VARIABLE PY_DEST) -string(STRIP ${PY_DEST} PY_DEST) -install(FILES ${PROJECT_BINARY_DIR}/apriltag${PY_EXT_SUFFIX} DESTINATION ${PY_DEST}) -endif (NOT Python3_NOT_FOUND AND NOT Numpy_NOT_FOUND AND PYTHONLIBS_FOUND AND BUILD_PYTHON_WRAPPER) + foreach(X IN ITEMS detect py_type estimate_tag_pose) + vtk_encode_string( + INPUT ${CMAKE_CURRENT_SOURCE_DIR}/apriltag_${X}.docstring + NAME apriltag_${X}_docstring + ) + endforeach() + add_custom_target(apriltag_py_docstrings DEPENDS + ${PROJECT_BINARY_DIR}/apriltag_detect_docstring.h + ${PROJECT_BINARY_DIR}/apriltag_py_type_docstring.h + ${PROJECT_BINARY_DIR}/apriltag_estimate_tag_pose_docstring.h + ) + + # set the SOABI manually since renaming the library via OUTPUT_NAME does not work on MSVC + set(apriltag_py_target "apriltag.${Python3_SOABI}") + Python3_add_library(${apriltag_py_target} MODULE ${CMAKE_CURRENT_SOURCE_DIR}/apriltag_pywrap.c) + add_dependencies(${apriltag_py_target} apriltag_py_docstrings) + # avoid linking against Python3::Python to prevent segmentation faults in Conda environments + # https://github.com/AprilRobotics/apriltag/issues/352 + target_link_libraries(${apriltag_py_target} PRIVATE apriltag Python3::NumPy) + target_include_directories(${apriltag_py_target} PRIVATE ${PROJECT_BINARY_DIR}) + + set(PY_DEST ${CMAKE_INSTALL_PREFIX}/lib/python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}/site-packages/) + install(TARGETS ${apriltag_py_target} LIBRARY DESTINATION ${PY_DEST}) +elseif(BUILD_PYTHON_WRAPPER) + message(WARNING + "Python bindings requested (BUILD_PYTHON_WRAPPER=ON) but Development and NumPy not found. " + "Python bindings will not be built. Set BUILD_PYTHON_WRAPPER=OFF to silent this warnings." + ) +endif() # Examples if (BUILD_EXAMPLES) @@ -198,6 +213,7 @@ if (BUILD_EXAMPLES) set(_OpenCV_REQUIRED_COMPONENTS core imgproc videoio highgui) find_package(OpenCV COMPONENTS ${_OpenCV_REQUIRED_COMPONENTS} QUIET CONFIG) if(OpenCV_FOUND) + enable_language(CXX) # NB: contrib required for TickMeter in OpenCV 2.4. This is only required for 16.04 backwards compatibility and can be removed in the future. # If we add it to the find_package initially, the demo won't build for newer OpenCV versions if(OpenCV_VERSION VERSION_LESS "3.0.0") @@ -216,3 +232,8 @@ if (BUILD_EXAMPLES) # install example programs install(TARGETS apriltag_demo RUNTIME DESTINATION bin) endif() + +if(BUILD_TESTING) + enable_testing() + add_subdirectory(test) +endif() diff --git a/vendor/apriltag/Makefile b/vendor/apriltag/Makefile deleted file mode 100644 index 95fee6c9..00000000 --- a/vendor/apriltag/Makefile +++ /dev/null @@ -1,41 +0,0 @@ -PREFIX ?= /usr/local - -CC = gcc -AR = ar - -CFLAGS = -std=gnu99 -fPIC -Wall -Wno-unused-parameter -Wno-unused-function -CFLAGS += -I. -O3 -fno-strict-overflow -g - -#APRILTAG_SRCS := $(shell ls *.c common/*.c) -APRILTAG_SRCS := apriltag.c apriltag_pose.c apriltag_quad_thresh.c common/g2d.c common/getopt.c common/homography.c common/image_u8.c common/image_u8x3.c common/image_u8x4.c common/matd.c common/pam.c common/pjpeg.c common/pjpeg-idct.c common/pnm.c common/string_util.c common/svd22.c common/time_util.c common/unionfind.c common/workerpool.c common/zarray.c common/zhash.c common/zmaxheap.c tag16h5.c tag25h9.c tag36h11.c tagCircle21h7.c tagCircle49h12.c tagCustom48h12.c tagStandard41h12.c tagStandard52h13.c -APRILTAG_HEADERS := $(shell ls *.h common/*.h) -APRILTAG_OBJS := $(APRILTAG_SRCS:%.c=%.o) -TARGETS := libapriltag.a libapriltag.so - -.PHONY: all -all: $(TARGETS) - @$(MAKE) -C example all - -.PHONY: install -install: libapriltag.so - @chmod +x install.sh - @./install.sh $(PREFIX)/lib libapriltag.so - @./install.sh $(PREFIX)/include/apriltag $(APRILTAG_HEADERS) - @ldconfig - -libapriltag.a: $(APRILTAG_OBJS) - @echo " [$@]" - @$(AR) -cq $@ $(APRILTAG_OBJS) - -libapriltag.so: $(APRILTAG_OBJS) - @echo " [$@]" - @$(CC) -fPIC -shared -o $@ $^ - -%.o: %.c - @echo " $@" - @$(CC) -o $@ -c $< $(CFLAGS) - -.PHONY: clean -clean: - @rm -rf *.o common/*.o $(TARGETS) - @$(MAKE) -C example clean diff --git a/vendor/apriltag/README.md b/vendor/apriltag/README.md index 51f1a5eb..31f71353 100644 --- a/vendor/apriltag/README.md +++ b/vendor/apriltag/README.md @@ -8,8 +8,6 @@ Table of Contents ================= - [Papers](#papers) - [Install](#install) - - [cmake](#cmake) - - [make](#make) - [Usage](#usage) - [Choosing a Tag Family](#choosing-a-tag-family) - [Getting Started with the Detector](#getting-started-with-the-detector) @@ -44,8 +42,6 @@ Officially only Linux operating systems are supported, although users have had s The default installation will place headers in /usr/local/include and shared library in /usr/local/lib. It also installs a pkg-config script into /usr/local/lib/pkgconfig and will install a python wrapper if python3 is installed. -## cmake -If you have CMake installed, then do: ``` cmake -B build -DCMAKE_BUILD_TYPE=Release cmake --build build --target install @@ -65,16 +61,6 @@ to generate and compile via the ninja build script. It will be much faster than You can omit `--target install` if you only want to use this locally without installing. -## make -Otherwise, we have a handwritten makefile you can use (be warned it will do slightly different things): -``` -make -j -sudo make install -``` - -To install to a different directory than /usr/local: - - $ PREFIX=/some/path sudo make install Usage ===== @@ -103,11 +89,15 @@ If none of these fit your needs, generate your own custom tag family [here](http detections = detector.detect(image) -Alternately you can use the AprilTag python bindings created by [duckietown](https://github.com/duckietown/apriltags3-py). +Alternately you can use the AprilTag python bindings created by [duckietown](https://github.com/duckietown/lib-dt-apriltags). ### C - image_u8_t* im = image_u8_create_from_pnm("test.png"); + image_u8_t* im = image_u8_create_from_pnm("test.pnm"); + if (im == NULL) { + fprintf(stderr, "Failed to load pnm image.\n"); + exit(1); + } apriltag_detector_t *td = apriltag_detector_create(); apriltag_family_t *tf = tagStandard41h12_create(); apriltag_detector_add_family(td, tf); @@ -146,10 +136,9 @@ Note that this library has no external dependencies. Most applications will require, at minimum, a method for acquiring images. See example/opencv_demo.cc for an example of using AprilTag in C++ with OpenCV. -This example application can be built by executing the following: +After building the repository you can run the example opencv application with: - $ cd examples - $ make opencv_demo + $ ./build/opencv_demo Image data in a cv::Mat object can be passed to AprilTag without creating a deep copy. Simply create an image_u8_t header for the cv::Mat data buffer: @@ -202,7 +191,7 @@ Note: The tag size should not be measured from the outside of the tag. The tag s ![The tag size is the width of the edge between the white and black borders.](tag_size_48h12.png) ### 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, with x-axis to the right, y-axis down, and z-axis into the tag. +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. Debugging ========= diff --git a/vendor/apriltag/apriltag.c b/vendor/apriltag/apriltag.c index ec4f0224..5e3be1d7 100644 --- a/vendor/apriltag/apriltag.c +++ b/vendor/apriltag/apriltag.c @@ -41,6 +41,7 @@ either expressed or implied, of the Regents of The University of Michigan. #include #include "common/image_u8.h" +#include "common/image_u8_parallel.h" #include "common/image_u8x3.h" #include "common/zarray.h" #include "common/matd.h" @@ -116,19 +117,13 @@ static double graymodel_interpolate(struct graymodel *gm, double x, double y) return gm->C[0]*x + gm->C[1]*y + gm->C[2]; } -struct quick_decode_entry +static inline int popcount64(uint64_t x) { - uint64_t rcode; // the queried code - uint16_t id; // the tag ID (a small integer) - uint8_t hamming; // how many errors corrected? - uint8_t rotation; // number of rotations [0, 3] -}; - -struct quick_decode -{ - int nentries; - struct quick_decode_entry *entries; -}; + x -= (x >> 1) & 0x5555555555555555ULL; + x = (x & 0x3333333333333333ULL) + ((x >> 2) & 0x3333333333333333ULL); + x = (x + (x >> 4)) & 0x0f0f0f0f0f0f0f0fULL; + return (x * 0x0101010101010101ULL) >> 56; +} /** * Assuming we are drawing the image one quadrant at a time, what would the rotated image look like? @@ -168,18 +163,34 @@ static struct quad *quad_copy(struct quad *quad) return q; } -static void quick_decode_add(struct quick_decode *qd, uint64_t code, int id, int hamming) +struct quick_decode_result { - uint32_t bucket = code % qd->nentries; + uint64_t rcode; // the queried code + uint16_t id; // the tag ID (a small integer) + uint8_t hamming; // how many errors corrected? + uint8_t rotation; // number of rotations [0, 3] +}; - while (qd->entries[bucket].rcode != UINT64_MAX) { - bucket = (bucket + 1) % qd->nentries; - } +#define NUM_CHUNKS 4 - qd->entries[bucket].rcode = code; - qd->entries[bucket].id = id; - qd->entries[bucket].hamming = hamming; -} +struct quick_decode +{ + int nbits; + int chunk_size; + int capacity; + int chunk_mask; + int shifts[NUM_CHUNKS]; + + // chunk_offsets is a map from chunk value to a range of locations in chunk_ids. + // Together with chunk_ids, this allows a lookup of all codes matching a chunk value. + uint16_t* chunk_offsets[NUM_CHUNKS]; + + // chunk_ids is an array of indices into the codes table + uint16_t* chunk_ids[NUM_CHUNKS]; + + int maxhamming; + int ncodes; +}; static void quick_decode_uninit(apriltag_family_t *fam) { @@ -187,7 +198,10 @@ static void quick_decode_uninit(apriltag_family_t *fam) return; struct quick_decode *qd = (struct quick_decode*) fam->impl; - free(qd->entries); + for (int i = 0; i < NUM_CHUNKS; i++) { + free(qd->chunk_offsets[i]); + free(qd->chunk_ids[i]); + } free(qd); fam->impl = NULL; } @@ -197,126 +211,130 @@ static void quick_decode_init(apriltag_family_t *family, int maxhamming) assert(family->impl == NULL); assert(family->ncodes < 65536); - struct quick_decode *qd = calloc(1, sizeof(struct quick_decode)); - int capacity = family->ncodes; - - int nbits = family->nbits; - - if (maxhamming >= 1) - capacity += family->ncodes * nbits; - - if (maxhamming >= 2) - capacity += family->ncodes * nbits * (nbits-1); - - if (maxhamming >= 3) - capacity += family->ncodes * nbits * (nbits-1) * (nbits-2); - - qd->nentries = capacity * 3; - -// debug_print("capacity %d, size: %.0f kB\n", -// capacity, qd->nentries * sizeof(struct quick_decode_entry) / 1024.0); + if (maxhamming > 3) { + debug_print("\"maxhamming\" beyond 3 not supported\n"); + errno = EINVAL; + return; + } - qd->entries = calloc(qd->nentries, sizeof(struct quick_decode_entry)); - if (qd->entries == NULL) { - debug_print("Failed to allocate hamming decode table\n"); - // errno already set to ENOMEM (Error No MEMory) by calloc() failure + struct quick_decode *qd = calloc(1, sizeof(struct quick_decode)); + if (!qd) { + debug_print("Memory allocation failed\n"); return; } + family->impl = qd; - for (int i = 0; i < qd->nentries; i++) - qd->entries[i].rcode = UINT64_MAX; + qd->maxhamming = maxhamming; + qd->ncodes = family->ncodes; + qd->nbits = family->nbits; - errno = 0; + qd->chunk_size = (qd->nbits + (NUM_CHUNKS - 1)) / NUM_CHUNKS; + qd->capacity = 1 << qd->chunk_size; + qd->chunk_mask = qd->capacity - 1; - for (uint32_t i = 0; i < family->ncodes; i++) { - uint64_t code = family->codes[i]; + for (int i = 0; i < NUM_CHUNKS; i++) { + qd->shifts[i] = i * qd->chunk_size; + } - // add exact code (hamming = 0) - quick_decode_add(qd, code, i, 0); + for (int i = 0; i < NUM_CHUNKS; i++) { + qd->chunk_offsets[i] = calloc(qd->capacity + 1, sizeof(uint16_t)); + if (!qd->chunk_offsets[i]) { + debug_print("Memory allocation failed\n"); + goto fail; + } + qd->chunk_ids[i] = calloc(qd->ncodes, sizeof(uint16_t)); + if (!qd->chunk_ids[i]) { + debug_print("Memory allocation failed\n"); + goto fail; + } + } - if (maxhamming >= 1) { - // add hamming 1 - for (int j = 0; j < nbits; j++) - quick_decode_add(qd, code ^ (APRILTAG_U64_ONE << j), i, 1); + // Count frequencies + for (int i = 0; i < qd->ncodes; i++) { + uint64_t code = family->codes[i]; + for (int j = 0; j < NUM_CHUNKS; j++) { + int val = (code >> qd->shifts[j]) & qd->chunk_mask; + qd->chunk_offsets[j][val + 1]++; } + } - if (maxhamming >= 2) { - // add hamming 2 - for (int j = 0; j < nbits; j++) - for (int k = 0; k < j; k++) - quick_decode_add(qd, code ^ (APRILTAG_U64_ONE << j) ^ (APRILTAG_U64_ONE << k), i, 2); + // Prefix sum + for (int i = 0; i < NUM_CHUNKS; i++) { + for (int j = 0; j < qd->capacity; j++) { + qd->chunk_offsets[i][j + 1] += qd->chunk_offsets[i][j]; } + } - if (maxhamming >= 3) { - // add hamming 3 - for (int j = 0; j < nbits; j++) - for (int k = 0; k < j; k++) - for (int m = 0; m < k; m++) - quick_decode_add(qd, code ^ (APRILTAG_U64_ONE << j) ^ (APRILTAG_U64_ONE << k) ^ (APRILTAG_U64_ONE << m), i, 3); + // Populate ids + uint16_t *cursors[NUM_CHUNKS]; + memset(cursors, 0, sizeof(cursors)); + for (int i = 0; i < NUM_CHUNKS; i++) { + cursors[i] = malloc((qd->capacity + 1) * sizeof(uint16_t)); + if (cursors[i] == NULL) { + debug_print("Memory allocation failed\n"); + for (int j = 0; j < NUM_CHUNKS; j++) + free(cursors[j]); + goto fail; } + memcpy(cursors[i], qd->chunk_offsets[i], (qd->capacity + 1) * sizeof(uint16_t)); + } - if (maxhamming > 3) { - debug_print("\"maxhamming\" beyond 3 not supported\n"); - // set errno to Error INvalid VALue - errno = EINVAL; - return; + for (int i = 0; i < qd->ncodes; i++) { + uint64_t code = family->codes[i]; + for (int j = 0; j < NUM_CHUNKS; j++) { + int val = (code >> qd->shifts[j]) & qd->chunk_mask; + int write_pos = cursors[j][val]; + qd->chunk_ids[j][write_pos] = i; + cursors[j][val]++; } } - family->impl = qd; + for (int i = 0; i < NUM_CHUNKS; i++) { + free(cursors[i]); + } - #if 0 - int longest_run = 0; - int run = 0; - int run_sum = 0; - int run_count = 0; - - // This accounting code doesn't check the last possible run that - // occurs at the wrap-around. That's pretty insignificant. - for (int i = 0; i < qd->nentries; i++) { - if (qd->entries[i].rcode == UINT64_MAX) { - if (run > 0) { - run_sum += run; - run_count ++; - } - run = 0; - } else { - run ++; - longest_run = imax(longest_run, run); - } - } + return; - printf("quick decode: longest run: %d, average run %.3f\n", longest_run, 1.0 * run_sum / run_count); - #endif +fail: + quick_decode_uninit(family); } -// returns an entry with hamming set to 255 if no decode was found. +// returns a result with hamming set to 255 if no decode was found. static void quick_decode_codeword(apriltag_family_t *tf, uint64_t rcode, - struct quick_decode_entry *entry) + struct quick_decode_result *res) { struct quick_decode *qd = (struct quick_decode*) tf->impl; // qd might be null if detector_add_family_bits() failed for (int ridx = 0; qd != NULL && ridx < 4; ridx++) { - for (int bucket = rcode % qd->nentries; - qd->entries[bucket].rcode != UINT64_MAX; - bucket = (bucket + 1) % qd->nentries) { - - if (qd->entries[bucket].rcode == rcode) { - *entry = qd->entries[bucket]; - entry->rotation = ridx; - return; + for (int i = 0; i < NUM_CHUNKS; i++) { + int val = (rcode >> qd->shifts[i]) & qd->chunk_mask; + int start = qd->chunk_offsets[i][val]; + int end = qd->chunk_offsets[i][val + 1]; + + for (int j = start; j < end; j++) { + uint16_t id = qd->chunk_ids[i][j]; + uint64_t correct_code = tf->codes[id]; + int hamming = popcount64(correct_code ^ rcode); + + if (hamming <= qd->maxhamming) { + res->rcode = rcode; + res->id = id; + res->hamming = hamming; + res->rotation = ridx; + return; + } } } rcode = rotate90(rcode, tf->nbits); } - entry->rcode = 0; - entry->id = 65535; - entry->hamming = 255; - entry->rotation = 0; + res->rcode = 0; + res->id = 65535; + res->hamming = 255; + res->rotation = 0; } static inline int detection_compare_function(const void *_a, const void *_b) @@ -415,7 +433,7 @@ struct evaluate_quad_ret matd_t *H, *Hinv; int decode_status; - struct quick_decode_entry e; + struct quick_decode_result res; }; static matd_t* homography_compute2(double c[4][4]) { @@ -445,6 +463,10 @@ static matd_t* homography_compute2(double c[4][4]) { } } + if (max_val_idx < 0) { + return NULL; + } + if (max_val < epsilon) { debug_print("WRN: Matrix is singular.\n"); return NULL; @@ -562,7 +584,7 @@ static void sharpen(apriltag_detector_t* td, double* values, int size) { } // returns the decision margin. Return < 0 if the detection should be rejected. -static float quad_decode(apriltag_detector_t* td, apriltag_family_t *family, image_u8_t *im, struct quad *quad, struct quick_decode_entry *entry, image_u8_t *im_samples) +static float quad_decode(apriltag_detector_t* td, apriltag_family_t *family, image_u8_t *im, struct quad *quad, struct quick_decode_result *res, image_u8_t *im_samples) { // decode the tag binary contents by sampling the pixel // closest to the center of each bit cell. @@ -731,7 +753,7 @@ static float quad_decode(apriltag_detector_t* td, apriltag_family_t *family, ima } } - quick_decode_codeword(family, rcode, entry); + quick_decode_codeword(family, rcode, res); free(values); return fmin(white_score / white_score_count, black_score / black_score_count); } @@ -785,10 +807,19 @@ static void refine_edges(apriltag_detector_t *td, image_u8_t *im_orig, struct qu // search on another pixel in the first place. Likewise, // for very small tags, we don't want the range to be too // big. - double range = td->quad_decimate + 1; + + int range = td->quad_decimate + 1; + + // To reduce the overhead of bilinear interpolation, we can + // reduce the number of steps per unit. + int steps_per_unit = 4; + double step_length = 1.0 / steps_per_unit; + int max_steps = 2 * steps_per_unit * range + 1; + double delta = 0.5; // XXX tunable step size. - for (double n = -range; n <= range; n += 0.25) { + for (int step = 0; step < max_steps; ++step) { + double n = -range + step_length * step; // Because of the guaranteed winding order of the // points in the quad, we will start inside the white // portion of the quad and work our way outward. @@ -798,19 +829,36 @@ static void refine_edges(apriltag_detector_t *td, image_u8_t *im_orig, struct qu // gradient more precisely, but are more sensitive to // noise. double grange = 1; - int x1 = x0 + (n + grange)*nx; - int y1 = y0 + (n + grange)*ny; - if (x1 < 0 || x1 >= im_orig->width || y1 < 0 || y1 >= im_orig->height) - continue; - int x2 = x0 + (n - grange)*nx; - int y2 = y0 + (n - grange)*ny; - if (x2 < 0 || x2 >= im_orig->width || y2 < 0 || y2 >= im_orig->height) + double x1 = x0 + (n + grange)*nx - delta; + double y1 = y0 + (n + grange)*ny - delta; + double x1i_d, y1i_d, a1, b1; + a1 = modf(x1, &x1i_d); + b1 = modf(y1, &y1i_d); + int x1i = x1i_d, y1i = y1i_d; + + if (x1i < 0 || x1i + 1 >= im_orig->width || y1i < 0 || y1i + 1 >= im_orig->height) continue; - int g1 = im_orig->buf[y1*im_orig->stride + x1]; - int g2 = im_orig->buf[y2*im_orig->stride + x2]; + double x2 = x0 + (n - grange)*nx - delta; + double y2 = y0 + (n - grange)*ny - delta; + double x2i_d, y2i_d, a2, b2; + a2 = modf(x2, &x2i_d); + b2 = modf(y2, &y2i_d); + int x2i = x2i_d, y2i = y2i_d; + if (x2i < 0 || x2i + 1 >= im_orig->width || y2i < 0 || y2i + 1 >= im_orig->height) + continue; + + // interpolate + double g1 = (1 - a1) * (1 - b1) * im_orig->buf[y1i*im_orig->stride + x1i] + + a1 * (1 - b1) * im_orig->buf[y1i*im_orig->stride + x1i + 1] + + (1 - a1) * b1 * im_orig->buf[(y1i + 1)*im_orig->stride + x1i] + + a1 * b1 * im_orig->buf[(y1i + 1)*im_orig->stride + x1i + 1]; + double g2 = (1 - a2) * (1 - b2) * im_orig->buf[y2i*im_orig->stride + x2i] + + a2 * (1 - b2) * im_orig->buf[y2i*im_orig->stride + x2i + 1] + + (1 - a2) * b2 * im_orig->buf[(y2i + 1)*im_orig->stride + x2i] + + a2 * b2 * im_orig->buf[(y2i + 1)*im_orig->stride + x2i + 1]; if (g1 < g2) // reject points whose gradient is "backwards". They can only hurt us. continue; @@ -918,19 +966,19 @@ static void quad_decode_task(void *_u) // optimization process over with the original quad. struct quad *quad = quad_copy(quad_original); - struct quick_decode_entry entry; + struct quick_decode_result res; - float decision_margin = quad_decode(td, family, im, quad, &entry, task->im_samples); + float decision_margin = quad_decode(td, family, im, quad, &res, task->im_samples); - if (decision_margin >= 0 && entry.hamming < 255) { + if (decision_margin >= 0 && res.hamming < 255) { apriltag_detection_t *det = calloc(1, sizeof(apriltag_detection_t)); det->family = family; - det->id = entry.id; - det->hamming = entry.hamming; + det->id = res.id; + det->hamming = res.hamming; det->decision_margin = decision_margin; - double theta = entry.rotation * M_PI / 2.0; + double theta = res.rotation * M_PI / 2.0; double c = cos(theta), s = sin(theta); // Fix the rotation of our homography to properly orient the tag @@ -1046,11 +1094,11 @@ zarray_t *apriltag_detector_detect(apriltag_detector_t *td, image_u8_t *im_orig) if (td->quad_sigma > 0) { // Apply a blur - image_u8_gaussian_blur(quad_im, sigma, ksz); + image_u8_gaussian_blur_parallel(td->wp, quad_im, sigma, ksz); } else { // SHARPEN the image by subtracting the low frequency components. image_u8_t *orig = image_u8_copy(quad_im); - image_u8_gaussian_blur(quad_im, sigma, ksz); + image_u8_gaussian_blur_parallel(td->wp, quad_im, sigma, ksz); for (int y = 0; y < orig->height; y++) { for (int x = 0; x < orig->width; x++) { @@ -1086,13 +1134,8 @@ zarray_t *apriltag_detector_detect(apriltag_detector_t *td, image_u8_t *im_orig) zarray_get_volatile(quads, i, &q); for (int j = 0; j < 4; j++) { - if (td->quad_decimate == 1.5) { - q->p[j][0] *= td->quad_decimate; - q->p[j][1] *= td->quad_decimate; - } else { - q->p[j][0] = (q->p[j][0] - 0.5)*td->quad_decimate + 0.5; - q->p[j][1] = (q->p[j][1] - 0.5)*td->quad_decimate + 0.5; - } + q->p[j][0] *= td->quad_decimate; + q->p[j][1] *= td->quad_decimate; } } } diff --git a/vendor/apriltag/apriltag_detect.docstring b/vendor/apriltag/apriltag_detect.docstring index 0f9bca59..a70c8e33 100644 --- a/vendor/apriltag/apriltag_detect.docstring +++ b/vendor/apriltag/apriltag_detect.docstring @@ -42,7 +42,9 @@ a tuple containing the detections. Each detection is a dict with keys: - id: integer identifying each detected tag -- center: pixel coordinates of the center of each detection +- center: pixel coordinates of the center of each detection. NOTE: Please be + cautious regarding the image coordinate convention. Here, we define (0,0) as + the left-top corner (not the center point) of the left-top-most pixel. - lb-rb-rt-lt: pixel coordinates of the 4 corners of each detection. The order is left-bottom, right-bottom, right-top, left-top @@ -58,3 +60,8 @@ a tuple containing the detections. Each detection is a dict with keys: of detection accuracy only for very small tags-- not effective for larger tags (where we could have sampled anywhere within a bit cell and still gotten a good detection.) + +- homography: A 3x3 homography matrix that describes the projection from an + "ideal" tag (with corners at (-1,1), (1,1), (1,-1), and (-1,-1)) to pixels + in the image. This matrix can be used to map points from the tag's coordinate + system to the image coordinate system, and is useful for pose estimation. diff --git a/vendor/apriltag/apriltag_estimate_tag_pose.docstring b/vendor/apriltag/apriltag_estimate_tag_pose.docstring new file mode 100644 index 00000000..8bc147b5 --- /dev/null +++ b/vendor/apriltag/apriltag_estimate_tag_pose.docstring @@ -0,0 +1,70 @@ +estimate_tag_pose(detection, tagsize, fx, fy, cx, cy) -> dict + +SYNOPSIS + + import cv2 + import numpy as np + from apriltag import apriltag + + imagepath = '/tmp/tst.jpg' + image = cv2.imread(imagepath, cv2.IMREAD_GRAYSCALE) + detector = apriltag("tag36h11") + + detections = detector.detect(image) + if detections: + # Estimate pose for the first detected tag + # tagsize is the physical size of the tag in meters + # fx, fy are focal lengths in pixels + # cx, cy are principal point coordinates in pixels + pose = detector.estimate_tag_pose(detections[0], + tagsize=0.16, # 16cm tag + fx=600, fy=600, # focal lengths + cx=320, cy=240) # principal point + print("Rotation matrix R:\n", pose['R']) + print("Translation vector t:", pose['t']) + print("Reprojection error:", pose['error']) + +DESCRIPTION + +The estimate_tag_pose() method estimates the 6-DOF pose (position and orientation) +of a detected AprilTag in 3D space. This method requires the detection result from +the detect() method, the physical size of the tag, and camera intrinsic parameters. + +The pose estimation uses the homography matrix from the detection result to +compute the transformation from the tag's coordinate system to the camera's +coordinate system. + +ARGUMENTS + +- detection: A dictionary containing detection information returned by the + detect() method. This dictionary must include the 'homography' key with the + 3x3 homography matrix. + +- tagsize: The physical side length of the AprilTag in meters. This is the real- + world size of the tag, which is necessary for computing the scale of the pose. + +- fx: Focal length in the x direction in pixels. This is a camera intrinsic + parameter that describes how the camera projects 3D points to 2D image space. + +- fy: Focal length in the y direction in pixels. This is a camera intrinsic + parameter that describes how the camera projects 3D points to 2D image space. + +- cx: Principal point x coordinate in pixels. This is the x coordinate of the + optical center of the camera in the image. + +- cy: Principal point y coordinate in pixels. This is the y coordinate of the + optical center of the camera in the image. + +RETURNED VALUE + +Returns a dictionary containing: + +- 'R': 3x3 rotation matrix as a numpy array that represents the orientation + of the tag in the camera coordinate system. + +- 't': 3x1 translation vector as a numpy array (in meters) that represents the + position of the tag in the camera coordinate system. + +- 'error': The object-space error after the iteration process, representing + the sum of squared reprojection errors between observed and estimated points + in object space. A lower value indicates a better pose estimate. diff --git a/vendor/apriltag/apriltag_pose.h b/vendor/apriltag/apriltag_pose.h index 07ee37b2..8120502f 100644 --- a/vendor/apriltag/apriltag_pose.h +++ b/vendor/apriltag/apriltag_pose.h @@ -44,9 +44,9 @@ void estimate_pose_for_tag_homography( * used to find a potential second local minima and Orthogonal Iteration is * used to refine this second estimate. * - * [1]: E. Olson, “Apriltag: A robust and flexible visual fiducial system,” in + * [1]: E. Olson, "Apriltag: A robust and flexible visual fiducial system," in * 2011 IEEE International Conference on Robotics and Automation, - * May 2011, pp. 3400–3407. + * May 2011, pp. 3400-3407. * [2]: Lu, G. D. Hager and E. Mjolsness, "Fast and globally convergent pose * estimation from video images," in IEEE Transactions on Pattern Analysis * and Machine Intelligence, vol. 22, no. 6, pp. 610-622, June 2000. @@ -77,4 +77,3 @@ double estimate_tag_pose(apriltag_detection_info_t* info, apriltag_pose_t* pose) #ifdef __cplusplus } #endif - diff --git a/vendor/apriltag/apriltag_py_type.docstring b/vendor/apriltag/apriltag_py_type.docstring index 97612e32..4ae1ff01 100644 --- a/vendor/apriltag/apriltag_py_type.docstring +++ b/vendor/apriltag/apriltag_py_type.docstring @@ -54,7 +54,7 @@ The constructor takes a number of arguments: All the other arguments are optional: -- Nthreads: how many threads the detector should use. Default is 1 +- threads: how many threads the detector should use. Default is 1 - maxhamming: max number of corrected bits. Larger values guzzle RAM. Default is 1 diff --git a/vendor/apriltag/apriltag_pywrap.c b/vendor/apriltag/apriltag_pywrap.c index cb991bb4..975cea4e 100644 --- a/vendor/apriltag/apriltag_pywrap.c +++ b/vendor/apriltag/apriltag_pywrap.c @@ -2,11 +2,15 @@ #include #include +#ifndef Py_PYTHREAD_H +#include +#endif #include #include #include #include "apriltag.h" +#include "apriltag_pose.h" #include "tag36h10.h" #include "tag36h11.h" #include "tag25h9.h" @@ -68,6 +72,7 @@ typedef struct { apriltag_family_t* tf; apriltag_detector_t* td; + PyThread_type_lock det_lock; void (*destroy_func)(apriltag_family_t *tf); } apriltag_py_t; @@ -75,6 +80,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); @@ -83,6 +90,12 @@ apriltag_new(PyTypeObject *type, PyObject *args, PyObject *kwargs) self->tf = NULL; self->td = NULL; + self->det_lock = PyThread_allocate_lock(); + if (self->det_lock == NULL) { + PyErr_SetString(PyExc_RuntimeError, "Unable to allocate detection lock"); + goto done; + } + const char* family = NULL; int Nthreads = 1; int maxhamming = 1; @@ -171,6 +184,11 @@ apriltag_new(PyTypeObject *type, PyObject *args, PyObject *kwargs) self->destroy_func(self->tf); self->tf = NULL; } + if(self->det_lock != NULL) + { + PyThread_free_lock(self->det_lock); + self->det_lock = NULL; + } Py_DECREF(self); } return NULL; @@ -193,6 +211,11 @@ static void apriltag_dealloc(apriltag_py_t* self) self->destroy_func(self->tf); self->tf = NULL; } + if(self->det_lock != NULL) + { + PyThread_free_lock(self->det_lock); + self->det_lock = NULL; + } Py_TYPE(self)->tp_free((PyObject*)self); } @@ -200,13 +223,18 @@ 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; + PyArrayObject* homography = NULL; PyArrayObject* image = NULL; PyObject* detections_tuple = NULL; +#ifdef _POSIX_C_SOURCE SET_SIGINT(); +#endif if(!PyArg_ParseTuple( args, "O&", PyArray_Converter, &image )) goto done; @@ -237,7 +265,13 @@ static PyObject* apriltag_detect(apriltag_py_t* self, .stride = strides[0], .buf = PyArray_DATA(image)}; - zarray_t* detections = apriltag_detector_detect(self->td, &im); + zarray_t *detections = NULL; // Declare detections variable outside the GIL macro block + Py_BEGIN_ALLOW_THREADS // Release the GIL to allow other Python threads to run + PyThread_acquire_lock(self->det_lock, 1); // Acquire the detection lock before running the detector (blocks until the lock is available) + detections = apriltag_detector_detect(self->td, &im); // Run detection + PyThread_release_lock(self->det_lock); // Release the detection lock + Py_END_ALLOW_THREADS // Acquire the GIL after releasing the detection lock + int N = zarray_size(detections); if (N == 0 && errno == EAGAIN){ @@ -251,7 +285,7 @@ static PyObject* apriltag_detect(apriltag_py_t* self, PyErr_Format(PyExc_RuntimeError, "Error creating output tuple of size %d", N); goto done; } - + for (int i=0; i < N; i++) { xy_c = (PyArrayObject*)PyArray_SimpleNew(1, ((npy_intp[]){2}), NPY_FLOAT64); @@ -279,13 +313,32 @@ static PyObject* apriltag_detect(apriltag_py_t* self, *(double*)PyArray_GETPTR2(xy_lb_rb_rt_lt, j, 1) = det->p[j][1]; } + // Add homography matrix (3x3) + homography = (PyArrayObject*)PyArray_SimpleNew(2, ((npy_intp[]){3,3}), NPY_FLOAT64); + if(homography == NULL) + { + Py_DECREF(xy_c); + Py_DECREF(xy_lb_rb_rt_lt); + PyErr_SetString(PyExc_RuntimeError, "Could not allocate homography array"); + goto done; + } + + for(int j=0; j<3; j++) + { + for(int k=0; k<3; k++) + { + *(double*)PyArray_GETPTR2(homography, j, k) = MATD_EL(det->H, j, k); + } + } + PyTuple_SET_ITEM(detections_tuple, i, - Py_BuildValue("{s:i,s:f,s:i,s:N,s:N}", + Py_BuildValue("{s:i,s:f,s:i,s:N,s:N,s:N}", "hamming", det->hamming, "margin", det->decision_margin, "id", det->id, "center", xy_c, - "lb-rb-rt-lt", xy_lb_rb_rt_lt)); + "lb-rb-rt-lt", xy_lb_rb_rt_lt, + "homography", homography)); xy_c = NULL; xy_lb_rb_rt_lt = NULL; } @@ -300,21 +353,155 @@ static PyObject* apriltag_detect(apriltag_py_t* self, Py_XDECREF(image); Py_XDECREF(detections_tuple); +#ifdef _POSIX_C_SOURCE RESET_SIGINT(); +#endif + return result; +} + +static PyObject* apriltag_estimate_tag_pose(apriltag_py_t* self, + PyObject* args) +{ + PyObject* result = NULL; + PyObject* detection_dict = NULL; + PyArrayObject* R_array = NULL; + PyArrayObject* t_array = NULL; + matd_t* H_matrix = NULL; + double tagsize, fx, fy, cx, cy; + + if(!PyArg_ParseTuple(args, "Oddddd", + &detection_dict, + &tagsize, + &fx, &fy, &cx, &cy)) + return NULL; + + if(!PyDict_Check(detection_dict)) + { + PyErr_SetString(PyExc_TypeError, "First argument must be a detection dictionary"); + return NULL; + } + + // Extract detection information from the dictionary + PyObject* py_id = PyDict_GetItemString(detection_dict, "id"); + PyObject* py_hamming = PyDict_GetItemString(detection_dict, "hamming"); + PyObject* py_margin = PyDict_GetItemString(detection_dict, "margin"); + PyObject* py_center = PyDict_GetItemString(detection_dict, "center"); + PyObject* py_corners = PyDict_GetItemString(detection_dict, "lb-rb-rt-lt"); + PyObject* py_homography = PyDict_GetItemString(detection_dict, "homography"); + + if(!py_id || !py_hamming || !py_margin || !py_center || !py_corners || !py_homography) + { + PyErr_SetString(PyExc_ValueError, + "Detection dictionary is missing required fields. " + "Make sure you're using a detection from the updated detect() method that includes 'homography'."); + return NULL; + } + + // Create a temporary detection structure + apriltag_detection_t det; + det.family = self->tf; + det.id = PyLong_AsLong(py_id); + det.hamming = PyLong_AsLong(py_hamming); + det.decision_margin = PyFloat_AsDouble(py_margin); + + // Extract center + PyArrayObject* center_array = (PyArrayObject*)py_center; + det.c[0] = *(double*)PyArray_GETPTR1(center_array, 0); + det.c[1] = *(double*)PyArray_GETPTR1(center_array, 1); + + // Extract corners + PyArrayObject* corners_array = (PyArrayObject*)py_corners; + for(int i = 0; i < 4; i++) + { + det.p[i][0] = *(double*)PyArray_GETPTR2(corners_array, i, 0); + det.p[i][1] = *(double*)PyArray_GETPTR2(corners_array, i, 1); + } + + // Extract and copy homography matrix + PyArrayObject* homography_array = (PyArrayObject*)py_homography; + H_matrix = matd_create(3, 3); + if(!H_matrix) + { + PyErr_SetString(PyExc_RuntimeError, "Could not allocate homography matrix"); + return NULL; + } + + for(int i = 0; i < 3; i++) + { + for(int j = 0; j < 3; j++) + { + MATD_EL(H_matrix, i, j) = *(double*)PyArray_GETPTR2(homography_array, i, j); + } + } + det.H = H_matrix; + + // Setup detection info + apriltag_detection_info_t info; + info.det = &det; + info.tagsize = tagsize; + info.fx = fx; + info.fy = fy; + info.cx = cx; + info.cy = cy; + + // Estimate pose + apriltag_pose_t pose; + double error = estimate_tag_pose(&info, &pose); + + // Create numpy arrays for R and t + R_array = (PyArrayObject*)PyArray_SimpleNew(2, ((npy_intp[]){3, 3}), NPY_FLOAT64); + t_array = (PyArrayObject*)PyArray_SimpleNew(2, ((npy_intp[]){3, 1}), NPY_FLOAT64); + + if(!R_array || !t_array) + { + PyErr_SetString(PyExc_RuntimeError, "Could not allocate output arrays"); + goto cleanup; + } + + // Copy rotation matrix + for(int i = 0; i < 3; i++) + { + for(int j = 0; j < 3; j++) + { + *(double*)PyArray_GETPTR2(R_array, i, j) = MATD_EL(pose.R, i, j); + } + } + + // Copy translation vector + for(int i = 0; i < 3; i++) + { + *(double*)PyArray_GETPTR2(t_array, i, 0) = MATD_EL(pose.t, i, 0); + } + + result = Py_BuildValue("{s:N,s:N,s:d}", + "R", R_array, + "t", t_array, + "error", error); + R_array = NULL; + t_array = NULL; + +cleanup: + if(H_matrix) + matd_destroy(H_matrix); + if(pose.R) + matd_destroy(pose.R); + if(pose.t) + matd_destroy(pose.t); + Py_XDECREF(R_array); + Py_XDECREF(t_array); + return result; } -static const char apriltag_detect_docstring[] = -#include "apriltag_detect.docstring.h" - ; -static const char apriltag_type_docstring[] = -#include "apriltag_py_type.docstring.h" - ; +#include "apriltag_detect_docstring.h" +#include "apriltag_py_type_docstring.h" +#include "apriltag_estimate_tag_pose_docstring.h" static PyMethodDef apriltag_methods[] = { PYMETHODDEF_ENTRY(apriltag_, detect, METH_VARARGS), - {} + PYMETHODDEF_ENTRY(apriltag_, estimate_tag_pose, METH_VARARGS), + {NULL, NULL, 0, NULL} }; static PyTypeObject apriltagType = @@ -326,11 +513,11 @@ static PyTypeObject apriltagType = .tp_dealloc = (destructor)apriltag_dealloc, .tp_methods = apriltag_methods, .tp_flags = Py_TPFLAGS_DEFAULT, - .tp_doc = apriltag_type_docstring + .tp_doc = apriltag_py_type_docstring }; static PyMethodDef methods[] = - { {} + { {NULL, NULL, 0, NULL} }; @@ -358,7 +545,11 @@ static struct PyModuleDef module_def = "apriltag", "AprilTags visual fiducial system detector", -1, - methods + methods, + 0, + 0, + 0, + 0 }; PyMODINIT_FUNC PyInit_apriltag(void) @@ -378,4 +569,3 @@ PyMODINIT_FUNC PyInit_apriltag(void) } #endif - diff --git a/vendor/apriltag/apriltag_quad_thresh.c b/vendor/apriltag/apriltag_quad_thresh.c index 38d3e43d..cad10e56 100644 --- a/vendor/apriltag/apriltag_quad_thresh.c +++ b/vendor/apriltag/apriltag_quad_thresh.c @@ -266,8 +266,13 @@ void fit_line(struct line_fit_pt *lfps, int sz, int i0, int i1, double *lineparm } double length = sqrtf(M); - lineparm[2] = nx/length; - lineparm[3] = ny/length; + if (fabs(length) < 1e-12) { + lineparm[2] = lineparm[3] = 0; + } + else { + lineparm[2] = nx/length; + lineparm[3] = ny/length; + } } // sum of squared errors = @@ -615,42 +620,45 @@ int quad_segment_agg(zarray_t *cluster, struct line_fit_pt *lfps, int indices[4] */ struct line_fit_pt* compute_lfps(int sz, zarray_t* cluster, image_u8_t* im) { struct line_fit_pt *lfps = calloc(sz, sizeof(struct line_fit_pt)); + double sum_Mx = 0, sum_My = 0, sum_Mxx = 0, sum_Myy = 0, sum_Mxy = 0, sum_W = 0; for (int i = 0; i < sz; i++) { struct pt *p; zarray_get_volatile(cluster, i, &p); - if (i > 0) { - memcpy(&lfps[i], &lfps[i-1], sizeof(struct line_fit_pt)); - } - - { - // we now undo our fixed-point arithmetic. - double delta = 0.5; // adjust for pixel center bias - double x = p->x * .5 + delta; - double y = p->y * .5 + delta; - int ix = x, iy = y; - double W = 1; + // we now undo our fixed-point arithmetic. + double delta = 0.5; // adjust for pixel center bias + double x = p->x * .5 + delta; + double y = p->y * .5 + delta; + int ix = x, iy = y; + double W = 1; - if (ix > 0 && ix+1 < im->width && iy > 0 && iy+1 < im->height) { - int grad_x = im->buf[iy * im->stride + ix + 1] - - im->buf[iy * im->stride + ix - 1]; + if (ix > 0 && ix+1 < im->width && iy > 0 && iy+1 < im->height) { + int grad_x = im->buf[iy * im->stride + ix + 1] - + im->buf[iy * im->stride + ix - 1]; - int grad_y = im->buf[(iy+1) * im->stride + ix] - - im->buf[(iy-1) * im->stride + ix]; + int grad_y = im->buf[(iy+1) * im->stride + ix] - + im->buf[(iy-1) * im->stride + ix]; - // XXX Tunable. How to shape the gradient magnitude? - W = sqrt(grad_x*grad_x + grad_y*grad_y) + 1; - } - - double fx = x, fy = y; - lfps[i].Mx += W * fx; - lfps[i].My += W * fy; - lfps[i].Mxx += W * fx * fx; - lfps[i].Mxy += W * fx * fy; - lfps[i].Myy += W * fy * fy; - lfps[i].W += W; + // XXX Tunable. How to shape the gradient magnitude? + W = sqrt(grad_x*grad_x + grad_y*grad_y) + 1; } + + double fx = x, fy = y; + sum_Mx += W * fx; + sum_My += W * fy; + sum_Mxx += W * fx * fx; + sum_Mxy += W * fx * fy; + sum_Myy += W * fy * fy; + sum_W += W; + + // Store cumulative sums + lfps[i].Mx = sum_Mx; + lfps[i].My = sum_My; + lfps[i].Mxx = sum_Mxx; + lfps[i].Mxy = sum_Mxy; + lfps[i].Myy = sum_Myy; + lfps[i].W = sum_W; } return lfps; } @@ -709,8 +717,16 @@ static inline void ptsort(struct pt *pts, int sz) #undef MAYBE_SWAP // a merge sort with temp storage. - - struct pt *tmp = malloc(sizeof(struct pt) * sz); + // Use stack allocation for small arrays to avoid malloc overhead + #define STACK_BUFFER_SIZE 256 + struct pt stack_buffer[STACK_BUFFER_SIZE]; + struct pt *tmp; + const bool use_heap = sz > STACK_BUFFER_SIZE; + if (use_heap) { + tmp = malloc(sizeof(struct pt) * sz); + } else { + tmp = stack_buffer; + } memcpy(tmp, pts, sizeof(struct pt) * sz); @@ -744,7 +760,9 @@ static inline void ptsort(struct pt *pts, int sz) if (bpos < bsz) memcpy(&pts[outpos], &bs[bpos], (bsz-bpos)*sizeof(struct pt)); - free(tmp); + if (use_heap) { + free(tmp); + } #undef MERGE } @@ -899,11 +917,11 @@ int fit_quad( double det = A00 * A11 - A10 * A01; // inverse. - double W00 = A11 / det, W01 = -A01 / det; if (fabs(det) < 0.001) { res = 0; goto finish; } + double W00 = A11 / det, W01 = -A01 / det; // solve double L0 = W00*B0 + W01*B1; @@ -1805,7 +1823,8 @@ zarray_t* fit_quads(apriltag_detector_t *td, int w, int h, zarray_t* clusters, i normal_border |= !family->reversed_border; reversed_border |= family->reversed_border; } - min_tag_width /= td->quad_decimate; + if (td->quad_decimate > 1) + min_tag_width /= td->quad_decimate; if (min_tag_width < 3) { min_tag_width = 3; } diff --git a/vendor/apriltag/common/g2d.c b/vendor/apriltag/common/g2d.c index 91bbcabc..ebb85e55 100644 --- a/vendor/apriltag/common/g2d.c +++ b/vendor/apriltag/common/g2d.c @@ -120,7 +120,7 @@ int g2d_polygon_contains_point_ref(const zarray_t *poly, double q[2]) double acc_theta = 0; - double last_theta; + double last_theta = 0; for (int i = 0; i <= psz; i++) { double p[2]; @@ -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/vendor/apriltag/common/image_u8.c b/vendor/apriltag/common/image_u8.c index 35458ddf..3064aff3 100644 --- a/vendor/apriltag/common/image_u8.c +++ b/vendor/apriltag/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 @@ -318,29 +321,29 @@ void image_u8_convolve_2D(image_u8_t *im, const uint8_t *k, int ksz) { assert((ksz & 1) == 1); // ksz must be odd. + uint8_t *x = malloc(sizeof(uint8_t)*im->stride); for (int y = 0; y < im->height; y++) { - uint8_t *x = malloc(sizeof(uint8_t)*im->stride); memcpy(x, &im->buf[y*im->stride], im->stride); convolve(x, &im->buf[y*im->stride], im->width, k, ksz); - free(x); } + free(x); + uint8_t *xb = malloc(sizeof(uint8_t)*im->height); + uint8_t *yb = malloc(sizeof(uint8_t)*im->height); for (int x = 0; x < im->width; x++) { - uint8_t *xb = malloc(sizeof(uint8_t)*im->height); - uint8_t *yb = malloc(sizeof(uint8_t)*im->height); for (int y = 0; y < im->height; y++) xb[y] = im->buf[y*im->stride + x]; convolve(xb, yb, im->height, k, ksz); - free(xb); for (int y = 0; y < im->height; y++) im->buf[y*im->stride + x] = yb[y]; - free(yb); } + free(xb); + free(yb); } void image_u8_gaussian_blur(image_u8_t *im, double sigma, int ksz) diff --git a/vendor/apriltag/common/image_u8_parallel.c b/vendor/apriltag/common/image_u8_parallel.c new file mode 100644 index 00000000..8751c76e --- /dev/null +++ b/vendor/apriltag/common/image_u8_parallel.c @@ -0,0 +1,165 @@ +/** + * @file image_u8_parallel.c + * @author MqCreaple (gmq14159@gmail.com) + * @brief Parallelized processing of various image_u8 related functions. + * @version 0.1 + * @date 2025-08-07 + * + * @copyright Copyright (c) 2025 + * + */ + +#include "common/image_u8_parallel.h" +#include "common/workerpool.h" +#include "common/math_util.h" + +static void convolve(const uint8_t *x, uint8_t *y, int sz, const uint8_t *k, int ksz) +{ + assert((ksz&1)==1); + + for (int i = 0; i < ksz/2 && i < sz; i++) + y[i] = x[i]; + + for (int i = 0; i < sz - ksz + 1; i++) { + uint32_t acc = 0; + + for (int j = 0; j < ksz; j++) + acc += k[j]*x[i+j]; + + y[ksz/2 + i] = acc >> 8; + } + + for (int i = sz - ksz/2; i < sz; i++) + y[i] = x[i]; +} + +struct image_u8_convolve_2D_task { + image_u8_t *im; + const uint8_t *k; + int ksz; + int idx_st; + int idx_ed; +}; + +static void _image_u8_convolve_2D_thread_1(void *p) { + struct image_u8_convolve_2D_task *params = (struct image_u8_convolve_2D_task*) p; + image_u8_t *im = params->im; + const uint8_t *k = params->k; + int ksz = params->ksz; + int y_st = params->idx_st; + int y_ed = params->idx_ed; + + assert((ksz & 1) == 1); // ksz must be odd. + + uint8_t *x = malloc(sizeof(uint8_t)*im->stride); + for (int y = y_st; y < y_ed; y++) { + memcpy(x, &im->buf[y*im->stride], im->stride); + convolve(x, &im->buf[y*im->stride], im->width, k, ksz); + } + free(x); +} + +static void _image_u8_convolve_2D_thread_2(void *p) { + struct image_u8_convolve_2D_task *params = (struct image_u8_convolve_2D_task*) p; + image_u8_t *im = params->im; + const uint8_t *k = params->k; + int ksz = params->ksz; + int x_st = params->idx_st; + int x_ed = params->idx_ed; + + uint8_t *xb = malloc(sizeof(uint8_t)*im->height); + uint8_t *yb = malloc(sizeof(uint8_t)*im->height); + for (int x = x_st; x < x_ed; x++) { + + for (int y = 0; y < im->height; y++) + xb[y] = im->buf[y*im->stride + x]; + + convolve(xb, yb, im->height, k, ksz); + + for (int y = 0; y < im->height; y++) + im->buf[y*im->stride + x] = yb[y]; + } + free(xb); + free(yb); +} + +void image_u8_convolve_2D_parallel(workerpool_t *wp, image_u8_t *im, const uint8_t *k, int ksz) { + if(im->width * im->height < 65536) { + // for small images, directly use single threaded convolution + image_u8_convolve_2D(im, k, ksz); + return; + } + int nthreads = workerpool_get_nthreads(wp); + + struct image_u8_convolve_2D_task *params = malloc(sizeof(struct image_u8_convolve_2D_task) * nthreads); + int y_inc = im->height / nthreads; + int y_remainder = im->height % nthreads; + int last_y = 0; + for(int idx = 0; idx < nthreads; idx++) { + params[idx].im = im; + params[idx].k = k; + params[idx].ksz = ksz; + params[idx].idx_st = last_y; + last_y += y_inc; + if(idx < y_remainder) { + last_y += 1; // distribute the remainders across the n threads + } + params[idx].idx_ed = last_y; + workerpool_add_task(wp, _image_u8_convolve_2D_thread_1, ¶ms[idx]); + } + workerpool_run(wp); + + int x_inc = im->width / nthreads; + int x_remainder = im->width % nthreads; + int last_x = 0; + for(int idx = 0; idx < nthreads; idx++) { + params[idx].im = im; + params[idx].k = k; + params[idx].ksz = ksz; + params[idx].idx_st = last_x; + last_x += x_inc; + if(idx < x_remainder) { + last_x += 1; // distribute the remainders across the n threads + } + params[idx].idx_ed = last_x; + workerpool_add_task(wp, _image_u8_convolve_2D_thread_2, ¶ms[idx]); + } + workerpool_run(wp); + + free(params); +} + +void image_u8_gaussian_blur_parallel(workerpool_t *wp, image_u8_t *im, double sigma, int ksz) { + if (sigma == 0) + return; + + assert((ksz & 1) == 1); // ksz must be odd. + + // build the kernel. + double *dk = malloc(sizeof(double)*ksz); + + // for kernel of length 5: + // dk[0] = f(-2), dk[1] = f(-1), dk[2] = f(0), dk[3] = f(1), dk[4] = f(2) + for (int i = 0; i < ksz; i++) { + int x = -ksz/2 + i; + double v = exp(-.5*sq(x / sigma)); + dk[i] = v; + } + + // normalize + double acc = 0; + for (int i = 0; i < ksz; i++) + acc += dk[i]; + + for (int i = 0; i < ksz; i++) + dk[i] /= acc; + + uint8_t *k = malloc(sizeof(uint8_t)*ksz); + for (int i = 0; i < ksz; i++) + k[i] = dk[i]*255; + + free(dk); + + image_u8_convolve_2D_parallel(wp, im, k, ksz); + free(k); +} diff --git a/vendor/apriltag/common/image_u8_parallel.h b/vendor/apriltag/common/image_u8_parallel.h new file mode 100644 index 00000000..bd60e475 --- /dev/null +++ b/vendor/apriltag/common/image_u8_parallel.h @@ -0,0 +1,19 @@ +/** + * @file image_u8_parallel.h + * @author MqCreaple (gmq14159@gmail.com) + * @brief Parallelized processing of various image_u8 related functions. + * @version 0.1 + * @date 2025-08-07 + * + * @copyright Copyright (c) 2025 + * + */ +#pragma once + +#include "image_u8.h" +#include "workerpool.h" +#include "math_util.h" + +void image_u8_convolve_2D_parallel(workerpool_t *wp, image_u8_t *im, const uint8_t *k, int ksz); + +void image_u8_gaussian_blur_parallel(workerpool_t *wp, image_u8_t *im, double sigma, int ksz); diff --git a/vendor/apriltag/common/matd.c b/vendor/apriltag/common/matd.c index 176394ed..067e40ed 100644 --- a/vendor/apriltag/common/matd.c +++ b/vendor/apriltag/common/matd.c @@ -51,18 +51,20 @@ matd_t *matd_create(int rows, int cols) if (rows == 0 || cols == 0) return matd_create_scalar(0); - matd_t *m = calloc(1, sizeof(matd_t) + (rows*cols*sizeof(double))); + matd_t *m = calloc(1, sizeof(matd_t)); m->nrows = rows; m->ncols = cols; + m->data = calloc(rows * cols, sizeof(double)); return m; } matd_t *matd_create_scalar(TYPE v) { - matd_t *m = calloc(1, sizeof(matd_t) + sizeof(double)); + matd_t *m = calloc(1, sizeof(matd_t)); m->nrows = 0; m->ncols = 0; + m->data = calloc(1, sizeof(double)); m->data[0] = v; return m; @@ -220,7 +222,8 @@ void matd_destroy(matd_t *m) if (!m) return; - assert(m != NULL); + assert(m->data != NULL); + free(m->data); free(m); } @@ -402,7 +405,7 @@ double matd_det_general(const matd_t *a) // The determinant of a can be calculated as // epsilon*det(L)*det(U), // where epsilon is just the sign of the corresponding permutation - // (which is +1 for an even number of permutations and is −1 + // (which is +1 for an even number of permutations and is -1 // for an uneven number of permutations). double det = mlu->pivsign * detL * detU; @@ -1148,7 +1151,7 @@ static matd_svd_t matd_svd_tall(matd_t *A, int flags) assert(maxiters > 0); // reassure clang int iter; - double maxv; // maximum non-zero value being reduced this iteration + double maxv = 0; // maximum non-zero value being reduced this iteration double tol = 1E-10; diff --git a/vendor/apriltag/common/matd.h b/vendor/apriltag/common/matd.h index a293321d..6b7c5f63 100644 --- a/vendor/apriltag/common/matd.h +++ b/vendor/apriltag/common/matd.h @@ -45,8 +45,7 @@ extern "C" { typedef struct { unsigned int nrows, ncols; - double data[]; -// double *data; + double *data; } matd_t; #define MATD_ALLOC(name, nrows, ncols) double name ## _storage [nrows*ncols]; matd_t name = { .nrows = nrows, .ncols = ncols, .data = &name ## _storage }; diff --git a/vendor/apriltag/common/pthreads_cross.cpp b/vendor/apriltag/common/pthreads_cross.c similarity index 95% rename from vendor/apriltag/common/pthreads_cross.cpp rename to vendor/apriltag/common/pthreads_cross.c index 3403863f..04e556cf 100644 --- a/vendor/apriltag/common/pthreads_cross.cpp +++ b/vendor/apriltag/common/pthreads_cross.c @@ -1,256 +1,256 @@ -/** -Copyright John Schember - -Permission is hereby granted, free of charge, to any person obtaining a copy of -this software and associated documentation files (the "Software"), to deal in -the Software without restriction, including without limitation the rights to -use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies -of the Software, and to permit persons to whom the Software is furnished to do -so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. - */ - -#include "common/pthreads_cross.h" - -#ifdef _WIN32 - -typedef struct { - SRWLOCK lock; - bool exclusive; -} pthread_rwlock_t; - -int pthread_rwlock_init(pthread_rwlock_t *rwlock, const pthread_rwlockattr_t *attr); -int pthread_rwlock_destroy(pthread_rwlock_t *rwlock); -int pthread_rwlock_rdlock(pthread_rwlock_t *rwlock); -int pthread_rwlock_tryrdlock(pthread_rwlock_t *rwlock); -int pthread_rwlock_wrlock(pthread_rwlock_t *rwlock); -int pthread_rwlock_trywrlock(pthread_rwlock_t *rwlock); -int pthread_rwlock_unlock(pthread_rwlock_t *rwlock); - -int pthread_create(pthread_t *thread, pthread_attr_t *attr, void *(*start_routine)(void *), void *arg) -{ - (void) attr; - - if (thread == NULL || start_routine == NULL) - return 1; - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wcast-function-type" - *thread = (HANDLE) CreateThread(NULL, 0, (LPTHREAD_START_ROUTINE)start_routine, arg, 0, NULL); -#pragma GCC diagnostic pop - if (*thread == NULL) - return 1; - return 0; -} - -int pthread_join(pthread_t thread, void **value_ptr) -{ - (void)value_ptr; - WaitForSingleObject(thread, INFINITE); - CloseHandle(thread); - return 0; -} - -int pthread_detach(pthread_t thread) -{ - CloseHandle(thread); - return 0; -} - -int pthread_mutex_init(pthread_mutex_t *mutex, pthread_mutexattr_t *attr) -{ - (void)attr; - - if (mutex == NULL) - return 1; - - InitializeCriticalSection(mutex); - return 0; -} - -int pthread_mutex_destroy(pthread_mutex_t *mutex) -{ - if (mutex == NULL) - return 1; - DeleteCriticalSection(mutex); - return 0; -} - -int pthread_mutex_lock(pthread_mutex_t *mutex) -{ - if (mutex == NULL) - return 1; - EnterCriticalSection(mutex); - return 0; -} - -int pthread_mutex_unlock(pthread_mutex_t *mutex) -{ - if (mutex == NULL) - return 1; - LeaveCriticalSection(mutex); - return 0; -} - -int pthread_cond_init(pthread_cond_t *cond, pthread_condattr_t *attr) -{ - (void)attr; - if (cond == NULL) - return 1; - InitializeConditionVariable(cond); - return 0; -} - -int pthread_cond_destroy(pthread_cond_t *cond) -{ - /* Windows does not have a destroy for conditionals */ - (void)cond; - return 0; -} - -int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex) -{ - if (cond == NULL || mutex == NULL) - return 1; - return pthread_cond_timedwait(cond, mutex, NULL); -} - -int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex, - const struct timespec *abstime) -{ - if (cond == NULL || mutex == NULL) - return 1; - if (!SleepConditionVariableCS(cond, mutex, timespec_to_ms(abstime))) - return 1; - return 0; -} - -int pthread_cond_signal(pthread_cond_t *cond) -{ - if (cond == NULL) - return 1; - WakeConditionVariable(cond); - return 0; -} - -int pthread_cond_broadcast(pthread_cond_t *cond) -{ - if (cond == NULL) - return 1; - WakeAllConditionVariable(cond); - return 0; -} - -int pthread_rwlock_init(pthread_rwlock_t *rwlock, const pthread_rwlockattr_t *attr) -{ - (void)attr; - if (rwlock == NULL) - return 1; - InitializeSRWLock(&(rwlock->lock)); - rwlock->exclusive = false; - return 0; -} - -int pthread_rwlock_destroy(pthread_rwlock_t *rwlock) -{ - (void)rwlock; - return 0; -} - -int pthread_rwlock_rdlock(pthread_rwlock_t *rwlock) -{ - if (rwlock == NULL) - return 1; - AcquireSRWLockShared(&(rwlock->lock)); - return 0; -} - -int pthread_rwlock_tryrdlock(pthread_rwlock_t *rwlock) -{ - if (rwlock == NULL) - return 1; - return !TryAcquireSRWLockShared(&(rwlock->lock)); -} - -int pthread_rwlock_wrlock(pthread_rwlock_t *rwlock) -{ - if (rwlock == NULL) - return 1; - AcquireSRWLockExclusive(&(rwlock->lock)); - rwlock->exclusive = true; - return 0; -} - -int pthread_rwlock_trywrlock(pthread_rwlock_t *rwlock) -{ - BOOLEAN ret; - - if (rwlock == NULL) - return 1; - - ret = TryAcquireSRWLockExclusive(&(rwlock->lock)); - if (ret) - rwlock->exclusive = true; - return ret; -} - -int pthread_rwlock_unlock(pthread_rwlock_t *rwlock) -{ - if (rwlock == NULL) - return 1; - - if (rwlock->exclusive) { - rwlock->exclusive = false; - ReleaseSRWLockExclusive(&(rwlock->lock)); - } else { - ReleaseSRWLockShared(&(rwlock->lock)); - } - return 0; -} - -int sched_yield() { - return (int)SwitchToThread(); -} - -void ms_to_timespec(struct timespec *ts, unsigned int ms) -{ - if (ts == NULL) - return; - ts->tv_sec = (ms / 1000) + time(NULL); - ts->tv_nsec = (ms % 1000) * 1000000; -} - -unsigned int timespec_to_ms(const struct timespec *abstime) -{ - if (abstime == NULL) - return INFINITE; - - return ((abstime->tv_sec - time(NULL)) * 1000) + (abstime->tv_nsec / 1000000); -} - -unsigned int pcthread_get_num_procs() -{ - SYSTEM_INFO sysinfo; - - GetSystemInfo(&sysinfo); - return sysinfo.dwNumberOfProcessors; -} - -#else - -#include -unsigned int pcthread_get_num_procs() -{ - return (unsigned int)sysconf(_SC_NPROCESSORS_ONLN); -} -#endif +/** +Copyright John Schember + +Permission is hereby granted, free of charge, to any person obtaining a copy of +this software and associated documentation files (the "Software"), to deal in +the Software without restriction, including without limitation the rights to +use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies +of the Software, and to permit persons to whom the Software is furnished to do +so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. + */ + +#include "common/pthreads_cross.h" + +#ifdef _WIN32 + +typedef struct { + SRWLOCK lock; + bool exclusive; +} pthread_rwlock_t; + +int pthread_rwlock_init(pthread_rwlock_t *rwlock, const pthread_rwlockattr_t *attr); +int pthread_rwlock_destroy(pthread_rwlock_t *rwlock); +int pthread_rwlock_rdlock(pthread_rwlock_t *rwlock); +int pthread_rwlock_tryrdlock(pthread_rwlock_t *rwlock); +int pthread_rwlock_wrlock(pthread_rwlock_t *rwlock); +int pthread_rwlock_trywrlock(pthread_rwlock_t *rwlock); +int pthread_rwlock_unlock(pthread_rwlock_t *rwlock); + +int pthread_create(pthread_t *thread, pthread_attr_t *attr, void *(*start_routine)(void *), void *arg) +{ + (void) attr; + + if (thread == NULL || start_routine == NULL) + return 1; + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-function-type" + *thread = (HANDLE) CreateThread(NULL, 0, (LPTHREAD_START_ROUTINE)start_routine, arg, 0, NULL); +#pragma GCC diagnostic pop + if (*thread == NULL) + return 1; + return 0; +} + +int pthread_join(pthread_t thread, void **value_ptr) +{ + (void)value_ptr; + WaitForSingleObject(thread, INFINITE); + CloseHandle(thread); + return 0; +} + +int pthread_detach(pthread_t thread) +{ + CloseHandle(thread); + return 0; +} + +int pthread_mutex_init(pthread_mutex_t *mutex, pthread_mutexattr_t *attr) +{ + (void)attr; + + if (mutex == NULL) + return 1; + + InitializeCriticalSection(mutex); + return 0; +} + +int pthread_mutex_destroy(pthread_mutex_t *mutex) +{ + if (mutex == NULL) + return 1; + DeleteCriticalSection(mutex); + return 0; +} + +int pthread_mutex_lock(pthread_mutex_t *mutex) +{ + if (mutex == NULL) + return 1; + EnterCriticalSection(mutex); + return 0; +} + +int pthread_mutex_unlock(pthread_mutex_t *mutex) +{ + if (mutex == NULL) + return 1; + LeaveCriticalSection(mutex); + return 0; +} + +int pthread_cond_init(pthread_cond_t *cond, pthread_condattr_t *attr) +{ + (void)attr; + if (cond == NULL) + return 1; + InitializeConditionVariable(cond); + return 0; +} + +int pthread_cond_destroy(pthread_cond_t *cond) +{ + /* Windows does not have a destroy for conditionals */ + (void)cond; + return 0; +} + +int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex) +{ + if (cond == NULL || mutex == NULL) + return 1; + return pthread_cond_timedwait(cond, mutex, NULL); +} + +int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex, + const struct timespec *abstime) +{ + if (cond == NULL || mutex == NULL) + return 1; + if (!SleepConditionVariableCS(cond, mutex, timespec_to_ms(abstime))) + return 1; + return 0; +} + +int pthread_cond_signal(pthread_cond_t *cond) +{ + if (cond == NULL) + return 1; + WakeConditionVariable(cond); + return 0; +} + +int pthread_cond_broadcast(pthread_cond_t *cond) +{ + if (cond == NULL) + return 1; + WakeAllConditionVariable(cond); + return 0; +} + +int pthread_rwlock_init(pthread_rwlock_t *rwlock, const pthread_rwlockattr_t *attr) +{ + (void)attr; + if (rwlock == NULL) + return 1; + InitializeSRWLock(&(rwlock->lock)); + rwlock->exclusive = false; + return 0; +} + +int pthread_rwlock_destroy(pthread_rwlock_t *rwlock) +{ + (void)rwlock; + return 0; +} + +int pthread_rwlock_rdlock(pthread_rwlock_t *rwlock) +{ + if (rwlock == NULL) + return 1; + AcquireSRWLockShared(&(rwlock->lock)); + return 0; +} + +int pthread_rwlock_tryrdlock(pthread_rwlock_t *rwlock) +{ + if (rwlock == NULL) + return 1; + return !TryAcquireSRWLockShared(&(rwlock->lock)); +} + +int pthread_rwlock_wrlock(pthread_rwlock_t *rwlock) +{ + if (rwlock == NULL) + return 1; + AcquireSRWLockExclusive(&(rwlock->lock)); + rwlock->exclusive = true; + return 0; +} + +int pthread_rwlock_trywrlock(pthread_rwlock_t *rwlock) +{ + BOOLEAN ret; + + if (rwlock == NULL) + return 1; + + ret = TryAcquireSRWLockExclusive(&(rwlock->lock)); + if (ret) + rwlock->exclusive = true; + return ret; +} + +int pthread_rwlock_unlock(pthread_rwlock_t *rwlock) +{ + if (rwlock == NULL) + return 1; + + if (rwlock->exclusive) { + rwlock->exclusive = false; + ReleaseSRWLockExclusive(&(rwlock->lock)); + } else { + ReleaseSRWLockShared(&(rwlock->lock)); + } + return 0; +} + +int sched_yield() { + return (int)SwitchToThread(); +} + +void ms_to_timespec(struct timespec *ts, unsigned int ms) +{ + if (ts == NULL) + return; + ts->tv_sec = (ms / 1000) + time(NULL); + ts->tv_nsec = (ms % 1000) * 1000000; +} + +unsigned int timespec_to_ms(const struct timespec *abstime) +{ + if (abstime == NULL) + return INFINITE; + + return ((abstime->tv_sec - time(NULL)) * 1000) + (abstime->tv_nsec / 1000000); +} + +unsigned int pcthread_get_num_procs() +{ + SYSTEM_INFO sysinfo; + + GetSystemInfo(&sysinfo); + return sysinfo.dwNumberOfProcessors; +} + +#else + +#include +unsigned int pcthread_get_num_procs() +{ + return (unsigned int)sysconf(_SC_NPROCESSORS_ONLN); +} +#endif diff --git a/vendor/apriltag/common/pthreads_cross.h b/vendor/apriltag/common/pthreads_cross.h index 5970c679..fc40c9ba 100644 --- a/vendor/apriltag/common/pthreads_cross.h +++ b/vendor/apriltag/common/pthreads_cross.h @@ -1,82 +1,82 @@ -/** -Copyright John Schember - -Permission is hereby granted, free of charge, to any person obtaining a copy of -this software and associated documentation files (the "Software"), to deal in -the Software without restriction, including without limitation the rights to -use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies -of the Software, and to permit persons to whom the Software is furnished to do -so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. -*/ - -#ifndef __CPTHREAD_H__ -#define __CPTHREAD_H__ - -#ifdef _WIN32 -#include -#include -#else -#include -#include -#endif -#include - -#ifdef _WIN32 - -typedef CRITICAL_SECTION pthread_mutex_t; -typedef void pthread_mutexattr_t; -typedef void pthread_attr_t; -typedef void pthread_condattr_t; -typedef void pthread_rwlockattr_t; -typedef HANDLE pthread_t; -typedef CONDITION_VARIABLE pthread_cond_t; - -#ifdef __cplusplus -extern "C" { -#endif -int pthread_create(pthread_t *thread, pthread_attr_t *attr, void *(*start_routine)(void *), void *arg); -int pthread_join(pthread_t thread, void **value_ptr); -int pthread_detach(pthread_t); - -int pthread_mutex_init(pthread_mutex_t *mutex, pthread_mutexattr_t *attr); -int pthread_mutex_destroy(pthread_mutex_t *mutex); -int pthread_mutex_lock(pthread_mutex_t *mutex); -int pthread_mutex_unlock(pthread_mutex_t *mutex); - -int pthread_cond_init(pthread_cond_t *cond, pthread_condattr_t *attr); -int pthread_cond_destroy(pthread_cond_t *cond); -int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex); -int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex, const struct timespec *abstime); -int pthread_cond_signal(pthread_cond_t *cond); -int pthread_cond_broadcast(pthread_cond_t *cond); - -int sched_yield(void); -#ifdef __cplusplus -} -#endif -#endif - - -#ifdef __cplusplus -extern "C" { -#endif -unsigned int pcthread_get_num_procs(); - -void ms_to_timespec(struct timespec *ts, unsigned int ms); -unsigned int timespec_to_ms(const struct timespec *abstime); -#ifdef __cplusplus -} -#endif - -#endif /* __CPTHREAD_H__ */ +/** +Copyright John Schember + +Permission is hereby granted, free of charge, to any person obtaining a copy of +this software and associated documentation files (the "Software"), to deal in +the Software without restriction, including without limitation the rights to +use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies +of the Software, and to permit persons to whom the Software is furnished to do +so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +#ifndef __CPTHREAD_H__ +#define __CPTHREAD_H__ + +#ifdef _WIN32 +#include +#include +#else +#include +#include +#endif +#include + +#ifdef _WIN32 + +typedef CRITICAL_SECTION pthread_mutex_t; +typedef void pthread_mutexattr_t; +typedef void pthread_attr_t; +typedef void pthread_condattr_t; +typedef void pthread_rwlockattr_t; +typedef HANDLE pthread_t; +typedef CONDITION_VARIABLE pthread_cond_t; + +#ifdef __cplusplus +extern "C" { +#endif +int pthread_create(pthread_t *thread, pthread_attr_t *attr, void *(*start_routine)(void *), void *arg); +int pthread_join(pthread_t thread, void **value_ptr); +int pthread_detach(pthread_t); + +int pthread_mutex_init(pthread_mutex_t *mutex, pthread_mutexattr_t *attr); +int pthread_mutex_destroy(pthread_mutex_t *mutex); +int pthread_mutex_lock(pthread_mutex_t *mutex); +int pthread_mutex_unlock(pthread_mutex_t *mutex); + +int pthread_cond_init(pthread_cond_t *cond, pthread_condattr_t *attr); +int pthread_cond_destroy(pthread_cond_t *cond); +int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex); +int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex, const struct timespec *abstime); +int pthread_cond_signal(pthread_cond_t *cond); +int pthread_cond_broadcast(pthread_cond_t *cond); + +int sched_yield(void); +#ifdef __cplusplus +} +#endif +#endif + + +#ifdef __cplusplus +extern "C" { +#endif +unsigned int pcthread_get_num_procs(); + +void ms_to_timespec(struct timespec *ts, unsigned int ms); +unsigned int timespec_to_ms(const struct timespec *abstime); +#ifdef __cplusplus +} +#endif + +#endif /* __CPTHREAD_H__ */ diff --git a/vendor/apriltag/common/time_util.h b/vendor/apriltag/common/time_util.h index c1840495..a2ead4ca 100644 --- a/vendor/apriltag/common/time_util.h +++ b/vendor/apriltag/common/time_util.h @@ -32,7 +32,11 @@ either expressed or implied, of the Regents of The University of Michigan. #include #ifdef _WIN32 -#include +#if defined __has_include && __has_include ("winsock2.h") +#include +#else +#include +#endif typedef long long suseconds_t; #endif diff --git a/vendor/apriltag/common/unionfind.h b/vendor/apriltag/common/unionfind.h index fdfef9dc..b6caaa68 100644 --- a/vendor/apriltag/common/unionfind.h +++ b/vendor/apriltag/common/unionfind.h @@ -82,26 +82,20 @@ static inline uint32_t unionfind_get_representative(unionfind_t *uf, uint32_t id // version above. static inline uint32_t unionfind_get_representative(unionfind_t *uf, uint32_t id) { - uint32_t root = uf->parent[id]; // unititialized node, so set to self - if (root == 0xffffffff) { + if (uf->parent[id] == 0xffffffff) { uf->parent[id] = id; return id; } - // chase down the root - while (uf->parent[root] != root) { - root = uf->parent[root]; + // Path halving: make every node point to its grandparent (single pass) + // This is simpler and faster than full path compression while still effective + while (uf->parent[id] != id) { + uf->parent[id] = uf->parent[uf->parent[id]]; // Point to grandparent + id = uf->parent[id]; // Move to grandparent } - // go back and collapse the tree. - while (uf->parent[id] != root) { - uint32_t tmp = uf->parent[id]; - uf->parent[id] = root; - id = tmp; - } - - return root; + return id; } static inline uint32_t unionfind_get_set_size(unionfind_t *uf, uint32_t id) diff --git a/vendor/apriltag/common/workerpool.c b/vendor/apriltag/common/workerpool.c index 359abfef..6b73541c 100644 --- a/vendor/apriltag/common/workerpool.c +++ b/vendor/apriltag/common/workerpool.c @@ -30,6 +30,7 @@ either expressed or implied, of the Regents of The University of Michigan. #define __USE_GNU #include "common/pthreads_cross.h" #include +#include #include #include #ifdef _WIN32 @@ -51,6 +52,7 @@ struct workerpool { pthread_mutex_t mutex; pthread_cond_t startcond; // used to signal the availability of work + bool start_predicate; // predicate that prevents spurious wakeups on startcond pthread_cond_t endcond; // used to signal completion of all work int end_count; // how many threads are done? @@ -70,7 +72,7 @@ void *worker_thread(void *p) struct task *task; pthread_mutex_lock(&wp->mutex); - while (wp->taskspos == zarray_size(wp->tasks)) { + while (wp->taskspos == zarray_size(wp->tasks) || !wp->start_predicate) { wp->end_count++; pthread_cond_broadcast(&wp->endcond); pthread_cond_wait(&wp->startcond, &wp->mutex); @@ -98,6 +100,7 @@ workerpool_t *workerpool_create(int nthreads) workerpool_t *wp = calloc(1, sizeof(workerpool_t)); wp->nthreads = nthreads; wp->tasks = zarray_create(sizeof(struct task)); + wp->start_predicate = false; if (nthreads > 1) { wp->threads = calloc(wp->nthreads, sizeof(pthread_t)); @@ -114,6 +117,13 @@ workerpool_t *workerpool_create(int nthreads) return NULL; } } + + // Wait for the worker threads to be ready + pthread_mutex_lock(&wp->mutex); + while (wp->end_count < wp->nthreads) { + pthread_cond_wait(&wp->endcond, &wp->mutex); + } + pthread_mutex_unlock(&wp->mutex); } return wp; @@ -130,6 +140,7 @@ void workerpool_destroy(workerpool_t *wp) workerpool_add_task(wp, NULL, NULL); pthread_mutex_lock(&wp->mutex); + wp->start_predicate = true; pthread_cond_broadcast(&wp->startcond); pthread_mutex_unlock(&wp->mutex); @@ -157,7 +168,13 @@ void workerpool_add_task(workerpool_t *wp, void (*f)(void *p), void *p) t.f = f; t.p = p; - zarray_add(wp->tasks, &t); + if (wp->nthreads > 1) { + pthread_mutex_lock(&wp->mutex); + zarray_add(wp->tasks, &t); + pthread_mutex_unlock(&wp->mutex); + } else { + zarray_add(wp->tasks, &t); + } } void workerpool_run_single(workerpool_t *wp) @@ -175,9 +192,9 @@ void workerpool_run_single(workerpool_t *wp) void workerpool_run(workerpool_t *wp) { if (wp->nthreads > 1) { - wp->end_count = 0; - pthread_mutex_lock(&wp->mutex); + wp->end_count = 0; + wp->start_predicate = true; pthread_cond_broadcast(&wp->startcond); while (wp->end_count < wp->nthreads) { @@ -185,9 +202,9 @@ void workerpool_run(workerpool_t *wp) pthread_cond_wait(&wp->endcond, &wp->mutex); } - pthread_mutex_unlock(&wp->mutex); - wp->taskspos = 0; + wp->start_predicate = false; + pthread_mutex_unlock(&wp->mutex); zarray_clear(wp->tasks); @@ -198,7 +215,7 @@ void workerpool_run(workerpool_t *wp) int workerpool_get_nprocs() { -#ifdef WIN32 +#ifdef _WIN32 SYSTEM_INFO sysinfo; GetSystemInfo(&sysinfo); return sysinfo.dwNumberOfProcessors; diff --git a/vendor/apriltag/common/zmaxheap.c b/vendor/apriltag/common/zmaxheap.c index 3ca30af7..75de9950 100644 --- a/vendor/apriltag/common/zmaxheap.c +++ b/vendor/apriltag/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/vendor/apriltag/example/.gitignore b/vendor/apriltag/example/.gitignore deleted file mode 100644 index 1cef2e8f..00000000 --- a/vendor/apriltag/example/.gitignore +++ /dev/null @@ -1 +0,0 @@ -apriltag_demo diff --git a/vendor/apriltag/example/Makefile b/vendor/apriltag/example/Makefile deleted file mode 100644 index 6027428e..00000000 --- a/vendor/apriltag/example/Makefile +++ /dev/null @@ -1,32 +0,0 @@ -CC = gcc -CXX = g++ - -CPPFLAGS = -I.. `pkg-config --cflags opencv4` -CFLAGS = -g -std=gnu99 -Wall -Wno-unused-parameter -Wno-unused-function -O3 -CXXFLAGS = -g -Wall -O3 -LDFLAGS = -lpthread -lm - -TARGETS := apriltag_demo opencv_demo - -.PHONY: all -all: apriltag_demo opencv_demo - -apriltag_demo: apriltag_demo.o ../libapriltag.a - @echo " [$@]" - @$(CC) -o $@ $^ $(LDFLAGS) - -opencv_demo: opencv_demo.o ../libapriltag.a - @echo " [$@]" - @$(CXX) -o $@ $^ $(LDFLAGS) `pkg-config --libs opencv4` - -%.o: %.c - @echo " $@" - @$(CC) -o $@ -c $< $(CFLAGS) $(CPPFLAGS) - -%.o: %.cc - @echo " $@" - @$(CXX) -o $@ -c $< $(CXXFLAGS) $(CPPFLAGS) - -.PHONY: clean -clean: - @rm -rf *.o $(TARGETS) diff --git a/vendor/apriltag/example/README b/vendor/apriltag/example/README deleted file mode 100644 index 5b3167df..00000000 --- a/vendor/apriltag/example/README +++ /dev/null @@ -1 +0,0 @@ -These example programs are meant for distribution, and thus will not build in the april2 tree without modifications. diff --git a/vendor/apriltag/install.sh b/vendor/apriltag/install.sh deleted file mode 100755 index 04bb5b2f..00000000 --- a/vendor/apriltag/install.sh +++ /dev/null @@ -1,16 +0,0 @@ -#!/bin/sh -e - -# Usage: install.sh TARGET [RELATIVE PATHS ...] -# -# e.g. ./install.sh /usr/local foo/file1 foo/file2 ... -# This creates the files /usr/local/foo/file1 and /usr/local/foo/file2 - -TARGETDIR=$1 -shift - -for src in "$@"; do - dest=$TARGETDIR/$src - mkdir -p $(dirname $dest) - cp $src $dest - echo $dest -done diff --git a/vendor/apriltag/package.xml b/vendor/apriltag/package.xml index f235a1af..8a6d2d76 100644 --- a/vendor/apriltag/package.xml +++ b/vendor/apriltag/package.xml @@ -2,7 +2,7 @@ apriltag - 3.3.0 + 3.4.5 AprilTag detector library Max Krogius @@ -16,6 +16,7 @@ Max Krogius cmake + python3-dev python3-numpy diff --git a/vendor/apriltag/python_build_flags.py b/vendor/apriltag/python_build_flags.py deleted file mode 100644 index f3f0e79a..00000000 --- a/vendor/apriltag/python_build_flags.py +++ /dev/null @@ -1,34 +0,0 @@ -from __future__ import print_function -import sysconfig -import re -import numpy as np -conf = sysconfig.get_config_vars() - -print('CFLAGS', end=';') -c_flags = [] -# Grab compiler flags minus the compiler itself. -c_flags.extend(conf.get('CC', '').split()[2:]) -c_flags.extend(conf.get('CFLAGS', '').split()) -c_flags.extend(conf.get('CCSHARED', '').split()) -c_flags.append('-I{}'.format(conf.get('INCLUDEPY', ''))) -c_flags.append('-I{}'.format(np.get_include())) -c_flags.append('-Wno-strict-prototypes') -c_flags = [x for x in c_flags if not x.startswith('-O')] -print(' '.join(c_flags), end=';') - - -print('LINKER', end=';') -print(conf.get('BLDSHARED', '').split()[0], end=';') - -print('LDFLAGS', end=';') -print(' '.join(conf.get('BLDSHARED', '').split()[1:]) + ' ' + conf.get('BLDLIBRARY', '') + ' ' + conf.get('LDFLAGS', ''), end=';') - -print('EXT_SUFFIX', end=';') -ext_suffix = '.so' -if 'EXT_SUFFIX' in conf: - ext_suffix = conf['EXT_SUFFIX'] -elif 'MULTIARCH' in conf: - ext_suffix = '.' + conf['MULTIARCH'] + '.so' - -print(ext_suffix, end=';') - diff --git a/vendor/apriltag/tag36h10.c b/vendor/apriltag/tag36h10.c index 44a129e7..9c02c34c 100644 --- a/vendor/apriltag/tag36h10.c +++ b/vendor/apriltag/tag36h10.c @@ -1,3 +1,30 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + #include #include "tag36h10.h" diff --git a/vendor/apriltag/test/CMakeLists.txt b/vendor/apriltag/test/CMakeLists.txt new file mode 100644 index 00000000..094182c6 --- /dev/null +++ b/vendor/apriltag/test/CMakeLists.txt @@ -0,0 +1,50 @@ +add_library(getline OBJECT getline.c) + +add_executable(test_detection test_detection.c) +target_link_libraries(test_detection ${PROJECT_NAME} getline) + +# Add the pose estimation test executable +add_executable(test_tag_pose_estimation test_tag_pose_estimation.c) +target_link_libraries(test_tag_pose_estimation ${PROJECT_NAME} getline) + +# test images with true detection +set(TEST_IMAGE_NAMES + "33369213973_9d9bb4cc96_c" + "34085369442_304b6bafd9_c" + "34139872896_defdb2f8d9_c" +) + +foreach(IMG IN LISTS TEST_IMAGE_NAMES) + add_test(NAME test_detection_${IMG} + COMMAND $ data/${IMG} + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + ) +endforeach() + +# Add pose estimation tests for each image +foreach(IMG IN LISTS TEST_IMAGE_NAMES) + add_test(NAME test_tag_pose_estimation_${IMG} + COMMAND $ + data/${IMG}.jpg data/${IMG}.txt + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + ) +endforeach() + +# Quick decode test +file(GLOB COMMON_SRC "${CMAKE_SOURCE_DIR}/common/*.c") +file(GLOB TAG_FILES "${CMAKE_SOURCE_DIR}/tag*.c") + +add_executable(test_quick_decode test_quick_decode.c "${CMAKE_SOURCE_DIR}/apriltag_quad_thresh.c" ${COMMON_SRC} ${TAG_FILES}) +target_include_directories(test_quick_decode PRIVATE "${CMAKE_SOURCE_DIR}") + +if (UNIX) + target_link_libraries(test_quick_decode m) +endif() + +if(NOT MSVC) + find_package(Threads REQUIRED) + target_link_libraries(test_quick_decode Threads::Threads) +endif() + +add_test(NAME test_quick_decode COMMAND test_quick_decode) + diff --git a/vendor/apriltag/test/data/33369213973_9d9bb4cc96_c.jpg b/vendor/apriltag/test/data/33369213973_9d9bb4cc96_c.jpg new file mode 100644 index 00000000..2526c1db Binary files /dev/null and b/vendor/apriltag/test/data/33369213973_9d9bb4cc96_c.jpg differ diff --git a/vendor/apriltag/test/data/33369213973_9d9bb4cc96_c.txt b/vendor/apriltag/test/data/33369213973_9d9bb4cc96_c.txt new file mode 100644 index 00000000..1b4ff352 --- /dev/null +++ b/vendor/apriltag/test/data/33369213973_9d9bb4cc96_c.txt @@ -0,0 +1,12 @@ +0, (277.9247 327.4884), (250.3570 329.7265), (251.6721 357.3356), (279.4720 354.6839) +0, (352.8467 346.8030), (328.9969 344.6012), (330.2313 372.6704), (354.3267 375.5784) +0, (414.9603 338.8232), (403.0023 333.3730), (404.3122 361.6505), (414.9944 366.4854) +0, (449.0000 335.4156), (421.7478 338.2029), (422.8918 366.6338), (449.0000 363.4273) +0, (451.8521 324.5121), (452.9211 352.3356), (462.6942 357.3213), (461.5424 329.8951) +0, (465.7551 341.7513), (466.5209 370.4993), (480.3520 375.3039), (478.8691 346.5728) +0, (512.0000 371.4803), (512.0000 342.4403), (485.9187 345.2694), (486.2385 374.6548) +0, (535.6610 364.6997), (524.0469 359.3780), (523.9808 387.9674), (537.1108 394.5279) +0, (571.5667 361.6513), (543.9484 364.7314), (544.2009 394.8912), (572.1226 391.7314) +0, (620.9534 374.3775), (621.1776 405.3268), (652.4188 403.1609), (650.3995 373.0815) +0, (639.7830 425.7310), (641.0399 458.6032), (669.9742 462.4909), (670.0141 429.6474) +0, (760.8949 429.0904), (726.9314 428.9567), (727.1409 461.2701), (761.0534 462.3109) diff --git a/vendor/apriltag/test/data/34085369442_304b6bafd9_c.jpg b/vendor/apriltag/test/data/34085369442_304b6bafd9_c.jpg new file mode 100644 index 00000000..130bf673 Binary files /dev/null and b/vendor/apriltag/test/data/34085369442_304b6bafd9_c.jpg differ diff --git a/vendor/apriltag/test/data/34085369442_304b6bafd9_c.txt b/vendor/apriltag/test/data/34085369442_304b6bafd9_c.txt new file mode 100644 index 00000000..8ec5d7c4 --- /dev/null +++ b/vendor/apriltag/test/data/34085369442_304b6bafd9_c.txt @@ -0,0 +1,25 @@ +0, (7.1998 367.7382), (8.2826 387.0433), (21.8467 388.8047), (22.3012 369.3906) +0, (40.0000 385.2895), (40.0000 366.5399), (26.9912 368.8071), (27.7584 388.3069) +0, (50.2235 357.2749), (62.8278 355.8416), (64.3150 337.5952), (49.2334 339.3408) +0, (56.2142 395.9775), (75.7085 395.7057), (76.4279 376.0304), (55.7100 376.1390) +0, (136.5859 405.9852), (136.6868 386.9614), (117.9318 385.6759), (119.3466 405.7574) +0, (136.8157 349.8532), (137.4467 331.6211), (119.2479 332.8495), (120.3091 350.9236) +0, (214.1390 345.1158), (231.0276 343.9631), (230.9505 326.2049), (213.9404 327.2949) +0, (216.2888 412.8570), (234.8337 413.0699), (236.6666 393.5693), (215.5450 393.1030) +0, (311.3437 343.7850), (328.7296 343.6689), (328.3899 326.1203), (310.4015 325.7319) +0, (319.0783 390.0601), (300.7925 391.4020), (300.1619 411.2209), (318.8514 409.6298) +0, (357.8598 346.5026), (372.8396 347.3288), (374.1022 329.4759), (356.7228 328.0721) +0, (400.0000 282.9315), (400.0000 300.0000), (417.1274 300.0000), (416.7581 283.1546) +0, (416.5377 352.6791), (430.8029 354.3849), (431.4457 336.2527), (415.8975 334.5414) +0, (437.0000 353.3342), (446.9166 350.9822), (446.7385 333.0705), (437.0000 335.3143) +0, (466.8379 288.1093), (467.5887 271.7813), (456.9818 269.5371), (457.0370 285.9229) +0, (472.0643 272.4668), (472.1009 288.5338), (484.8149 287.5498), (485.0922 270.7832) +0, (492.0000 247.0000), (492.0000 263.0000), (507.0938 263.0000), (507.0838 247.0000) +0, (542.8355 351.0359), (543.3651 333.1548), (525.1558 332.4807), (525.6393 350.9261) +0, (615.1451 276.3273), (614.9308 293.1076), (629.6509 292.7443), (630.5632 275.8616) +0, (630.5645 330.1998), (630.6873 348.0871), (646.7246 347.8124), (647.3389 329.9061) +0, (678.0500 302.4676), (677.9111 285.4095), (666.0000 283.6531), (666.0000 300.7083) +0, (683.0000 303.2291), (694.9082 301.7316), (695.1923 285.0323), (683.0000 286.0783) +0, (696.0000 107.8969), (696.0000 121.2508), (707.5752 120.6790), (708.6246 108.2199) +0, (712.3666 321.4679), (695.3271 319.9513), (695.3467 337.6513), (711.9149 339.4104) +0, (734.1375 326.0000), (750.7946 326.0000), (751.4586 308.0991), (733.6561 308.1078) diff --git a/vendor/apriltag/test/data/34139872896_defdb2f8d9_c.jpg b/vendor/apriltag/test/data/34139872896_defdb2f8d9_c.jpg new file mode 100644 index 00000000..f93d8362 Binary files /dev/null and b/vendor/apriltag/test/data/34139872896_defdb2f8d9_c.jpg differ diff --git a/vendor/apriltag/test/data/34139872896_defdb2f8d9_c.txt b/vendor/apriltag/test/data/34139872896_defdb2f8d9_c.txt new file mode 100644 index 00000000..b2d72512 --- /dev/null +++ b/vendor/apriltag/test/data/34139872896_defdb2f8d9_c.txt @@ -0,0 +1,10 @@ +0, (329.0657 399.3667), (285.7438 402.8478), (286.5317 446.8641), (330.4509 443.1603) +0, (422.3676 450.0573), (421.1896 405.5042), (376.7516 407.5195), (378.1724 452.5206) +0, (449.6035 294.2907), (408.7995 289.9406), (394.2166 316.1437), (435.2841 319.5461) +0, (450.7480 281.4591), (444.8973 246.3509), (404.0564 242.6216), (409.2780 277.5639) +0, (585.6249 383.3414), (587.1271 427.5599), (608.1880 435.4088), (606.5087 391.2607) +0, (658.3193 429.8612), (656.9769 385.1598), (616.7130 389.3761), (618.0273 434.5546) +0, (695.4741 419.9608), (676.5197 411.5248), (677.6697 456.8612), (697.3136 465.9787) +0, (708.7199 356.2537), (723.1980 347.7975), (681.8147 345.8170), (666.7383 354.4589) +0, (711.3130 364.1447), (678.2820 360.0336), (652.4395 366.2831), (685.0097 370.6662) +0, (751.8777 416.2984), (708.5916 420.4094), (709.7285 466.8840), (753.3184 462.5622) diff --git a/vendor/apriltag/test/data/README.md b/vendor/apriltag/test/data/README.md new file mode 100644 index 00000000..c68873ed --- /dev/null +++ b/vendor/apriltag/test/data/README.md @@ -0,0 +1,6 @@ +Test image from NASA Swarmathon competition at KSC. Licenced under "CC BY-SA 2.0 DEED". + +Sources: +- https://www.flickr.com/photos/nasakennedy/33369213973/ +- https://www.flickr.com/photos/nasakennedy/34085369442/ +- https://www.flickr.com/photos/nasakennedy/34139872896/ diff --git a/vendor/apriltag/test/getline.c b/vendor/apriltag/test/getline.c new file mode 100644 index 00000000..a79323e5 --- /dev/null +++ b/vendor/apriltag/test/getline.c @@ -0,0 +1,55 @@ +#include "getline.h" +#include +#include + +// https://stackoverflow.com/a/47229318/8144672 +/* 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 apriltag_test_getline(char **lineptr, size_t *n, FILE *stream) { + size_t pos; + int c; + + if (lineptr == NULL || stream == NULL || n == NULL) { + errno = EINVAL; + return -1; + } + + c = getc(stream); + if (c == EOF) { + return -1; + } + + if (*lineptr == NULL) { + *lineptr = malloc(128); + if (*lineptr == NULL) { + return -1; + } + *n = 128; + } + + pos = 0; + while(c != EOF) { + if (pos + 1 >= *n) { + size_t new_size = *n + (*n >> 2); + if (new_size < 128) { + new_size = 128; + } + char *new_ptr = realloc(*lineptr, new_size); + if (new_ptr == NULL) { + return -1; + } + *n = new_size; + *lineptr = new_ptr; + } + + ((unsigned char *)(*lineptr))[pos ++] = c; + if (c == '\n') { + break; + } + c = getc(stream); + } + + (*lineptr)[pos] = '\0'; + return pos; +} diff --git a/vendor/apriltag/test/getline.h b/vendor/apriltag/test/getline.h new file mode 100644 index 00000000..ba190730 --- /dev/null +++ b/vendor/apriltag/test/getline.h @@ -0,0 +1,9 @@ +#pragma once + +#include +#include +#include + +typedef intptr_t ssize_t; + +ssize_t apriltag_test_getline(char **lineptr, size_t *n, FILE *stream); diff --git a/vendor/apriltag/test/test_detection.c b/vendor/apriltag/test/test_detection.c new file mode 100644 index 00000000..2912a6ac --- /dev/null +++ b/vendor/apriltag/test/test_detection.c @@ -0,0 +1,170 @@ +#include +#include +#include +#include +#include + +#include "getline.h" + + +char* +format(const char* fmt, ...) +{ + va_list args1; + va_list args2; + + va_start(args1, fmt); + va_copy(args2, args1); + + const int N = vsnprintf(NULL, 0, fmt, args1); + va_end(args1); + + if (N < 0) { + va_end(args2); + return NULL; + } + + char* res_fmt = calloc(N+1, sizeof(char)); + const int n = vsnprintf(res_fmt, N+1, fmt, args2); + va_end(args2); + + if (n != N) { + free(res_fmt); + return NULL; + } + + // caller has to free returned string + return res_fmt; +} + +int +detection_compare_function(const apriltag_detection_t *a, const apriltag_detection_t *b) +{ + if (a->id != b->id) { + return copysign(1, a->id - b->id); + } + + 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; +} + +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 +main(int argc, char *argv[]) +{ + if (argc!=2) { + return EXIT_FAILURE; + } + + // load image + char* const path_img = format("%s.jpg", argv[1]); + pjpeg_t *pjpeg = pjpeg_create_from_file(path_img, 0, NULL); + image_u8_t *im = pjpeg_to_u8_baseline(pjpeg); + free(path_img); + + // load true detection + char* const path_det_true = format("%s.txt", argv[1]); + FILE *fp = fopen(path_det_true, "r"); + if (fp == NULL) { + return EXIT_FAILURE; + } + free(path_det_true); + + apriltag_detector_t *td = apriltag_detector_create(); + td->quad_decimate = 1; + td->refine_edges = false; + apriltag_family_t *tf = tag36h11_create(); + apriltag_detector_add_family(td, tf); + + 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_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_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 = apriltag_test_getline(&line, &len, fp); + if (nread == -1) { + free(line); + return EXIT_FAILURE; + } + + printf("Got: %s\n", det_fmt); + printf("Expected: %s\n", line); + + // 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; + } + + free(det_fmt); + free(line); + } + + // check that we compared the expected amount of detections + // if there are no "true" detections left, we should be at the end of the file + // if there are no "detected" detections left, we should be at the end of the array + if ((fgetc(fp) != -1) && (i != zarray_size(detections))) { + return EXIT_FAILURE; + } + + fclose(fp); + + apriltag_detections_destroy(detections); + image_u8_destroy(im); + pjpeg_destroy(pjpeg); + + apriltag_detector_destroy(td); + tag36h11_destroy(tf); + + if (!ok) { + return EXIT_FAILURE; + } + + return EXIT_SUCCESS; +} diff --git a/vendor/apriltag/test/test_quick_decode.c b/vendor/apriltag/test/test_quick_decode.c new file mode 100644 index 00000000..91e035ac --- /dev/null +++ b/vendor/apriltag/test/test_quick_decode.c @@ -0,0 +1,126 @@ +#include +#include +#include + +#include "apriltag.h" +#include "tag16h5.h" +#include "tag25h9.h" +#include "tag36h10.h" +#include "tag36h11.h" +#include "tagCircle21h7.h" +#include "tagCircle49h12.h" +#include "tagCustom48h12.h" +#include "tagStandard41h12.h" +#include "tagStandard52h13.h" + +#include "../apriltag.c" + +void test_family(apriltag_family_t *fam) { + printf("Testing family %s with %d codes, %d bits\n", fam->name, fam->ncodes, fam->nbits); + + // Calculate theoretical max correction + int limit = (fam->h - 1) / 2; + if (limit > 3) limit = 3; + printf("Family %s: h=%d, max correction tested=%d\n", fam->name, fam->h, limit); + + apriltag_detector_t *td = apriltag_detector_create(); + // Enable up to limit bits correction + apriltag_detector_add_family_bits(td, fam, limit); + + // Validate initialization + struct quick_decode *qd = (struct quick_decode*) fam->impl; + if (!qd) { + printf("Failed to init quick_decode for %s\n", fam->name); + exit(1); + } + + if (qd->maxhamming != limit) { + printf("Failed to set maxhamming to %d for %s\n", limit, fam->name); + exit(1); + } + + int nbits = fam->nbits; + + for (uint32_t i = 0; i < fam->ncodes; i++) { + uint64_t code = fam->codes[i]; + + // Test 0 errors + struct quick_decode_result res; + quick_decode_codeword(fam, code, &res); + if (res.id != i || res.hamming != 0) { + printf("Failed 0 errors: code %u, got id %d hamming %d\n", i, res.id, res.hamming); + exit(1); + } + + if (limit < 1) continue; + + // Test 1 bit error + for (int b1 = 0; b1 < nbits; b1++) { + uint64_t c1 = code ^ (1ULL << b1); + quick_decode_codeword(fam, c1, &res); + if (res.id != i || res.hamming != 1) { + printf("Failed 1 error: code %u bit %d, got id %d hamming %d\n", i, b1, res.id, res.hamming); + exit(1); + } + + if (limit < 2) continue; + + // Test 2 bit errors + for (int b2 = b1 + 1; b2 < nbits; b2++) { + uint64_t c2 = c1 ^ (1ULL << b2); + quick_decode_codeword(fam, c2, &res); + if (res.id != i || res.hamming != 2) { + printf("Failed 2 errors: code %u bits %d,%d, got id %d hamming %d\n", i, b1, b2, res.id, res.hamming); + exit(1); + } + + if (limit < 3) continue; + + // Test 3 bit errors + for (int b3 = b2 + 1; b3 < nbits; b3++) { + uint64_t c3 = c2 ^ (1ULL << b3); + quick_decode_codeword(fam, c3, &res); + if (res.id != i || res.hamming != 3) { + printf("Failed 3 errors: code %u bits %d,%d,%d, got id %d hamming %d\n", i, b1, b2, b3, res.id, res.hamming); + exit(1); + } + } + } + } + } + + apriltag_detector_destroy(td); + printf("Family %s passed.\n", fam->name); +} + +int main() { + apriltag_family_t *fams[] = { + tag16h5_create(), + tag25h9_create(), + tag36h10_create(), + tag36h11_create(), + tagCircle21h7_create(), + tagCircle49h12_create(), + tagCustom48h12_create(), + tagStandard41h12_create(), + tagStandard52h13_create(), + NULL + }; + + for (int i = 0; fams[i]; i++) { + test_family(fams[i]); + } + + tag16h5_destroy(fams[0]); + tag25h9_destroy(fams[1]); + tag36h10_destroy(fams[2]); + tag36h11_destroy(fams[3]); + tagCircle21h7_destroy(fams[4]); + tagCircle49h12_destroy(fams[5]); + tagCustom48h12_destroy(fams[6]); + tagStandard41h12_destroy(fams[7]); + tagStandard52h13_destroy(fams[8]); + + printf("All quick_decode tests passed!\n"); + return 0; +} diff --git a/vendor/apriltag/test/test_tag_pose_estimation.c b/vendor/apriltag/test/test_tag_pose_estimation.c new file mode 100644 index 00000000..1aacc495 --- /dev/null +++ b/vendor/apriltag/test/test_tag_pose_estimation.c @@ -0,0 +1,209 @@ +#include +#include +#include +#include +#include +#include // Added this header file to support variable arguments + +#include "apriltag.h" +#include "apriltag_pose.h" +#include "common/matd.h" +#include "common/pjpeg.h" +#include "test/getline.h" + +// Declare tag family creation functions - use tag36h11 instead of tagStandard41h12 +apriltag_family_t *tag36h11_create(void); +void tag36h11_destroy(apriltag_family_t *tf); + +// format function copied from test_detection.c +char* format(const char *fmt, ...) { + va_list args; + va_start(args, fmt); + int required = vsnprintf(NULL, 0, fmt, args); + va_end(args); + + if (required < 0) { + return NULL; + } + + char *buffer = malloc(required + 1); + if (!buffer) { + return NULL; + } + + va_start(args, fmt); + int result = vsnprintf(buffer, required + 1, fmt, args); + va_end(args); + + if (result < 0) { + free(buffer); + return NULL; + } + + return buffer; +} + +// Parse expected detection results +int parse_expected_detection(const char* line, int* id, double corners[4][2]) { + char* line_copy = strdup(line); + if (!line_copy) { + return 0; + } + + // Split ID and corners using comma + char* token = strtok(line_copy, ","); + if (!token) { + free(line_copy); + return 0; + } + + *id = atoi(token); + + // Parse four corners + for (int i = 0; i < 4; i++) { + token = strtok(NULL, " ()"); + if (!token) { + free(line_copy); + return 0; + } + corners[i][0] = atof(token); + + token = strtok(NULL, " ()"); + if (!token) { + free(line_copy); + return 0; + } + corners[i][1] = atof(token); + } + + free(line_copy); + return 1; +} + +int main(int argc, char *argv[]) { + if (argc < 3) { + printf("Usage: %s \n", argv[0]); + printf("Example: %s test/data/33369213973_9d9bb4cc96_c.jpg test/data/33369213973_9d9bb4cc96_c.txt\n", argv[0]); + return 1; + } + + const char *image_path = argv[1]; + const char *expected_data_path = argv[2]; + + printf("Testing tag pose estimation with image: %s\n", image_path); + printf("Expected data file: %s\n", expected_data_path); + + // Create AprilTag detector - use tag36h11 family + apriltag_detector_t *td = apriltag_detector_create(); + apriltag_family_t *tf = tag36h11_create(); + apriltag_detector_add_family(td, tf); + + // Set detector parameters - use same parameters as test_detection.c + td->quad_decimate = 1.0; // Consistent with test_detection.c + td->quad_sigma = 0.0; + td->nthreads = 4; + td->debug = 0; + td->refine_edges = 0; // Consistent with test_detection.c + // Remove non-existent parameters + // td->refine_decode = 0; + // td->refine_pose = 0; + + // Load image + int pjpeg_error; + pjpeg_t *pjpeg = pjpeg_create_from_file(image_path, 0, &pjpeg_error); // Fix function call, add error parameter + if (!pjpeg || pjpeg_error) { + printf("Failed to load image: %s, error code: %d\n", image_path, pjpeg_error); + return 1; + } + + image_u8_t *im = pjpeg_to_u8_baseline(pjpeg); + pjpeg_destroy(pjpeg); + + if (!im) { + printf("Failed to decode image: %s\n", image_path); + return 1; + } + + // Perform detection + zarray_t *detections = apriltag_detector_detect(td, im); + + // Read expected data file + FILE *expected_file = fopen(expected_data_path, "r"); + if (!expected_file) { + printf("Failed to open expected data file: %s\n", expected_data_path); + image_u8_destroy(im); + apriltag_detections_destroy(detections); + apriltag_detector_destroy(td); + tag36h11_destroy(tf); + return 1; + } + + printf("Found %d detections in image\n", zarray_size(detections)); + + // Perform pose estimation test for each detection + int num_detections = zarray_size(detections); + int pose_estimation_success = 0; + + for (int i = 0; i < num_detections; i++) { + apriltag_detection_t *det; + zarray_get(detections, i, &det); + + printf("Processing detection %d: tag ID %d\n", i, det->id); + + // Set camera parameters and tag size + apriltag_detection_info_t info; + info.det = det; + info.tagsize = 0.16; // Assume tag size is 16cm + info.fx = 600.0; // Assume focal length + info.fy = 600.0; + info.cx = im->width / 2.0; // Assume optical center is at image center + info.cy = im->height / 2.0; + + // Perform pose estimation + apriltag_pose_t pose; + double err = estimate_tag_pose(&info, &pose); + + printf(" Estimated pose for tag %d:\n", det->id); + printf(" Translation: [%f, %f, %f]\n", + MATD_EL(pose.t, 0, 0), + MATD_EL(pose.t, 1, 0), + MATD_EL(pose.t, 2, 0)); + printf(" Rotation matrix:\n"); + printf(" [%f, %f, %f]\n", + MATD_EL(pose.R, 0, 0), MATD_EL(pose.R, 0, 1), MATD_EL(pose.R, 0, 2)); + printf(" [%f, %f, %f]\n", + MATD_EL(pose.R, 1, 0), MATD_EL(pose.R, 1, 1), MATD_EL(pose.R, 1, 2)); + printf(" [%f, %f, %f]\n", + MATD_EL(pose.R, 2, 0), MATD_EL(pose.R, 2, 1), MATD_EL(pose.R, 2, 2)); + printf(" Reprojection error: %f\n", err); + + // Check if pose estimation was successful + if (pose.R != NULL && pose.t != NULL && err < 1.0) { // Assume error less than 1.0 means success + pose_estimation_success++; + } + + // Free pose memory + if (pose.R) matd_destroy(pose.R); + if (pose.t) matd_destroy(pose.t); + } + + // Output test results + printf("\nPose estimation test results:\n"); + printf(" Successful pose estimations: %d/%d\n", pose_estimation_success, num_detections); + + if (pose_estimation_success == num_detections && num_detections > 0) { + printf(" All pose estimations successful!\n"); + } else if (num_detections > 0) { + printf(" Some pose estimations failed.\n"); + } else { + printf(" No detections found in image.\n"); + } + + // Clean up resources + image_u8_destroy(im); + apriltag_detections_destroy(detections); + apriltag_detector_destroy(td); + tag36h11_destroy(tf); + + return (pose_estimation_success == num_detections && num_detections > 0) ? 0 : 1; +} diff --git a/vendor/apriltag/test/test_tag_pose_estimation.py b/vendor/apriltag/test/test_tag_pose_estimation.py new file mode 100644 index 00000000..eb139a83 --- /dev/null +++ b/vendor/apriltag/test/test_tag_pose_estimation.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python3 + +import sys +import os +import cv2 +import numpy as np +from apriltag import apriltag + +def parse_expected_detection(line): + """ + Parse expected detection results from a line in the data file. + Format: ID, (x1 y1), (x2 y2), (x3 y3), (x4 y4) + """ + try: + parts = line.strip().split(',') + if len(parts) != 5: + return None, None + + tag_id = int(parts[0].strip()) + + corners = [] + for i in range(1, 5): + corner_str = parts[i].strip().strip('()') + x, y = map(float, corner_str.split()) + corners.append([x, y]) + + return tag_id, np.array(corners) + except (ValueError, IndexError): + return None, None + +def main(): + if len(sys.argv) < 3: + print(f"Usage: {sys.argv[0]} ") + print(f"Example: {sys.argv[0]} data/33369213973_9d9bb4cc96_c.jpg data/33369213973_9d9bb4cc96_c.txt") + return 1 + + image_path = sys.argv[1] + expected_data_path = sys.argv[2] + + print(f"Testing tag pose estimation with image: {image_path}") + print(f"Expected data file: {expected_data_path}") + + # Load image + image = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE) + if image is None: + print(f"Failed to load image: {image_path}") + return 1 + + # Create AprilTag detector - use tag36h11 family to match the C version + detector = apriltag("tag36h11", + threads=4, + decimate=1.0, # Consistent with test_detection.c + refine_edges=False) # Consistent with test_detection.c + + # Perform detection + detections = detector.detect(image) + + # Read expected data file + try: + with open(expected_data_path, 'r') as expected_file: + expected_lines = expected_file.readlines() + except FileNotFoundError: + print(f"Failed to open expected data file: {expected_data_path}") + return 1 + + print(f"Found {len(detections)} detections in image") + + # Perform pose estimation test for each detection + num_detections = len(detections) + pose_estimation_success = 0 + + for i, det in enumerate(detections): + print(f"Processing detection {i}: tag ID {det['id']}") + + # Set camera parameters and tag size + tagsize = 0.16 # Assume tag size is 16cm + fx = 600.0 # Assume focal length + fy = 600.0 + cx = image.shape[1] / 2.0 # Assume optical center is at image center + cy = image.shape[0] / 2.0 + + # Perform pose estimation + try: + pose = detector.estimate_tag_pose(det, tagsize, fx, fy, cx, cy) + R = pose['R'] + t = pose['t'] + err = pose['error'] + + print(f" Estimated pose for tag {det['id']}:") + print(f" Translation: [{t[0][0]:f}, {t[1][0]:f}, {t[2][0]:f}]") + print(f" Rotation matrix:") + print(f" [{R[0,0]:f}, {R[0,1]:f}, {R[0,2]:f}]") + print(f" [{R[1,0]:f}, {R[1,1]:f}, {R[1,2]:f}]") + print(f" [{R[2,0]:f}, {R[2,1]:f}, {R[2,2]:f}]") + print(f" Reprojection error: {err:f}") + + # Check if pose estimation was successful + if err < 1.0: # Assume error less than 1.0 means success + pose_estimation_success += 1 + except Exception as e: + print(f" Pose estimation failed for tag {det['id']}: {e}") + + # Output test results + print(f"\nPose estimation test results:") + print(f" Successful pose estimations: {pose_estimation_success}/{num_detections}") + + if pose_estimation_success == num_detections and num_detections > 0: + print(" All pose estimations successful!") + elif num_detections > 0: + print(" Some pose estimations failed.") + else: + print(" No detections found in image.") + + return 0 if (pose_estimation_success == num_detections and num_detections > 0) else 1 + +if __name__ == "__main__": + sys.exit(main())