├── .github ├── ISSUE_TEMPLATE │ └── bug_report.md └── workflows │ ├── bloom.yml │ ├── cmake-multi-platform.yml │ ├── cmake-ubuntu.yml │ ├── colcon-workspace.yml │ └── industrial_ci_action.yml ├── .gitignore ├── CMake ├── apriltagConfig.cmake.in └── vtkEncodeString.cmake ├── CMakeLists.txt ├── LICENSE.md ├── README.md ├── apriltag.c ├── apriltag.h ├── apriltag.pc.in ├── apriltag_detect.docstring ├── apriltag_math.h ├── apriltag_pose.c ├── apriltag_pose.h ├── apriltag_py_type.docstring ├── apriltag_pywrap.c ├── apriltag_quad_thresh.c ├── common ├── debug_print.h ├── doubles.h ├── doubles_floats_impl.h ├── floats.h ├── g2d.c ├── g2d.h ├── getopt.c ├── getopt.h ├── homography.c ├── homography.h ├── image_types.h ├── image_u8.c ├── image_u8.h ├── image_u8x3.c ├── image_u8x3.h ├── image_u8x4.c ├── image_u8x4.h ├── matd.c ├── matd.h ├── math_util.h ├── pam.c ├── pam.h ├── pjpeg-idct.c ├── pjpeg.c ├── pjpeg.h ├── pnm.c ├── pnm.h ├── postscript_utils.h ├── pthreads_cross.c ├── pthreads_cross.h ├── string_util.c ├── string_util.h ├── svd22.c ├── svd22.h ├── time_util.c ├── time_util.h ├── timeprofile.h ├── unionfind.c ├── unionfind.h ├── workerpool.c ├── workerpool.h ├── zarray.c ├── zarray.h ├── zhash.c ├── zhash.h ├── zmaxheap.c └── zmaxheap.h ├── example ├── apriltag_demo.c └── opencv_demo.cc ├── package.xml ├── tag16h5.c ├── tag16h5.h ├── tag25h9.c ├── tag25h9.h ├── tag36h10.c ├── tag36h10.h ├── tag36h11.c ├── tag36h11.h ├── tagCircle21h7.c ├── tagCircle21h7.h ├── tagCircle49h12.c ├── tagCircle49h12.h ├── tagCustom48h12.c ├── tagCustom48h12.h ├── tagStandard41h12.c ├── tagStandard41h12.h ├── tagStandard52h13.c ├── tagStandard52h13.h ├── tag_size_48h12.png └── test ├── CMakeLists.txt ├── data ├── 33369213973_9d9bb4cc96_c.jpg ├── 33369213973_9d9bb4cc96_c.txt ├── 34085369442_304b6bafd9_c.jpg ├── 34085369442_304b6bafd9_c.txt ├── 34139872896_defdb2f8d9_c.jpg ├── 34139872896_defdb2f8d9_c.txt └── README.md ├── getline.c ├── getline.h └── test_detection.c /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help improve AprilTag 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | A clear and concise description of what the bug is. 12 | 13 | **To Reproduce** 14 | Steps to reproduce the behavior: 15 | [e.g. Run apriltag_demo on the attached input image.] 16 | 17 | **Expected behavior** 18 | A clear and concise description of what you expected to happen. 19 | 20 | ** Input Image** 21 | If applicable, please attach the input image that reproduces the problem. 22 | 23 | **Screenshots** 24 | If applicable, add screenshots to help explain your problem. 25 | 26 | **Operating Sytem** 27 | [e.g. Ubuntu, iOS, Windows] 28 | 29 | ** Installation Method** 30 | [e.g. I built AprilTag from source following the instructions in the README] 31 | 32 | ** Code version ** 33 | What does the version tag in package.xml say? Or, if you have downloaded the code from github right now, say current github HEAD. 34 | 35 | **Additional context** 36 | Add any other context about the problem here. 37 | -------------------------------------------------------------------------------- /.github/workflows/bloom.yml: -------------------------------------------------------------------------------- 1 | name: bloom 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | build_linux: 7 | name: "Ubuntu (${{ matrix.ros_distribution }})" 8 | 9 | runs-on: ubuntu-latest 10 | 11 | strategy: 12 | matrix: 13 | include: 14 | - docker_image: ubuntu:20.04 15 | ros_distribution: noetic 16 | 17 | - docker_image: ubuntu:22.04 18 | ros_distribution: humble 19 | 20 | - docker_image: ubuntu:24.04 21 | ros_distribution: jazzy 22 | 23 | container: 24 | image: ${{ matrix.docker_image }} 25 | 26 | env: 27 | DEBIAN_FRONTEND: noninteractive 28 | 29 | steps: 30 | - name: install core dependencies 31 | run: | 32 | apt update 33 | apt install -y --no-install-recommends git ca-certificates 34 | 35 | - uses: actions/checkout@v4 36 | 37 | - uses: ros-tooling/setup-ros@v0.7 38 | 39 | - name: install build tool dependencies 40 | run: | 41 | apt install -y --no-install-recommends devscripts equivs python3-bloom 42 | 43 | - name: bloom 44 | run: | 45 | rosdep update 46 | bloom-generate rosdebian --ros-distro ${{ matrix.ros_distribution }} 47 | mk-build-deps 48 | apt install -y --no-install-recommends ./ros-${{ matrix.ros_distribution }}-apriltag-build-deps_*_all.deb 49 | dpkg-buildpackage -b 50 | 51 | - name: install bloomed packages 52 | run: | 53 | apt install -y --no-install-recommends ../ros-${{ matrix.ros_distribution }}-apriltag_*.deb ../ros-${{ matrix.ros_distribution }}-apriltag-dbgsym_*.ddeb 54 | -------------------------------------------------------------------------------- /.github/workflows/cmake-multi-platform.yml: -------------------------------------------------------------------------------- 1 | name: CMake 2 | 3 | on: 4 | push: 5 | branches: [ "master" ] 6 | pull_request: 7 | branches: [ "master" ] 8 | 9 | jobs: 10 | build: 11 | runs-on: ${{ matrix.os }} 12 | 13 | strategy: 14 | matrix: 15 | os: [ubuntu-latest, windows-latest, macos-latest] 16 | build_type: [Release] 17 | c_compiler: [gcc, clang, cl] 18 | shared_libs: ['ON', 'OFF'] 19 | include: 20 | - os: windows-latest 21 | c_compiler: cl 22 | cpp_compiler: cl 23 | - os: windows-latest 24 | c_compiler: gcc 25 | cpp_compiler: g++ 26 | - os: windows-latest 27 | c_compiler: clang 28 | cpp_compiler: clang++ 29 | - os: ubuntu-latest 30 | c_compiler: gcc 31 | cpp_compiler: g++ 32 | - os: ubuntu-latest 33 | c_compiler: clang 34 | cpp_compiler: clang++ 35 | - os: macos-latest 36 | c_compiler: clang 37 | cpp_compiler: clang++ 38 | exclude: 39 | - os: ubuntu-latest 40 | c_compiler: cl 41 | - os: macos-latest 42 | c_compiler: cl 43 | - os: macos-latest 44 | c_compiler: gcc 45 | 46 | steps: 47 | - uses: actions/checkout@v4 48 | 49 | - name: Set reusable strings 50 | id: strings 51 | shell: bash 52 | run: | 53 | echo "build-output-dir=${{ github.workspace }}/build" >> "$GITHUB_OUTPUT" 54 | 55 | - uses: seanmiddleditch/gha-setup-ninja@master 56 | 57 | - uses: ilammy/msvc-dev-cmd@v1 58 | 59 | - uses: actions/setup-python@v5 60 | with: 61 | python-version: '3.10' 62 | 63 | - run: pip install numpy 64 | 65 | - name: Configure CMake 66 | run: > 67 | cmake -B ${{ steps.strings.outputs.build-output-dir }} 68 | -G Ninja 69 | -D CMAKE_CXX_COMPILER=${{ matrix.cpp_compiler }} 70 | -D CMAKE_C_COMPILER=${{ matrix.c_compiler }} 71 | -D CMAKE_BUILD_TYPE=${{ matrix.build_type }} 72 | -D BUILD_SHARED_LIBS=${{ matrix.shared_libs }} 73 | -D BUILD_TESTING=ON 74 | -S ${{ github.workspace }} 75 | 76 | - name: Build 77 | run: cmake --build ${{ steps.strings.outputs.build-output-dir }} --config ${{ matrix.build_type }} 78 | 79 | - name: Install (Windows) 80 | if: matrix.os == 'windows-latest' 81 | run: Start-Process -Verb RunAs -FilePath cmake "--build ${{ steps.strings.outputs.build-output-dir }} --config ${{ matrix.build_type }} --target install" 82 | 83 | - name: Install (sudo) 84 | if: matrix.os != 'windows-latest' 85 | run: sudo cmake --build ${{ steps.strings.outputs.build-output-dir }} --config ${{ matrix.build_type }} --target install 86 | 87 | - name: add DLL to test folder 88 | if: matrix.os == 'windows-latest' 89 | working-directory: ${{ steps.strings.outputs.build-output-dir }} 90 | run: | 91 | cp *apriltag.dll test/ 92 | 93 | - name: Test 94 | working-directory: ${{ steps.strings.outputs.build-output-dir }} 95 | run: | 96 | ctest --build-config ${{ matrix.build_type }} --no-tests=error --output-on-failure --verbose 97 | 98 | - name: Test Python Module Import 99 | working-directory: ${{ steps.strings.outputs.build-output-dir }} 100 | run: | 101 | python3 -c "import apriltag; apriltag.apriltag(family='tag36h11')" 102 | -------------------------------------------------------------------------------- /.github/workflows/cmake-ubuntu.yml: -------------------------------------------------------------------------------- 1 | name: CMake (Ubuntu) 2 | 3 | on: 4 | push: 5 | branches: [ "master" ] 6 | pull_request: 7 | branches: [ "master" ] 8 | 9 | jobs: 10 | build: 11 | runs-on: ubuntu-latest 12 | 13 | strategy: 14 | matrix: 15 | version: ["20.04", "22.04", "24.04"] 16 | c_compiler: [gcc, clang] 17 | shared_libs: ['ON', 'OFF'] 18 | # manual selection for the latest, non-default, compilers 19 | include: 20 | - version: "24.04" 21 | c_compiler: gcc-14 22 | - version: "24.04" 23 | c_compiler: clang-18 24 | 25 | container: 26 | image: ubuntu:${{ matrix.version }} 27 | 28 | env: 29 | DEBIAN_FRONTEND: noninteractive 30 | 31 | steps: 32 | - name: install dependencies 33 | run: | 34 | apt update 35 | apt install -y --no-install-recommends cmake ninja-build ${{ matrix.c_compiler }} 36 | apt install -y --no-install-recommends python3-dev python3-numpy 37 | 38 | - uses: actions/checkout@v4 39 | 40 | - name: Set reusable strings 41 | id: strings 42 | shell: bash 43 | run: | 44 | echo "build-output-dir=$GITHUB_WORKSPACE/build" >> "$GITHUB_OUTPUT" 45 | 46 | - name: Configure CMake 47 | run: > 48 | cmake -B ${{ steps.strings.outputs.build-output-dir }} 49 | -G Ninja 50 | -D CMAKE_C_COMPILER=${{ matrix.c_compiler }} 51 | -D CMAKE_BUILD_TYPE=Release 52 | -D BUILD_SHARED_LIBS=${{ matrix.shared_libs }} 53 | -D BUILD_TESTING=ON 54 | -S $GITHUB_WORKSPACE 55 | 56 | - name: Build & Install 57 | run: cmake --build ${{ steps.strings.outputs.build-output-dir }} --target install 58 | 59 | - name: Test 60 | working-directory: ${{ steps.strings.outputs.build-output-dir }} 61 | run: | 62 | ctest --no-tests=error --output-on-failure --verbose 63 | -------------------------------------------------------------------------------- /.github/workflows/colcon-workspace.yml: -------------------------------------------------------------------------------- 1 | name: colcon workspace 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | # build on Ubuntu docker images 7 | build_linux: 8 | name: "Ubuntu (${{ matrix.ros_distribution }})" 9 | 10 | runs-on: ubuntu-latest 11 | 12 | strategy: 13 | matrix: 14 | include: 15 | - docker_image: ubuntu:20.04 16 | ros_distribution: noetic 17 | ros_version: 1 18 | 19 | - docker_image: ubuntu:22.04 20 | ros_distribution: humble 21 | ros_version: 2 22 | 23 | - docker_image: ubuntu:24.04 24 | ros_distribution: jazzy 25 | ros_version: 2 26 | 27 | container: 28 | image: ${{ matrix.docker_image }} 29 | 30 | steps: 31 | - name: install core dependencies 32 | run: | 33 | apt update 34 | apt install --no-install-recommends -y git ca-certificates 35 | 36 | - uses: actions/checkout@v4 37 | 38 | - name: Setup ROS environment 39 | uses: ros-tooling/setup-ros@v0.7 40 | 41 | - name: ROS 1 CI Action 42 | if: ${{ matrix.ros_version == 1 }} 43 | uses: ros-tooling/action-ros-ci@v0.3 44 | with: 45 | package-name: apriltag 46 | target-ros1-distro: ${{ matrix.ros_distribution }} 47 | 48 | - name: ROS 2 CI Action 49 | if: ${{ matrix.ros_version == 2 }} 50 | uses: ros-tooling/action-ros-ci@v0.3 51 | with: 52 | package-name: apriltag 53 | target-ros2-distro: ${{ matrix.ros_distribution }} 54 | 55 | 56 | # build on Windows native 57 | build_windows: 58 | name: "Windows (${{ matrix.ros_distribution }})" 59 | 60 | runs-on: windows-2019 61 | 62 | strategy: 63 | matrix: 64 | include: 65 | - ros_distribution: noetic 66 | ros_version: 1 67 | 68 | - ros_distribution: humble 69 | ros_version: 2 70 | 71 | - ros_distribution: jazzy 72 | ros_version: 2 73 | 74 | steps: 75 | - uses: actions/checkout@v4 76 | with: 77 | submodules: recursive 78 | 79 | - name: Setup ROS environment 80 | uses: ros-tooling/setup-ros@v0.7 81 | 82 | - name: ROS 1 CI Action 83 | if: ${{ matrix.ros_version == 1 }} 84 | uses: ros-tooling/action-ros-ci@v0.3 85 | with: 86 | package-name: apriltag 87 | target-ros1-distro: ${{ matrix.ros_distribution }} 88 | 89 | - name: ROS 2 CI Action 90 | if: ${{ matrix.ros_version == 2 }} 91 | uses: ros-tooling/action-ros-ci@v0.3 92 | with: 93 | package-name: apriltag 94 | target-ros2-distro: ${{ matrix.ros_distribution }} 95 | 96 | # build on macOS native 97 | build_macos: 98 | name: "macOS (${{ matrix.ros_distribution }})" 99 | 100 | runs-on: macos-latest 101 | 102 | strategy: 103 | matrix: 104 | ros_distribution: [humble, jazzy] 105 | 106 | steps: 107 | - uses: actions/checkout@v4 108 | with: 109 | submodules: recursive 110 | 111 | - uses: actions/setup-python@v4 112 | with: 113 | python-version: "3.10" 114 | 115 | - name: Setup ROS environment 116 | uses: ros-tooling/setup-ros@v0.7 117 | 118 | - name: ROS 2 CI Action 119 | uses: ros-tooling/action-ros-ci@v0.3 120 | with: 121 | package-name: apriltag 122 | target-ros2-distro: ${{ matrix.ros_distribution }} 123 | -------------------------------------------------------------------------------- /.github/workflows/industrial_ci_action.yml: -------------------------------------------------------------------------------- 1 | # This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). 2 | # For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) 3 | 4 | name: ROS-CI 5 | 6 | # This determines when this workflow is run 7 | on: [push, pull_request] # on all pushes and PRs 8 | 9 | jobs: 10 | CI: 11 | strategy: 12 | matrix: 13 | env: 14 | # ROS 1 15 | - {ROS_DISTRO: noetic} 16 | # ROS 2 17 | - {ROS_DISTRO: humble} 18 | env: 19 | CCACHE_DIR: /github/home/.ccache # Enable ccache 20 | runs-on: ubuntu-latest 21 | steps: 22 | - uses: actions/checkout@v4 23 | with: 24 | submodules: recursive 25 | # This step will fetch/store the directory used by ccache before/after the ci run 26 | - uses: actions/cache@v3 27 | with: 28 | path: ${{ env.CCACHE_DIR }} 29 | key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }} 30 | # Run industrial_ci 31 | - uses: 'ros-industrial/industrial_ci@master' 32 | env: ${{ matrix.env }} 33 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.a 2 | *.so 3 | *.o 4 | *.swp 5 | example/opencv_demo 6 | example/apriltag_demo 7 | build/ 8 | 9 | -------------------------------------------------------------------------------- /CMake/apriltagConfig.cmake.in: -------------------------------------------------------------------------------- 1 | @PACKAGE_INIT@ 2 | 3 | if(NOT MSVC) 4 | include(CMakeFindDependencyMacro) 5 | find_dependency(Threads) 6 | endif() 7 | 8 | include("${CMAKE_CURRENT_LIST_DIR}/@targets_export_name@.cmake") 9 | check_required_components("@PROJECT_NAME@") 10 | 11 | if(NOT TARGET apriltag AND ${CMAKE_VERSION} VERSION_GREATER "3.10.99") 12 | # Make imported target globally visible in order to create an ALIAS 13 | # IMPORTED_GLOBAL is only available in CMake 3.11+ 14 | set_target_properties(apriltag::apriltag PROPERTIES IMPORTED_GLOBAL TRUE) 15 | # Create alias for backwards compatibility with 3.1.2 and earlier (will be removed in the future - please migrate to apriltag::apriltag) 16 | add_library(apriltag ALIAS apriltag::apriltag) 17 | endif() 18 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (C) 2013-2016, The Regents of The University of Michigan. 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | The views and conclusions contained in the software and documentation are those 28 | of the authors and should not be interpreted as representing official policies, 29 | either expressed or implied, of the Regents of The University of Michigan. 30 | -------------------------------------------------------------------------------- /apriltag.pc.in: -------------------------------------------------------------------------------- 1 | prefix=@CMAKE_INSTALL_PREFIX@ 2 | exec_prefix=${prefix} 3 | includedir=${prefix}/include 4 | libdir=${exec_prefix}/lib 5 | 6 | Name: @PROJECT_NAME@ 7 | Description: AprilTag is a visual fiducial system popular for robotics research. 8 | Version: @PROJECT_VERSION@ 9 | Cflags: -I${includedir} 10 | Libs: -L${libdir} -lapriltag 11 | -------------------------------------------------------------------------------- /apriltag_detect.docstring: -------------------------------------------------------------------------------- 1 | AprilTag detector 2 | 3 | SYNOPSIS 4 | 5 | import cv2 6 | import numpy as np 7 | from apriltag import apriltag 8 | 9 | imagepath = '/tmp/tst.jpg' 10 | image = cv2.imread(imagepath, cv2.IMREAD_GRAYSCALE) 11 | detector = apriltag("tag36h11") 12 | 13 | detections = detector.detect(image) 14 | 15 | print("Saw tags {} at\n{}". \ 16 | format([d['id'] for d in detections], 17 | np.array([d['center'] for d in detections]))) 18 | 19 | ----> Saw tags [3, 5, 7, 8, 10, 10, 14] at 20 | [[582.42911184 172.90587335] 21 | [703.32149701 271.50587376] 22 | [288.1462089 227.01502779] 23 | [463.63679264 227.91185418] 24 | [ 93.88534443 241.61109765] 25 | [121.94062798 237.97010936] 26 | [356.46940849 260.20169159]] 27 | 28 | DESCRIPTION 29 | 30 | The AprilTags visual fiducial system project page is here: 31 | https://april.eecs.umich.edu/software/apriltag 32 | 33 | This is a Python class to provide AprilTags functionality in Python programs. To 34 | run the detector you 35 | 36 | 1. Construct an object of type apriltag.apriltag() 37 | 38 | 2. Invoke the detect() method on this object 39 | 40 | The detect() method takes a single argument: an image array. The return value is 41 | a tuple containing the detections. Each detection is a dict with keys: 42 | 43 | - id: integer identifying each detected tag 44 | 45 | - center: pixel coordinates of the center of each detection. NOTE: Please be 46 | cautious regarding the image coordinate convention. Here, we define (0,0) as 47 | the left-top corner (not the center point) of the left-top-most pixel. 48 | 49 | - lb-rb-rt-lt: pixel coordinates of the 4 corners of each detection. The order 50 | is left-bottom, right-bottom, right-top, left-top 51 | 52 | - hamming: How many error bits were corrected? Note: accepting large numbers of 53 | corrected errors leads to greatly increased false positive rates. NOTE: As of 54 | this implementation, the detector cannot detect tags with a hamming distance 55 | greater than 2. 56 | 57 | - margin: A measure of the quality of the binary decoding process: the average 58 | difference between the intensity of a data bit versus the decision threshold. 59 | Higher numbers roughly indicate better decodes. This is a reasonable measure 60 | of detection accuracy only for very small tags-- not effective for larger tags 61 | (where we could have sampled anywhere within a bit cell and still gotten a 62 | good detection.) 63 | -------------------------------------------------------------------------------- /apriltag_math.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | // Computes the cholesky factorization of A, putting the lower 33 | // triangular matrix into R. 34 | static inline void mat33_chol(const double *A, 35 | double *R) 36 | { 37 | // A[0] = R[0]*R[0] 38 | R[0] = sqrt(A[0]); 39 | 40 | // A[1] = R[0]*R[3]; 41 | R[3] = A[1] / R[0]; 42 | 43 | // A[2] = R[0]*R[6]; 44 | R[6] = A[2] / R[0]; 45 | 46 | // A[4] = R[3]*R[3] + R[4]*R[4] 47 | R[4] = sqrt(A[4] - R[3]*R[3]); 48 | 49 | // A[5] = R[3]*R[6] + R[4]*R[7] 50 | R[7] = (A[5] - R[3]*R[6]) / R[4]; 51 | 52 | // A[8] = R[6]*R[6] + R[7]*R[7] + R[8]*R[8] 53 | R[8] = sqrt(A[8] - R[6]*R[6] - R[7]*R[7]); 54 | 55 | R[1] = 0; 56 | R[2] = 0; 57 | R[5] = 0; 58 | } 59 | 60 | static inline void mat33_lower_tri_inv(const double *A, 61 | double *R) 62 | { 63 | // A[0]*R[0] = 1 64 | R[0] = 1 / A[0]; 65 | 66 | // A[3]*R[0] + A[4]*R[3] = 0 67 | R[3] = -A[3]*R[0] / A[4]; 68 | 69 | // A[4]*R[4] = 1 70 | R[4] = 1 / A[4]; 71 | 72 | // A[6]*R[0] + A[7]*R[3] + A[8]*R[6] = 0 73 | R[6] = (-A[6]*R[0] - A[7]*R[3]) / A[8]; 74 | 75 | // A[7]*R[4] + A[8]*R[7] = 0 76 | R[7] = -A[7]*R[4] / A[8]; 77 | 78 | // A[8]*R[8] = 1 79 | R[8] = 1 / A[8]; 80 | } 81 | 82 | 83 | static inline void mat33_sym_solve(const double *A, 84 | const double *B, 85 | double *R) 86 | { 87 | double L[9]; 88 | mat33_chol(A, L); 89 | 90 | double M[9]; 91 | mat33_lower_tri_inv(L, M); 92 | 93 | double tmp[3]; 94 | tmp[0] = M[0]*B[0]; 95 | tmp[1] = M[3]*B[0] + M[4]*B[1]; 96 | tmp[2] = M[6]*B[0] + M[7]*B[1] + M[8]*B[2]; 97 | 98 | R[0] = M[0]*tmp[0] + M[3]*tmp[1] + M[6]*tmp[2]; 99 | R[1] = M[4]*tmp[1] + M[7]*tmp[2]; 100 | R[2] = M[8]*tmp[2]; 101 | } 102 | -------------------------------------------------------------------------------- /apriltag_pose.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifdef __cplusplus 4 | extern "C" { 5 | #endif 6 | 7 | 8 | #include "apriltag.h" 9 | #include "common/matd.h" 10 | 11 | typedef struct { 12 | apriltag_detection_t* det; 13 | double tagsize; // In meters. 14 | double fx; // In pixels. 15 | double fy; // In pixels. 16 | double cx; // In pixels. 17 | double cy; // In pixels. 18 | } apriltag_detection_info_t; 19 | 20 | /** 21 | * This struct holds the transformation from the camera optical frame to 22 | * the April tag frame. The pose refers to the position of the tag within 23 | * the camera frame. 24 | */ 25 | typedef struct { 26 | matd_t* R; // Rotation matrix 3x3 of doubles. 27 | matd_t* t; // Translation matrix 3x1 of doubles. 28 | } apriltag_pose_t; 29 | 30 | /** 31 | * Estimate pose of the tag using the homography method described in [1]. 32 | * @outparam pose 33 | */ 34 | void estimate_pose_for_tag_homography( 35 | apriltag_detection_info_t* info, 36 | apriltag_pose_t* pose); 37 | 38 | /** 39 | * Estimate pose of the tag. This returns one or two possible poses for the 40 | * tag, along with the object-space error of each. 41 | * 42 | * This uses the homography method described in [1] for the initial estimate. 43 | * Then Orthogonal Iteration [2] is used to refine this estimate. Then [3] is 44 | * used to find a potential second local minima and Orthogonal Iteration is 45 | * used to refine this second estimate. 46 | * 47 | * [1]: E. Olson, "Apriltag: A robust and flexible visual fiducial system," in 48 | * 2011 IEEE International Conference on Robotics and Automation, 49 | * May 2011, pp. 3400-3407. 50 | * [2]: Lu, G. D. Hager and E. Mjolsness, "Fast and globally convergent pose 51 | * estimation from video images," in IEEE Transactions on Pattern Analysis 52 | * and Machine Intelligence, vol. 22, no. 6, pp. 610-622, June 2000. 53 | * doi: 10.1109/34.862199 54 | * [3]: Schweighofer and A. Pinz, "Robust Pose Estimation from a Planar Target," 55 | * in IEEE Transactions on Pattern Analysis and Machine Intelligence, 56 | * vol. 28, no. 12, pp. 2024-2030, Dec. 2006. doi: 10.1109/TPAMI.2006.252 57 | * 58 | * @outparam err1, pose1, err2, pose2 59 | */ 60 | void estimate_tag_pose_orthogonal_iteration( 61 | apriltag_detection_info_t* info, 62 | double* err1, 63 | apriltag_pose_t* pose1, 64 | double* err2, 65 | apriltag_pose_t* pose2, 66 | int nIters); 67 | 68 | /** 69 | * Estimate tag pose. 70 | * This method is an easier to use interface to estimate_tag_pose_orthogonal_iteration. 71 | * 72 | * @outparam pose 73 | * @return Object-space error of returned pose. 74 | */ 75 | double estimate_tag_pose(apriltag_detection_info_t* info, apriltag_pose_t* pose); 76 | 77 | #ifdef __cplusplus 78 | } 79 | #endif 80 | -------------------------------------------------------------------------------- /apriltag_py_type.docstring: -------------------------------------------------------------------------------- 1 | AprilTag detector 2 | 3 | SYNOPSIS 4 | 5 | import cv2 6 | import numpy as np 7 | from apriltag import apriltag 8 | 9 | imagepath = '/tmp/tst.jpg' 10 | image = cv2.imread(imagepath, cv2.IMREAD_GRAYSCALE) 11 | detector = apriltag("tag36h11") 12 | 13 | detections = detector.detect(image) 14 | 15 | print("Saw tags {} at\n{}". \ 16 | format([d['id'] for d in detections], 17 | np.array([d['center'] for d in detections]))) 18 | 19 | ----> Saw tags [3, 5, 7, 8, 10, 10, 14] at 20 | [[582.42911184 172.90587335] 21 | [703.32149701 271.50587376] 22 | [288.1462089 227.01502779] 23 | [463.63679264 227.91185418] 24 | [ 93.88534443 241.61109765] 25 | [121.94062798 237.97010936] 26 | [356.46940849 260.20169159]] 27 | 28 | DESCRIPTION 29 | 30 | The AprilTags visual fiducial system project page is here: 31 | https://april.eecs.umich.edu/software/apriltag 32 | 33 | This is a Python class to provide AprilTags functionality in Python programs. To 34 | run the detector you 35 | 36 | 1. Construct an object of type apriltag.apriltag() 37 | 38 | 2. Invoke the detect() method on this object 39 | 40 | The constructor takes a number of arguments: 41 | 42 | - family: a string for the tag type we're detecting. This argument is required. 43 | If an invalid string is given, the known list of tag families will be 44 | reported. At the time of this writing the known families are: 45 | 46 | - "tag36h11" 47 | - "tag25h9" 48 | - "tag16h5" 49 | - "tagCircle21h7" 50 | - "tagCircle49h12" 51 | - "tagStandard41h12" 52 | - "tagStandard52h13" 53 | - "tagCustom48h12" 54 | 55 | All the other arguments are optional: 56 | 57 | - threads: how many threads the detector should use. Default is 1 58 | 59 | - maxhamming: max number of corrected bits. Larger values guzzle RAM. Default is 60 | 1 61 | 62 | - decimate: detection of quads can be done on a lower-resolution image, 63 | improving speed at a cost of pose accuracy and a slight decrease in detection 64 | rate. Decoding the binary payload is still done at full resolution. Default is 1.0 65 | 66 | - blur: What Gaussian blur should be applied to the segmented image (used for 67 | quad detection?) Parameter is the standard deviation in pixels. Very noisy 68 | images benefit from non-zero values (e.g. 0.8). Default is 0.0 69 | 70 | - refine_edges: When non-zero, the edges of the each quad are adjusted to "snap 71 | to" strong gradients nearby. This is useful when decimation is employed, as it 72 | can increase the quality of the initial quad estimate substantially. Generally 73 | recommended to be on. Very computationally inexpensive. Option is ignored if 74 | decimate == 1. Default is True 75 | 76 | - debug: When non-zero, write a variety of debugging images to the current 77 | working directory at various stages through the detection process. (Somewhat 78 | slow). Default is False 79 | 80 | The detect() method takes a single argument: an image array 81 | -------------------------------------------------------------------------------- /common/debug_print.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | #pragma once 28 | 29 | #if !defined(NDEBUG) || defined(_DEBUG) 30 | 31 | #include 32 | #include 33 | #define DEBUG 1 34 | 35 | #else 36 | #define DEBUG 0 37 | #endif 38 | 39 | #define debug_print(fmt, ...) \ 40 | do { if (DEBUG) fprintf(stderr, "%s:%d:%s(): " fmt, strrchr("/"__FILE__,'/')+1, \ 41 | __LINE__, __func__, ##__VA_ARGS__); fflush(stderr);} while (0) 42 | -------------------------------------------------------------------------------- /common/doubles.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #pragma once 29 | 30 | #define TNAME double 31 | #include "doubles_floats_impl.h" 32 | #undef TNAME 33 | -------------------------------------------------------------------------------- /common/floats.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #pragma once 29 | 30 | #define TNAME float 31 | #include "doubles_floats_impl.h" 32 | #undef TNAME 33 | -------------------------------------------------------------------------------- /common/g2d.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #pragma once 29 | 30 | #ifdef __cplusplus 31 | extern "C" { 32 | #endif 33 | 34 | #include "zarray.h" 35 | 36 | // This library tries to avoid needless proliferation of types. 37 | // 38 | // A point is a double[2]. (Note that when passing a double[2] as an 39 | // argument, it is passed by pointer, not by value.) 40 | // 41 | // A polygon is a zarray_t of double[2]. (Note that in this case, the 42 | // zarray contains the actual vertex data, and not merely a pointer to 43 | // some other data. IMPORTANT: A polygon must be specified in CCW 44 | // order. It is implicitly closed (do not list the same point at the 45 | // beginning at the end. 46 | // 47 | // Where sensible, it is assumed that objects should be allocated 48 | // sparingly; consequently "init" style methods, rather than "create" 49 | // methods are used. 50 | 51 | //////////////////////////////////////////////////////////////////// 52 | // Lines 53 | 54 | typedef struct 55 | { 56 | // Internal representation: a point that the line goes through (p) and 57 | // the direction of the line (u). 58 | double p[2]; 59 | double u[2]; // always a unit vector 60 | } g2d_line_t; 61 | 62 | // initialize a line object. 63 | void g2d_line_init_from_points(g2d_line_t *line, const double p0[2], const double p1[2]); 64 | 65 | // The line defines a one-dimensional coordinate system whose origin 66 | // is p. Where is q? (If q is not on the line, the point nearest q is 67 | // returned. 68 | double g2d_line_get_coordinate(const g2d_line_t *line, const double q[2]); 69 | 70 | // Intersect two lines. The intersection, if it exists, is written to 71 | // p (if not NULL), and 1 is returned. Else, zero is returned. 72 | int g2d_line_intersect_line(const g2d_line_t *linea, const g2d_line_t *lineb, double *p); 73 | 74 | //////////////////////////////////////////////////////////////////// 75 | // Line Segments. line.p is always one endpoint; p1 is the other 76 | // endpoint. 77 | typedef struct 78 | { 79 | g2d_line_t line; 80 | double p1[2]; 81 | } g2d_line_segment_t; 82 | 83 | void g2d_line_segment_init_from_points(g2d_line_segment_t *seg, const double p0[2], const double p1[2]); 84 | 85 | // Intersect two segments. The intersection, if it exists, is written 86 | // to p (if not NULL), and 1 is returned. Else, zero is returned. 87 | int g2d_line_segment_intersect_segment(const g2d_line_segment_t *sega, const g2d_line_segment_t *segb, double *p); 88 | 89 | void g2d_line_segment_closest_point(const g2d_line_segment_t *seg, const double *q, double *p); 90 | double g2d_line_segment_closest_point_distance(const g2d_line_segment_t *seg, const double *q); 91 | 92 | //////////////////////////////////////////////////////////////////// 93 | // Polygons 94 | 95 | zarray_t *g2d_polygon_create_data(double v[][2], int sz); 96 | 97 | zarray_t *g2d_polygon_create_zeros(int sz); 98 | 99 | zarray_t *g2d_polygon_create_empty(); 100 | 101 | void g2d_polygon_add(zarray_t *poly, double v[2]); 102 | 103 | // Takes a polygon in either CW or CCW and modifies it (if necessary) 104 | // to be CCW. 105 | void g2d_polygon_make_ccw(zarray_t *poly); 106 | 107 | // Return 1 if point q lies within poly. 108 | int g2d_polygon_contains_point(const zarray_t *poly, double q[2]); 109 | 110 | // Do the edges of the polygons cross? (Does not test for containment). 111 | int g2d_polygon_intersects_polygon(const zarray_t *polya, const zarray_t *polyb); 112 | 113 | // Does polya completely contain polyb? 114 | int g2d_polygon_contains_polygon(const zarray_t *polya, const zarray_t *polyb); 115 | 116 | // Is there some point which is in both polya and polyb? 117 | int g2d_polygon_overlaps_polygon(const zarray_t *polya, const zarray_t *polyb); 118 | 119 | // returns the number of points written to x. see comments. 120 | int g2d_polygon_rasterize(const zarray_t *poly, double y, double *x); 121 | 122 | #ifdef __cplusplus 123 | } 124 | #endif 125 | -------------------------------------------------------------------------------- /common/getopt.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #pragma once 29 | 30 | #include "zarray.h" 31 | #include "string_util.h" 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | typedef struct getopt getopt_t; 38 | 39 | getopt_t *getopt_create(); 40 | void getopt_destroy(getopt_t *gopt); 41 | void getopt_option_destroy_void(void *goo); 42 | 43 | // Parse args. Returns 1 on success 44 | int getopt_parse(getopt_t *gopt, int argc, char *argv[], int showErrors); 45 | void getopt_do_usage(getopt_t *gopt); 46 | 47 | // Returns a string containing the usage. Must be freed by caller 48 | char * getopt_get_usage(getopt_t *gopt); 49 | 50 | void getopt_add_spacer(getopt_t *gopt, const char *s); 51 | void getopt_add_bool(getopt_t *gopt, char sopt, const char *lname, int def, const char *help); 52 | void getopt_add_int(getopt_t *gopt, char sopt, const char *lname, const char *def, const char *help); 53 | void getopt_add_string(getopt_t *gopt, char sopt, const char *lname, const char *def, const char *help); 54 | void getopt_add_double(getopt_t *gopt, char sopt, const char *lname, const char *def, const char *help); 55 | 56 | const char *getopt_get_string(getopt_t *gopt, const char *lname); 57 | int getopt_get_int(getopt_t *getopt, const char *lname); 58 | int getopt_get_bool(getopt_t *getopt, const char *lname); 59 | double getopt_get_double(getopt_t *getopt, const char *lname); 60 | int getopt_was_specified(getopt_t *gopt, const char *lname); 61 | const zarray_t *getopt_get_extra_args(getopt_t *gopt); 62 | 63 | #ifdef __cplusplus 64 | } 65 | #endif 66 | -------------------------------------------------------------------------------- /common/homography.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #pragma once 29 | 30 | #include "matd.h" 31 | #include "zarray.h" 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | /** Given a 3x3 homography matrix and the focal lengths of the 38 | * camera, compute the pose of the tag. The focal lengths should 39 | * be given in pixels. For example, if the camera's focal length 40 | * is twice the width of the sensor, and the sensor is 600 pixels 41 | * across, the focal length in pixels is 2*600. Note that the 42 | * focal lengths in the fx and fy direction will be approximately 43 | * equal for most lenses, and is not a function of aspect ratio. 44 | * 45 | * Theory: The homography matrix is the product of the camera 46 | * projection matrix and the tag's pose matrix (the matrix that 47 | * projects points from the tag's local coordinate system to the 48 | * camera's coordinate frame). 49 | * 50 | * [ h00 h01 h02 h03] = [ fx 0 cx 0 ] [ R00 R01 R02 TX ] 51 | * [ h10 h11 h12 h13] = [ 0 fy cy 0 ] [ R10 R11 R12 TY ] 52 | * [ h20 h21 h22 h23] = [ 0 0 s 0 ] [ R20 R21 R22 TZ ] 53 | * [ 0 0 0 1 ] 54 | * 55 | * fx is the focal length in the x direction of the camera 56 | * (typically measured in pixels), fy is the focal length. cx and 57 | * cy give the focal center (usually the middle of the image), and 58 | * s is either +1 or -1, depending on the conventions you use. (We 59 | * use 1.) 60 | 61 | * When observing a tag, the points we project in world space all 62 | * have z=0, so we can form a 3x3 matrix by eliminating the 3rd 63 | * column of the pose matrix. 64 | * 65 | * [ h00 h01 h02 ] = [ fx 0 cx 0 ] [ R00 R01 TX ] 66 | * [ h10 h11 h12 ] = [ 0 fy cy 0 ] [ R10 R11 TY ] 67 | * [ h20 h21 h22 ] = [ 0 0 s 0 ] [ R20 R21 TZ ] 68 | * [ 0 0 1 ] 69 | * 70 | * (note that these h's are different from the ones above.) 71 | * 72 | * We can multiply the right-hand side to yield a set of equations 73 | * relating the values of h to the values of the pose matrix. 74 | * 75 | * There are two wrinkles. The first is that the homography matrix 76 | * is known only up to scale. We recover the unknown scale by 77 | * constraining the magnitude of the first two columns of the pose 78 | * matrix to be 1. We use the geometric average scale. The sign of 79 | * the scale factor is recovered by constraining the observed tag 80 | * to be in front of the camera. Once scaled, we recover the first 81 | * two colmuns of the rotation matrix. The third column is the 82 | * cross product of these. 83 | * 84 | * The second wrinkle is that the computed rotation matrix might 85 | * not be exactly orthogonal, so we perform a polar decomposition 86 | * to find a good pure rotation approximation. 87 | * 88 | * Tagsize is the size of the tag in your desired units. I.e., if 89 | * your tag measures 0.25m along the side, your tag size is 90 | * 0.25. (The homography is computed in terms of *half* the tag 91 | * size, i.e., that a tag is 2 units wide as it spans from -1 to 92 | * +1, but this code makes the appropriate adjustment.) 93 | * 94 | * A note on signs: 95 | * 96 | * The code below incorporates no additional negative signs, but 97 | * respects the sign of any parameters that you pass in. Flipping 98 | * the signs allows you to modify the projection to suit a wide 99 | * variety of conditions. 100 | * 101 | * In the "pure geometry" projection matrix, the image appears 102 | * upside down; i.e., the x and y coordinates on the left hand 103 | * side are the opposite of those on the right of the camera 104 | * projection matrix. This would happen for all parameters 105 | * positive: recall that points in front of the camera have 106 | * negative Z values, which will cause the sign of all points to 107 | * flip. 108 | * 109 | * However, most cameras flip things so that the image appears 110 | * "right side up" as though you were looking through the lens 111 | * directly. This means that the projected points should have the 112 | * same sign as the points on the right of the camera projection 113 | * matrix. To achieve this, flip fx and fy. 114 | * 115 | * One further complication: cameras typically put y=0 at the top 116 | * of the image, instead of the bottom. Thus you generally want to 117 | * flip y yet again (so it's now positive again). 118 | * 119 | * General advice: you probably want fx negative, fy positive, cx 120 | * and cy positive, and s=1. 121 | **/ 122 | 123 | // correspondences is a list of float[4]s, consisting of the points x 124 | // and y concatenated. We will compute a homography such that y = Hx 125 | // Specifically, float [] { a, b, c, d } where x = [a b], y = [c d]. 126 | 127 | 128 | #define HOMOGRAPHY_COMPUTE_FLAG_INVERSE 1 129 | #define HOMOGRAPHY_COMPUTE_FLAG_SVD 0 130 | 131 | matd_t *homography_compute(zarray_t *correspondences, int flags); 132 | 133 | //void homography_project(const matd_t *H, double x, double y, double *ox, double *oy); 134 | static inline void homography_project(const matd_t *H, double x, double y, double *ox, double *oy) 135 | { 136 | double xx = MATD_EL(H, 0, 0)*x + MATD_EL(H, 0, 1)*y + MATD_EL(H, 0, 2); 137 | double yy = MATD_EL(H, 1, 0)*x + MATD_EL(H, 1, 1)*y + MATD_EL(H, 1, 2); 138 | double zz = MATD_EL(H, 2, 0)*x + MATD_EL(H, 2, 1)*y + MATD_EL(H, 2, 2); 139 | 140 | *ox = xx / zz; 141 | *oy = yy / zz; 142 | } 143 | 144 | // assuming that the projection matrix is: 145 | // [ fx 0 cx 0 ] 146 | // [ 0 fy cy 0 ] 147 | // [ 0 0 1 0 ] 148 | // 149 | // And that the homography is equal to the projection matrix times the model matrix, 150 | // recover the model matrix (which is returned). Note that the third column of the model 151 | // matrix is missing in the expression below, reflecting the fact that the homography assumes 152 | // all points are at z=0 (i.e., planar) and that the element of z is thus omitted. 153 | // (3x1 instead of 4x1). 154 | // 155 | // [ fx 0 cx 0 ] [ R00 R01 TX ] [ H00 H01 H02 ] 156 | // [ 0 fy cy 0 ] [ R10 R11 TY ] = [ H10 H11 H12 ] 157 | // [ 0 0 1 0 ] [ R20 R21 TZ ] = [ H20 H21 H22 ] 158 | // [ 0 0 1 ] 159 | // 160 | // fx*R00 + cx*R20 = H00 (note, H only known up to scale; some additional adjustments required; see code.) 161 | // fx*R01 + cx*R21 = H01 162 | // fx*TX + cx*TZ = H02 163 | // fy*R10 + cy*R20 = H10 164 | // fy*R11 + cy*R21 = H11 165 | // fy*TY + cy*TZ = H12 166 | // R20 = H20 167 | // R21 = H21 168 | // TZ = H22 169 | matd_t *homography_to_pose(const matd_t *H, double fx, double fy, double cx, double cy); 170 | 171 | // Similar to above 172 | // Recover the model view matrix assuming that the projection matrix is: 173 | // 174 | // [ F 0 A 0 ] (see glFrustrum) 175 | // [ 0 G B 0 ] 176 | // [ 0 0 C D ] 177 | // [ 0 0 -1 0 ] 178 | 179 | matd_t *homography_to_model_view(const matd_t *H, double F, double G, double A, double B); 180 | 181 | #ifdef __cplusplus 182 | } 183 | #endif 184 | -------------------------------------------------------------------------------- /common/image_types.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | // to support conversions between different types, we define all image 33 | // types at once. Type-specific implementations can then #include this 34 | // file, assured that the basic types of each image are known. 35 | 36 | typedef struct image_u8 image_u8_t; 37 | struct image_u8 38 | { 39 | const int32_t width; 40 | const int32_t height; 41 | const int32_t stride; 42 | 43 | uint8_t *buf; 44 | }; 45 | 46 | typedef struct image_u8x3 image_u8x3_t; 47 | struct image_u8x3 48 | { 49 | const int32_t width; 50 | const int32_t height; 51 | const int32_t stride; // bytes per line 52 | 53 | uint8_t *buf; 54 | }; 55 | 56 | typedef struct image_u8x4 image_u8x4_t; 57 | struct image_u8x4 58 | { 59 | const int32_t width; 60 | const int32_t height; 61 | const int32_t stride; // bytes per line 62 | 63 | uint8_t *buf; 64 | }; 65 | 66 | typedef struct image_f32 image_f32_t; 67 | struct image_f32 68 | { 69 | const int32_t width; 70 | const int32_t height; 71 | const int32_t stride; // floats per line 72 | 73 | float *buf; // indexed as buf[y*stride + x] 74 | }; 75 | 76 | typedef struct image_u32 image_u32_t; 77 | struct image_u32 78 | { 79 | const int32_t width; 80 | const int32_t height; 81 | const int32_t stride; // int32_ts per line 82 | 83 | uint32_t *buf; // indexed as buf[y*stride + x] 84 | }; 85 | -------------------------------------------------------------------------------- /common/image_u8.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include "image_types.h" 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | typedef struct image_u8_lut image_u8_lut_t; 38 | struct image_u8_lut 39 | { 40 | // When drawing, we compute the squared distance between a given pixel and a filled region. 41 | // int idx = squared_distance * scale; 42 | // We then index into values[idx] to obtain the color. (If we must index beyond nvalues, 43 | // no drawing is performed.) 44 | float scale; 45 | 46 | int nvalues; 47 | uint8_t *values; 48 | }; 49 | 50 | 51 | // Create or load an image. returns NULL on failure. Uses default 52 | // stride alignment. 53 | image_u8_t *image_u8_create_stride(unsigned int width, unsigned int height, unsigned int stride); 54 | image_u8_t *image_u8_create(unsigned int width, unsigned int height); 55 | image_u8_t *image_u8_create_alignment(unsigned int width, unsigned int height, unsigned int alignment); 56 | image_u8_t *image_u8_create_from_f32(image_f32_t *fim); 57 | 58 | image_u8_t *image_u8_create_from_pnm(const char *path); 59 | image_u8_t *image_u8_create_from_pnm_alignment(const char *path, int alignment); 60 | 61 | image_u8_t *image_u8_copy(const image_u8_t *in); 62 | void image_u8_draw_line(image_u8_t *im, float x0, float y0, float x1, float y1, int v, int width); 63 | void image_u8_draw_circle(image_u8_t *im, float x0, float y0, float r, int v); 64 | void image_u8_draw_annulus(image_u8_t *im, float x0, float y0, float r0, float r1, int v); 65 | 66 | void image_u8_fill_line_max(image_u8_t *im, const image_u8_lut_t *lut, const float *xy0, const float *xy1); 67 | 68 | void image_u8_clear(image_u8_t *im); 69 | void image_u8_darken(image_u8_t *im); 70 | void image_u8_convolve_2D(image_u8_t *im, const uint8_t *k, int ksz); 71 | void image_u8_gaussian_blur(image_u8_t *im, double sigma, int k); 72 | 73 | // 1.5, 2, 3, 4, ... supported 74 | image_u8_t *image_u8_decimate(image_u8_t *im, float factor); 75 | 76 | void image_u8_destroy(image_u8_t *im); 77 | 78 | // Write a pnm. Returns 0 on success 79 | // Currently only supports GRAY and RGBA. Does not write out alpha for RGBA 80 | int image_u8_write_pnm(const image_u8_t *im, const char *path); 81 | 82 | // rotate the image by 'rad' radians. (Rotated in the "intuitive 83 | // sense", i.e., if Y were up. When input values are unavailable, the 84 | // value 'pad' is inserted instead. The geometric center of the output 85 | // image corresponds to the geometric center of the input image. 86 | image_u8_t *image_u8_rotate(const image_u8_t *in, double rad, uint8_t pad); 87 | 88 | #ifdef __cplusplus 89 | } 90 | #endif 91 | -------------------------------------------------------------------------------- /common/image_u8x3.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include "image_types.h" 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | 38 | ///////////////////////////////////// 39 | // IMPORTANT NOTE ON BYTE ORDER 40 | // 41 | // Format conversion routines will (unless otherwise specified) assume 42 | // R, G, B, ordering of bytes. This is consistent with GTK, PNM, etc. 43 | // 44 | ///////////////////////////////////// 45 | 46 | // Create or load an image. returns NULL on failure 47 | image_u8x3_t *image_u8x3_create(unsigned int width, unsigned int height); 48 | image_u8x3_t *image_u8x3_create_alignment(unsigned int width, unsigned int height, unsigned int alignment); 49 | image_u8x3_t *image_u8x3_create_from_pnm(const char *path); 50 | 51 | image_u8x3_t *image_u8x3_copy(const image_u8x3_t *in); 52 | 53 | void image_u8x3_gaussian_blur(image_u8x3_t *im, double sigma, int ksz); 54 | 55 | void image_u8x3_destroy(image_u8x3_t *im); 56 | 57 | int image_u8x3_write_pnm(const image_u8x3_t *im, const char *path); 58 | 59 | // only width 1 supported 60 | void image_u8x3_draw_line(image_u8x3_t *im, float x0, float y0, float x1, float y1, uint8_t rgb[3]); 61 | 62 | #ifdef __cplusplus 63 | } 64 | #endif 65 | -------------------------------------------------------------------------------- /common/image_u8x4.c: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include "pam.h" 34 | #include "pnm.h" 35 | #include "image_u8x4.h" 36 | 37 | // least common multiple of 64 (sandy bridge cache line) and 64 (stride needed 38 | // for 16byte-wide RGBA processing). 39 | #define DEFAULT_ALIGNMENT_U8X4 64 40 | 41 | image_u8x4_t *image_u8x4_create(unsigned int width, unsigned int height) 42 | { 43 | return image_u8x4_create_alignment(width, height, DEFAULT_ALIGNMENT_U8X4); 44 | } 45 | 46 | image_u8x4_t *image_u8x4_create_alignment(unsigned int width, unsigned int height, unsigned int alignment) 47 | { 48 | int stride = 4*width; 49 | 50 | if ((stride % alignment) != 0) 51 | stride += alignment - (stride % alignment); 52 | 53 | uint8_t *buf = calloc(height*stride, sizeof(uint8_t)); 54 | 55 | // const initializer 56 | image_u8x4_t tmp = { .width = width, .height = height, .stride = stride, .buf = buf }; 57 | 58 | image_u8x4_t *im = calloc(1, sizeof(image_u8x4_t)); 59 | memcpy(im, &tmp, sizeof(image_u8x4_t)); 60 | return im; 61 | } 62 | 63 | image_u8x4_t *image_u8x4_copy(const image_u8x4_t *in) 64 | { 65 | uint8_t *buf = malloc(in->height*in->stride*sizeof(uint8_t)); 66 | memcpy(buf, in->buf, in->height*in->stride*sizeof(uint8_t)); 67 | 68 | // const initializer 69 | image_u8x4_t tmp = { .width = in->width, .height = in->height, .stride = in->stride, .buf = buf }; 70 | 71 | image_u8x4_t *copy = calloc(1, sizeof(image_u8x4_t)); 72 | memcpy(copy, &tmp, sizeof(image_u8x4_t)); 73 | return copy; 74 | } 75 | 76 | void image_u8x4_destroy(image_u8x4_t *im) 77 | { 78 | if (!im) 79 | return; 80 | 81 | free(im->buf); 82 | free(im); 83 | } 84 | 85 | //////////////////////////////////////////////////////////// 86 | image_u8x4_t *image_u8x4_create_from_pam(const char *inpath) 87 | { 88 | pam_t *pam = pam_create_from_file(inpath); 89 | if (!pam) 90 | return NULL; 91 | 92 | image_u8x4_t *im = image_u8x4_create(pam->width, pam->height); 93 | 94 | for (int y = 0; y < pam->height; y++) { 95 | if (pam->depth == 1) { 96 | for (int x = 0; x < pam->width; x++) { 97 | im->buf[y*im->stride + 4*x + 0] = pam->data[pam->width*y + x + 0]; 98 | im->buf[y*im->stride + 4*x + 1] = pam->data[pam->width*y + x + 0]; 99 | im->buf[y*im->stride + 4*x + 2] = pam->data[pam->width*y + x + 0]; 100 | im->buf[y*im->stride + 4*x + 3] = 255; 101 | } 102 | } else if (pam->depth == 3) { 103 | for (int x = 0; x < pam->width; x++) { 104 | im->buf[y*im->stride + 4*x + 0] = pam->data[3*pam->width*y + 3*x + 0]; 105 | im->buf[y*im->stride + 4*x + 1] = pam->data[3*pam->width*y + 3*x + 1]; 106 | im->buf[y*im->stride + 4*x + 2] = pam->data[3*pam->width*y + 3*x + 2]; 107 | im->buf[y*im->stride + 4*x + 3] = 255; 108 | } 109 | } else if (pam->depth == 4) { 110 | memcpy(&im->buf[y*im->stride], &pam->data[4*pam->width*y], 4*pam->width); 111 | } else { 112 | assert(0); // not implemented 113 | } 114 | } 115 | 116 | pam_destroy(pam); 117 | return im; 118 | } 119 | //////////////////////////////////////////////////////////// 120 | // PNM file i/o 121 | 122 | // Create an RGBA image from PNM 123 | image_u8x4_t *image_u8x4_create_from_pnm(const char *path) 124 | { 125 | pnm_t *pnmp = pnm_create_from_file(path); 126 | if (pnmp == NULL) 127 | return NULL; 128 | 129 | pnm_t pnm = *pnmp; 130 | image_u8x4_t *imp = NULL; 131 | 132 | switch (pnm.format) { 133 | case PNM_FORMAT_GRAY: { 134 | imp = image_u8x4_create(pnm.width, pnm.height); 135 | 136 | // copy struct by value for common subexpression elimination 137 | const image_u8x4_t im = *imp; 138 | 139 | for (int y = 0; y < im.height; y++) { 140 | for (int x = 0; x < im.width; x++) { 141 | uint8_t gray = pnm.buf[y*pnm.width + x]; 142 | im.buf[y*im.stride + 4*x + 0] = gray; 143 | im.buf[y*im.stride + 4*x + 1] = gray; 144 | im.buf[y*im.stride + 4*x + 2] = gray; 145 | im.buf[y*im.stride + 4*x + 3] = 0xff; 146 | } 147 | } 148 | 149 | break; 150 | } 151 | 152 | case PNM_FORMAT_RGB: { 153 | imp = image_u8x4_create(pnm.width, pnm.height); 154 | 155 | // copy struct by value for common subexpression elimination 156 | const image_u8x4_t im = *imp; 157 | 158 | // Gray conversion for RGB is gray = (r + g + g + b)/4 159 | for (int y = 0; y < im.height; y++) { 160 | for (int x = 0; x < im.width; x++) { 161 | 162 | uint8_t r = pnm.buf[y*pnm.width*3 + 3*x + 0]; 163 | uint8_t g = pnm.buf[y*pnm.width*3 + 3*x + 1]; 164 | uint8_t b = pnm.buf[y*pnm.width*3 + 3*x + 2]; 165 | 166 | im.buf[y*im.stride + 4*x + 0] = r; 167 | im.buf[y*im.stride + 4*x + 1] = g; 168 | im.buf[y*im.stride + 4*x + 2] = b; 169 | im.buf[y*im.stride + 4*x + 3] = 0xff; 170 | } 171 | } 172 | 173 | break; 174 | } 175 | } 176 | 177 | pnm_destroy(pnmp); 178 | return imp; 179 | } 180 | 181 | int image_u8x4_write_pnm(const image_u8x4_t *imp, const char *path) 182 | { 183 | // copy struct by value to ensure common subexpression elimination occurs 184 | const image_u8x4_t im = *imp; 185 | 186 | FILE *f = fopen(path, "wb"); 187 | int res = 0; 188 | 189 | if (f == NULL) { 190 | res = -1; 191 | goto finish; 192 | } 193 | 194 | // Only outputs to RGB 195 | fprintf(f, "P6\n%d %d\n255\n", im.width, im.height); 196 | 197 | for (int y = im.height-1; y >= 0; y--) { 198 | for (int x = 0; x < im.width; x++) { 199 | 200 | uint8_t r = im.buf[y*im.stride + 4*x + 0]; 201 | uint8_t g = im.buf[y*im.stride + 4*x + 1]; 202 | uint8_t b = im.buf[y*im.stride + 4*x + 2]; 203 | 204 | fwrite(&r, 1, 1, f); 205 | fwrite(&g, 1, 1, f); 206 | fwrite(&b, 1, 1, f); 207 | } 208 | } 209 | 210 | finish: 211 | if (f != NULL) 212 | fclose(f); 213 | 214 | return res; 215 | } 216 | 217 | void image_u8x4_write_pam(const image_u8x4_t *im, const char *path) 218 | { 219 | FILE *f = fopen(path, "w"); 220 | fprintf(f, "P7\n"); 221 | fprintf(f, "WIDTH %d\n", im->width); 222 | fprintf(f, "HEIGHT %d\n", im->height); 223 | fprintf(f, "DEPTH 4\n"); 224 | fprintf(f, "MAXVAL 255\n"); 225 | fprintf(f, "TUPLTYPE RGB_ALPHA\n"); 226 | fprintf(f, "ENDHDR\n"); 227 | 228 | for (int y = 0; y < im->height; y++) 229 | fwrite(&im->buf[y*im->stride], 1, 4*im->width, f); 230 | 231 | fclose(f); 232 | 233 | } 234 | -------------------------------------------------------------------------------- /common/image_u8x4.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include "image_types.h" 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | 38 | ///////////////////////////////////// 39 | // IMPORTANT NOTE ON BYTE ORDER 40 | // 41 | // Format conversion routines will (unless otherwise specified) assume 42 | // R, G, B, A ordering of bytes. 43 | // 44 | ///////////////////////////////////// 45 | 46 | // Create or load an image. returns NULL on failure 47 | image_u8x4_t *image_u8x4_create(unsigned int width, unsigned int height); 48 | image_u8x4_t *image_u8x4_create_alignment(unsigned int width, unsigned int height, unsigned int alignment); 49 | image_u8x4_t *image_u8x4_create_from_pnm(const char *path); 50 | 51 | image_u8x4_t *image_u8x4_copy(const image_u8x4_t *in); 52 | 53 | void image_u8x4_destroy(image_u8x4_t *im); 54 | 55 | // Write a pnm. Return 0 on success. 56 | // Currently supports GRAY and RGB 57 | int image_u8x4_write_pnm(const image_u8x4_t *im, const char *path); 58 | 59 | image_u8x4_t *image_u8x4_create_from_pam(const char *path); 60 | 61 | void image_u8x4_write_pam(const image_u8x4_t *im, const char *path); 62 | 63 | #ifdef __cplusplus 64 | } 65 | #endif 66 | -------------------------------------------------------------------------------- /common/math_util.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #pragma once 29 | 30 | #define _USE_MATH_DEFINES 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #ifdef __cplusplus 37 | extern "C" { 38 | #endif 39 | 40 | #define to_radians(x) ( (x) * (M_PI / 180.0 )) 41 | #define to_degrees(x) ( (x) * (180.0 / M_PI )) 42 | 43 | /* DEPRECATE, threshold meaningless without context. 44 | static inline int dequals(double a, double b) 45 | { 46 | double thresh = 1e-9; 47 | return (fabs(a-b) < thresh); 48 | } 49 | */ 50 | 51 | static inline int dequals_mag(double a, double b, double thresh) 52 | { 53 | return (fabs(a-b) < thresh); 54 | } 55 | 56 | static inline int isq(int v) 57 | { 58 | return v*v; 59 | } 60 | 61 | static inline float fsq(float v) 62 | { 63 | return v*v; 64 | } 65 | 66 | static inline double sq(double v) 67 | { 68 | return v*v; 69 | } 70 | 71 | static inline double sgn(double v) 72 | { 73 | return (v>=0) ? 1 : -1; 74 | } 75 | 76 | // random number between [0, 1) 77 | static inline float randf() 78 | { 79 | return (float)(rand() / (RAND_MAX + 1.0)); 80 | } 81 | 82 | 83 | static inline float signed_randf() 84 | { 85 | return randf()*2 - 1; 86 | } 87 | 88 | // return a random integer between [0, bound) 89 | static inline int irand(int bound) 90 | { 91 | int v = (int) (randf()*bound); 92 | if (v == bound) 93 | return (bound-1); 94 | //assert(v >= 0); 95 | //assert(v < bound); 96 | return v; 97 | } 98 | 99 | /** Map vin to [0, 2*PI) **/ 100 | static inline double mod2pi_positive(double vin) 101 | { 102 | return vin - M_2_PI * floor(vin / M_2_PI); 103 | } 104 | 105 | /** Map vin to [-PI, PI) **/ 106 | static inline double mod2pi(double vin) 107 | { 108 | return mod2pi_positive(vin + M_PI) - M_PI; 109 | } 110 | 111 | /** Return vin such that it is within PI degrees of ref **/ 112 | static inline double mod2pi_ref(double ref, double vin) 113 | { 114 | return ref + mod2pi(vin - ref); 115 | } 116 | 117 | /** Map vin to [0, 360) **/ 118 | static inline double mod360_positive(double vin) 119 | { 120 | return vin - 360 * floor(vin / 360); 121 | } 122 | 123 | /** Map vin to [-180, 180) **/ 124 | static inline double mod360(double vin) 125 | { 126 | return mod360_positive(vin + 180) - 180; 127 | } 128 | 129 | static inline int mod_positive(int vin, int mod) { 130 | return (vin % mod + mod) % mod; 131 | } 132 | 133 | static inline int theta_to_int(double theta, int max) 134 | { 135 | theta = mod2pi_ref(M_PI, theta); 136 | int v = (int) (theta / M_2_PI * max); 137 | 138 | if (v == max) 139 | v = 0; 140 | 141 | assert (v >= 0 && v < max); 142 | 143 | return v; 144 | } 145 | 146 | static inline int imin(int a, int b) 147 | { 148 | return (a < b) ? a : b; 149 | } 150 | 151 | static inline int imax(int a, int b) 152 | { 153 | return (a > b) ? a : b; 154 | } 155 | 156 | static inline int64_t imin64(int64_t a, int64_t b) 157 | { 158 | return (a < b) ? a : b; 159 | } 160 | 161 | static inline int64_t imax64(int64_t a, int64_t b) 162 | { 163 | return (a > b) ? a : b; 164 | } 165 | 166 | static inline int iclamp(int v, int minv, int maxv) 167 | { 168 | return imax(minv, imin(v, maxv)); 169 | } 170 | 171 | static inline double dclamp(double a, double min, double max) 172 | { 173 | if (a < min) 174 | return min; 175 | if (a > max) 176 | return max; 177 | return a; 178 | } 179 | 180 | static inline int fltcmp (float f1, float f2) 181 | { 182 | float epsilon = f1-f2; 183 | if (epsilon < 0.0) 184 | return -1; 185 | else if (epsilon > 0.0) 186 | return 1; 187 | else 188 | return 0; 189 | } 190 | 191 | static inline int dblcmp (double d1, double d2) 192 | { 193 | double epsilon = d1-d2; 194 | if (epsilon < 0.0) 195 | return -1; 196 | else if (epsilon > 0.0) 197 | return 1; 198 | else 199 | return 0; 200 | } 201 | 202 | #ifdef __cplusplus 203 | } 204 | #endif 205 | -------------------------------------------------------------------------------- /common/pam.c: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include "pam.h" 34 | 35 | pam_t *pam_create_from_file(const char *inpath) 36 | { 37 | FILE *infile = fopen(inpath, "r"); 38 | if (infile == NULL) { 39 | printf("pam.c: couldn't open input file: %s\n", inpath); 40 | return NULL; 41 | } 42 | 43 | pam_t *pam = calloc(1, sizeof(pam_t)); 44 | pam->width = -1; 45 | pam->height = -1; 46 | pam->depth = -1; 47 | pam->maxval = -1; 48 | pam->type = -1; 49 | 50 | int linenumber = 0; 51 | 52 | while (1) { 53 | char line[1024]; 54 | if (!fgets(line, sizeof(line), infile)) { 55 | printf("pam.c: unexpected EOF\n"); 56 | goto fail; 57 | } 58 | linenumber++; 59 | 60 | char *tok0 = line; 61 | char *tok1 = NULL; 62 | 63 | if (line[0] == '#') // comment 64 | continue; 65 | 66 | size_t linelen = strlen(line); 67 | for (size_t idx = 0; idx < linelen; idx++) { 68 | if (line[idx] == ' ') { 69 | line[idx] = 0; 70 | if (tok1) { 71 | printf("pam.c: More than two tokens, %s:%d\n", inpath, linenumber); 72 | } 73 | 74 | tok1 = &line[idx+1]; 75 | } 76 | if (line[idx] == '\n') 77 | line[idx] = 0; 78 | } 79 | 80 | if (!strcmp(tok0, "P7")) 81 | continue; 82 | 83 | if (!strcmp(tok0, "ENDHDR")) 84 | break; 85 | 86 | if (!strcmp(tok0, "WIDTH") && tok1) { 87 | pam->width = atoi(tok1); 88 | continue; 89 | } 90 | 91 | if (!strcmp(tok0, "HEIGHT") && tok1) { 92 | pam->height = atoi(tok1); 93 | continue; 94 | } 95 | 96 | if (!strcmp(tok0, "DEPTH") && tok1) { 97 | pam->depth = atoi(tok1); 98 | continue; 99 | } 100 | 101 | if (!strcmp(tok0, "MAXVAL") && tok1) { 102 | pam->maxval = atoi(tok1); 103 | continue; 104 | } 105 | 106 | if (!strcmp(tok0, "TUPLTYPE") && tok1) { 107 | if (!strcmp(tok1, "GRAYSCALE_ALPHA")) { 108 | pam->type = PAM_GRAYSCALE_ALPHA; 109 | continue; 110 | } 111 | 112 | if (!strcmp(tok1, "RGB_ALPHA")) { 113 | pam->type = PAM_RGB_ALPHA; 114 | continue; 115 | } 116 | 117 | if (!strcmp(tok1, "RGB")) { 118 | pam->type = PAM_RGB; 119 | continue; 120 | } 121 | 122 | if (!strcmp(tok1, "GRAYSCALE")) { 123 | pam->type = PAM_GRAYSCALE; 124 | continue; 125 | } 126 | 127 | printf("pam.c: unrecognized tupl type %s\n", tok1); 128 | continue; 129 | } 130 | 131 | printf("pam.c: unrecognized attribute %s\n", tok0); 132 | } 133 | 134 | if (pam->width < 0 || pam->height < 0 || pam->depth < 0 || 135 | pam->maxval < 0 || pam->type < 0) { 136 | printf("pam.c: missing required metadata field\n"); 137 | goto fail; 138 | } 139 | 140 | assert(pam->maxval == 255); 141 | 142 | pam->datalen = pam->width * pam->height * pam->depth; 143 | pam->data = malloc(pam->datalen); 144 | if (pam->datalen != fread(pam->data, 1, pam->datalen, infile)) { 145 | printf("pam.c: couldn't read body\n"); 146 | goto fail; 147 | } 148 | 149 | fclose(infile); 150 | return pam; 151 | 152 | fail: 153 | free(pam); 154 | fclose(infile); 155 | return NULL; 156 | } 157 | 158 | int pam_write_file(pam_t *pam, const char *outpath) 159 | { 160 | FILE *f = fopen(outpath, "w+"); 161 | if (!f) 162 | return -1; 163 | 164 | const char *tupl = NULL; 165 | switch (pam->type) { 166 | case PAM_GRAYSCALE_ALPHA: 167 | tupl = "GRAYSCALE_ALPHA"; 168 | break; 169 | case PAM_RGB_ALPHA: 170 | tupl = "RGB_ALPHA"; 171 | break; 172 | case PAM_RGB: 173 | tupl = "RGB"; 174 | break; 175 | case PAM_GRAYSCALE: 176 | tupl = "GRAYSCALE"; 177 | break; 178 | default: 179 | assert(0); 180 | } 181 | 182 | fprintf(f, "P7\nWIDTH %d\nHEIGHT %d\nDEPTH %d\nMAXVAL %d\nTUPLTYPE %s\nENDHDR\n", 183 | pam->width, pam->height, pam->depth, pam->maxval, tupl); 184 | size_t len = pam->width * pam->height * pam->depth; 185 | if (len != fwrite(pam->data, 1, len, f)) { 186 | fclose(f); 187 | return -2; 188 | } 189 | 190 | fclose(f); 191 | 192 | return 0; 193 | } 194 | 195 | void pam_destroy(pam_t *pam) 196 | { 197 | if (!pam) 198 | return; 199 | 200 | free(pam->data); 201 | free(pam); 202 | } 203 | 204 | pam_t *pam_copy(pam_t *pam) 205 | { 206 | pam_t *copy = calloc(1, sizeof(pam_t)); 207 | copy->width = pam->width; 208 | copy->height = pam->height; 209 | copy->depth = pam->depth; 210 | copy->maxval = pam->maxval; 211 | copy->type = pam->type; 212 | 213 | copy->datalen = pam->datalen; 214 | copy->data = malloc(pam->datalen); 215 | memcpy(copy->data, pam->data, pam->datalen); 216 | 217 | return copy; 218 | } 219 | 220 | pam_t *pam_convert(pam_t *in, int type) 221 | { 222 | if (type == in->type) 223 | return pam_copy(in); 224 | 225 | assert(type == PAM_RGB_ALPHA); // we don't support a lot yet 226 | assert(in->maxval == 255); 227 | 228 | int w = in->width; 229 | int h = in->height; 230 | 231 | pam_t *out = calloc(1, sizeof(pam_t)); 232 | out->type = type; 233 | out->width = w; 234 | out->height = h; 235 | out->maxval = in->maxval; 236 | out->depth = 4; 237 | out->datalen = 4 * w * h; 238 | out->data = malloc(out->datalen); 239 | 240 | if (in->type == PAM_RGB) { 241 | assert(in->depth == 3); 242 | for (int y = 0; y < h; y++) { 243 | for (int x = 0; x < w; x++) { 244 | out->data[y*4*w + 4*x + 0] = in->data[y*3*w + 3*x + 0]; 245 | out->data[y*4*w + 4*x + 1] = in->data[y*3*w + 3*x + 1]; 246 | out->data[y*4*w + 4*x + 2] = in->data[y*3*w + 3*x + 2]; 247 | out->data[y*4*w + 4*x + 3] = 255; 248 | } 249 | } 250 | } else { 251 | printf("pam.c unsupported type %d\n", in->type); 252 | assert(0); 253 | } 254 | 255 | return out; 256 | } 257 | -------------------------------------------------------------------------------- /common/pam.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | enum { PAM_GRAYSCALE_ALPHA = 5000, PAM_RGB_ALPHA, PAM_RGB, PAM_GRAYSCALE }; 33 | 34 | typedef struct pam pam_t; 35 | struct pam 36 | { 37 | int type; // one of PAM_* 38 | 39 | int width, height; // note, stride always width. 40 | int depth; // bytes per pixel 41 | int maxval; // maximum value per channel, e.g. 255 for 8bpp 42 | 43 | size_t datalen; // in bytes 44 | uint8_t *data; 45 | }; 46 | 47 | pam_t *pam_create_from_file(const char *inpath); 48 | int pam_write_file(pam_t *pam, const char *outpath); 49 | void pam_destroy(pam_t *pam); 50 | 51 | pam_t *pam_copy(pam_t *pam); 52 | 53 | // NB doesn't handle many conversions yet. 54 | pam_t *pam_convert(pam_t *in, int type); 55 | -------------------------------------------------------------------------------- /common/pjpeg.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #pragma once 29 | 30 | #include "image_u8.h" 31 | #include "image_u8x3.h" 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | typedef struct pjpeg_component pjpeg_component_t; 38 | struct pjpeg_component 39 | { 40 | // resolution of this component (which is smaller than the 41 | // dimensions of the image if the channel has been sub-sampled.) 42 | uint32_t width, height; 43 | 44 | // number of bytes per row. May be larger than width for alignment 45 | // reasons. 46 | uint32_t stride; 47 | 48 | // data[y*stride + x] 49 | uint8_t *data; 50 | 51 | //////////////////////////////////////////////////////////////// 52 | // These items probably not of great interest to most 53 | // applications. 54 | uint8_t id; // the identifier associated with this component 55 | uint8_t hv; // horiz scale (high 4 bits) / vert scale (low 4 bits) 56 | uint8_t scalex, scaley; // derived from hv above 57 | uint8_t tq; // quantization table index 58 | 59 | // this filled in at the last moment by SOS 60 | uint8_t tda; // which huff tables will we use for DC (high 4 bits) and AC (low 4 bits) 61 | }; 62 | 63 | typedef struct pjpeg pjpeg_t; 64 | struct pjpeg 65 | { 66 | // status of the decode is put here. Non-zero means error. 67 | int error; 68 | 69 | uint32_t width, height; // pixel dimensions 70 | 71 | int ncomponents; 72 | pjpeg_component_t *components; 73 | }; 74 | 75 | enum PJPEG_FLAGS { 76 | PJPEG_STRICT = 1, // Don't try to recover from errors. 77 | PJPEG_MJPEG = 2, // Support JPGs with missing DHT segments. 78 | }; 79 | 80 | enum PJPEG_ERROR { 81 | PJPEG_OKAY = 0, 82 | PJPEG_ERR_FILE, // something wrong reading file 83 | PJPEG_ERR_DQT, // something wrong with DQT marker 84 | PJPEG_ERR_SOF, // something wrong with SOF marker 85 | PJPEG_ERR_DHT, // something wrong with DHT marker 86 | PJPEG_ERR_SOS, // something wrong with SOS marker 87 | PJPEG_ERR_MISSING_DHT, // missing a necessary huffman table 88 | PJPEG_ERR_DRI, // something wrong with DRI marker 89 | PJPEG_ERR_RESET, // didn't get a reset marker where we expected. Corruption? 90 | PJPEG_ERR_EOF, // ran out of bytes while decoding 91 | PJEPG_ERR_UNSUPPORTED, // an unsupported format 92 | }; 93 | 94 | pjpeg_t *pjpeg_create_from_file(const char *path, uint32_t flags, int *error); 95 | pjpeg_t *pjpeg_create_from_buffer(uint8_t *buf, int buflen, uint32_t flags, int *error); 96 | void pjpeg_destroy(pjpeg_t *pj); 97 | 98 | image_u8_t *pjpeg_to_u8_baseline(pjpeg_t *pj); 99 | image_u8x3_t *pjpeg_to_u8x3_baseline(pjpeg_t *pj); 100 | 101 | #ifdef __cplusplus 102 | } 103 | #endif 104 | -------------------------------------------------------------------------------- /common/pnm.c: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | #include "pnm.h" 33 | 34 | pnm_t *pnm_create_from_file(const char *path) 35 | { 36 | FILE *f = fopen(path, "rb"); 37 | if (f == NULL) 38 | return NULL; 39 | 40 | pnm_t *pnm = calloc(1, sizeof(pnm_t)); 41 | pnm->format = -1; 42 | 43 | char tmp[1024]; 44 | int nparams = 0; // will be 3 when we're all done. 45 | int params[3]; 46 | 47 | while (nparams < 3 && !(pnm->format == PNM_FORMAT_BINARY && nparams == 2)) { 48 | if (fgets(tmp, sizeof(tmp), f) == NULL) 49 | goto error; 50 | 51 | // skip comments 52 | if (tmp[0]=='#') 53 | continue; 54 | 55 | char *p = tmp; 56 | 57 | if (pnm->format == -1 && tmp[0]=='P') { 58 | pnm->format = tmp[1]-'0'; 59 | assert(pnm->format == PNM_FORMAT_GRAY || pnm->format == PNM_FORMAT_RGB || pnm->format == PNM_FORMAT_BINARY); 60 | p = &tmp[2]; 61 | } 62 | 63 | // pull integers out of this line until there are no more. 64 | while (nparams < 3 && *p!=0) { 65 | while (*p==' ') 66 | p++; 67 | 68 | // encounter rubbish? (End of line?) 69 | if (*p < '0' || *p > '9') 70 | break; 71 | 72 | int acc = 0; 73 | while (*p >= '0' && *p <= '9') { 74 | acc = acc*10 + *p - '0'; 75 | p++; 76 | } 77 | 78 | params[nparams++] = acc; 79 | p++; 80 | } 81 | } 82 | 83 | pnm->width = params[0]; 84 | pnm->height = params[1]; 85 | pnm->max = params[2]; 86 | 87 | switch (pnm->format) { 88 | case PNM_FORMAT_BINARY: { 89 | // files in the wild sometimes simply don't set max 90 | pnm->max = 1; 91 | 92 | pnm->buflen = pnm->height * ((pnm->width + 7) / 8); 93 | pnm->buf = malloc(pnm->buflen); 94 | size_t len = fread(pnm->buf, 1, pnm->buflen, f); 95 | if (len != pnm->buflen) 96 | goto error; 97 | 98 | fclose(f); 99 | return pnm; 100 | } 101 | 102 | case PNM_FORMAT_GRAY: { 103 | if (pnm->max == 255) 104 | pnm->buflen = pnm->width * pnm->height; 105 | else if (pnm->max == 65535) 106 | pnm->buflen = 2 * pnm->width * pnm->height; 107 | else 108 | assert(0); 109 | 110 | pnm->buf = malloc(pnm->buflen); 111 | size_t len = fread(pnm->buf, 1, pnm->buflen, f); 112 | if (len != pnm->buflen) 113 | goto error; 114 | 115 | fclose(f); 116 | return pnm; 117 | } 118 | 119 | case PNM_FORMAT_RGB: { 120 | if (pnm->max == 255) 121 | pnm->buflen = pnm->width * pnm->height * 3; 122 | else if (pnm->max == 65535) 123 | pnm->buflen = 2 * pnm->width * pnm->height * 3; 124 | else 125 | assert(0); 126 | 127 | pnm->buf = malloc(pnm->buflen); 128 | size_t len = fread(pnm->buf, 1, pnm->buflen, f); 129 | if (len != pnm->buflen) 130 | goto error; 131 | fclose(f); 132 | return pnm; 133 | } 134 | } 135 | 136 | error: 137 | fclose(f); 138 | 139 | if (pnm != NULL) { 140 | free(pnm->buf); 141 | free(pnm); 142 | } 143 | 144 | return NULL; 145 | } 146 | 147 | void pnm_destroy(pnm_t *pnm) 148 | { 149 | if (pnm == NULL) 150 | return; 151 | 152 | free(pnm->buf); 153 | free(pnm); 154 | } 155 | -------------------------------------------------------------------------------- /common/pnm.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | #ifdef __cplusplus 33 | extern "C" { 34 | #endif 35 | 36 | #define PNM_FORMAT_BINARY 4 37 | #define PNM_FORMAT_GRAY 5 38 | #define PNM_FORMAT_RGB 6 39 | 40 | // supports ppm, pnm, pgm 41 | 42 | typedef struct pnm pnm_t; 43 | struct pnm 44 | { 45 | int width, height; 46 | int format; 47 | int max; // 1 = binary, 255 = one byte, 65535 = two bytes 48 | 49 | uint32_t buflen; 50 | uint8_t *buf; // if max=65535, in big endian 51 | }; 52 | 53 | pnm_t *pnm_create_from_file(const char *path); 54 | void pnm_destroy(pnm_t *pnm); 55 | 56 | #ifdef __cplusplus 57 | } 58 | #endif 59 | -------------------------------------------------------------------------------- /common/postscript_utils.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #pragma once 29 | 30 | // write commands in postscript language to render an image in the current 31 | // graphics environment. The image will be rendered in one pixel per unit 32 | // with Y up coordinate axis (e.g. upside down). 33 | static void postscript_image(FILE *f, image_u8_t *im) 34 | { 35 | // fprintf(f, "/readstring {\n currentfile exch readhexstring pop\n} bind def\n"); 36 | fprintf(f, "/picstr %d string def\n", im->width); 37 | 38 | fprintf(f, "%d %d 8 [1 0 0 1 0 0]\n", 39 | im->width, im->height); 40 | 41 | fprintf(f, "{currentfile picstr readhexstring pop}\nimage\n"); 42 | 43 | for (int y = 0; y < im->height; y++) { 44 | for (int x = 0; x < im->width; x++) { 45 | uint8_t v = im->buf[y*im->stride + x]; 46 | fprintf(f, "%02x", v); 47 | if ((x % 32)==31) 48 | fprintf(f, "\n"); 49 | } 50 | } 51 | 52 | fprintf(f, "\n"); 53 | } 54 | -------------------------------------------------------------------------------- /common/pthreads_cross.c: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright John Schember 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy of 5 | this software and associated documentation files (the "Software"), to deal in 6 | the Software without restriction, including without limitation the rights to 7 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 8 | of the Software, and to permit persons to whom the Software is furnished to do 9 | so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in all 12 | copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | */ 22 | 23 | #include "common/pthreads_cross.h" 24 | 25 | #ifdef _WIN32 26 | 27 | typedef struct { 28 | SRWLOCK lock; 29 | bool exclusive; 30 | } pthread_rwlock_t; 31 | 32 | int pthread_rwlock_init(pthread_rwlock_t *rwlock, const pthread_rwlockattr_t *attr); 33 | int pthread_rwlock_destroy(pthread_rwlock_t *rwlock); 34 | int pthread_rwlock_rdlock(pthread_rwlock_t *rwlock); 35 | int pthread_rwlock_tryrdlock(pthread_rwlock_t *rwlock); 36 | int pthread_rwlock_wrlock(pthread_rwlock_t *rwlock); 37 | int pthread_rwlock_trywrlock(pthread_rwlock_t *rwlock); 38 | int pthread_rwlock_unlock(pthread_rwlock_t *rwlock); 39 | 40 | int pthread_create(pthread_t *thread, pthread_attr_t *attr, void *(*start_routine)(void *), void *arg) 41 | { 42 | (void) attr; 43 | 44 | if (thread == NULL || start_routine == NULL) 45 | return 1; 46 | 47 | #pragma GCC diagnostic push 48 | #pragma GCC diagnostic ignored "-Wcast-function-type" 49 | *thread = (HANDLE) CreateThread(NULL, 0, (LPTHREAD_START_ROUTINE)start_routine, arg, 0, NULL); 50 | #pragma GCC diagnostic pop 51 | if (*thread == NULL) 52 | return 1; 53 | return 0; 54 | } 55 | 56 | int pthread_join(pthread_t thread, void **value_ptr) 57 | { 58 | (void)value_ptr; 59 | WaitForSingleObject(thread, INFINITE); 60 | CloseHandle(thread); 61 | return 0; 62 | } 63 | 64 | int pthread_detach(pthread_t thread) 65 | { 66 | CloseHandle(thread); 67 | return 0; 68 | } 69 | 70 | int pthread_mutex_init(pthread_mutex_t *mutex, pthread_mutexattr_t *attr) 71 | { 72 | (void)attr; 73 | 74 | if (mutex == NULL) 75 | return 1; 76 | 77 | InitializeCriticalSection(mutex); 78 | return 0; 79 | } 80 | 81 | int pthread_mutex_destroy(pthread_mutex_t *mutex) 82 | { 83 | if (mutex == NULL) 84 | return 1; 85 | DeleteCriticalSection(mutex); 86 | return 0; 87 | } 88 | 89 | int pthread_mutex_lock(pthread_mutex_t *mutex) 90 | { 91 | if (mutex == NULL) 92 | return 1; 93 | EnterCriticalSection(mutex); 94 | return 0; 95 | } 96 | 97 | int pthread_mutex_unlock(pthread_mutex_t *mutex) 98 | { 99 | if (mutex == NULL) 100 | return 1; 101 | LeaveCriticalSection(mutex); 102 | return 0; 103 | } 104 | 105 | int pthread_cond_init(pthread_cond_t *cond, pthread_condattr_t *attr) 106 | { 107 | (void)attr; 108 | if (cond == NULL) 109 | return 1; 110 | InitializeConditionVariable(cond); 111 | return 0; 112 | } 113 | 114 | int pthread_cond_destroy(pthread_cond_t *cond) 115 | { 116 | /* Windows does not have a destroy for conditionals */ 117 | (void)cond; 118 | return 0; 119 | } 120 | 121 | int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex) 122 | { 123 | if (cond == NULL || mutex == NULL) 124 | return 1; 125 | return pthread_cond_timedwait(cond, mutex, NULL); 126 | } 127 | 128 | int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex, 129 | const struct timespec *abstime) 130 | { 131 | if (cond == NULL || mutex == NULL) 132 | return 1; 133 | if (!SleepConditionVariableCS(cond, mutex, timespec_to_ms(abstime))) 134 | return 1; 135 | return 0; 136 | } 137 | 138 | int pthread_cond_signal(pthread_cond_t *cond) 139 | { 140 | if (cond == NULL) 141 | return 1; 142 | WakeConditionVariable(cond); 143 | return 0; 144 | } 145 | 146 | int pthread_cond_broadcast(pthread_cond_t *cond) 147 | { 148 | if (cond == NULL) 149 | return 1; 150 | WakeAllConditionVariable(cond); 151 | return 0; 152 | } 153 | 154 | int pthread_rwlock_init(pthread_rwlock_t *rwlock, const pthread_rwlockattr_t *attr) 155 | { 156 | (void)attr; 157 | if (rwlock == NULL) 158 | return 1; 159 | InitializeSRWLock(&(rwlock->lock)); 160 | rwlock->exclusive = false; 161 | return 0; 162 | } 163 | 164 | int pthread_rwlock_destroy(pthread_rwlock_t *rwlock) 165 | { 166 | (void)rwlock; 167 | return 0; 168 | } 169 | 170 | int pthread_rwlock_rdlock(pthread_rwlock_t *rwlock) 171 | { 172 | if (rwlock == NULL) 173 | return 1; 174 | AcquireSRWLockShared(&(rwlock->lock)); 175 | return 0; 176 | } 177 | 178 | int pthread_rwlock_tryrdlock(pthread_rwlock_t *rwlock) 179 | { 180 | if (rwlock == NULL) 181 | return 1; 182 | return !TryAcquireSRWLockShared(&(rwlock->lock)); 183 | } 184 | 185 | int pthread_rwlock_wrlock(pthread_rwlock_t *rwlock) 186 | { 187 | if (rwlock == NULL) 188 | return 1; 189 | AcquireSRWLockExclusive(&(rwlock->lock)); 190 | rwlock->exclusive = true; 191 | return 0; 192 | } 193 | 194 | int pthread_rwlock_trywrlock(pthread_rwlock_t *rwlock) 195 | { 196 | BOOLEAN ret; 197 | 198 | if (rwlock == NULL) 199 | return 1; 200 | 201 | ret = TryAcquireSRWLockExclusive(&(rwlock->lock)); 202 | if (ret) 203 | rwlock->exclusive = true; 204 | return ret; 205 | } 206 | 207 | int pthread_rwlock_unlock(pthread_rwlock_t *rwlock) 208 | { 209 | if (rwlock == NULL) 210 | return 1; 211 | 212 | if (rwlock->exclusive) { 213 | rwlock->exclusive = false; 214 | ReleaseSRWLockExclusive(&(rwlock->lock)); 215 | } else { 216 | ReleaseSRWLockShared(&(rwlock->lock)); 217 | } 218 | return 0; 219 | } 220 | 221 | int sched_yield() { 222 | return (int)SwitchToThread(); 223 | } 224 | 225 | void ms_to_timespec(struct timespec *ts, unsigned int ms) 226 | { 227 | if (ts == NULL) 228 | return; 229 | ts->tv_sec = (ms / 1000) + time(NULL); 230 | ts->tv_nsec = (ms % 1000) * 1000000; 231 | } 232 | 233 | unsigned int timespec_to_ms(const struct timespec *abstime) 234 | { 235 | if (abstime == NULL) 236 | return INFINITE; 237 | 238 | return ((abstime->tv_sec - time(NULL)) * 1000) + (abstime->tv_nsec / 1000000); 239 | } 240 | 241 | unsigned int pcthread_get_num_procs() 242 | { 243 | SYSTEM_INFO sysinfo; 244 | 245 | GetSystemInfo(&sysinfo); 246 | return sysinfo.dwNumberOfProcessors; 247 | } 248 | 249 | #else 250 | 251 | #include 252 | unsigned int pcthread_get_num_procs() 253 | { 254 | return (unsigned int)sysconf(_SC_NPROCESSORS_ONLN); 255 | } 256 | #endif 257 | -------------------------------------------------------------------------------- /common/pthreads_cross.h: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright John Schember 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy of 5 | this software and associated documentation files (the "Software"), to deal in 6 | the Software without restriction, including without limitation the rights to 7 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 8 | of the Software, and to permit persons to whom the Software is furnished to do 9 | so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in all 12 | copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | */ 22 | 23 | #ifndef __CPTHREAD_H__ 24 | #define __CPTHREAD_H__ 25 | 26 | #ifdef _WIN32 27 | #include 28 | #include 29 | #else 30 | #include 31 | #include 32 | #endif 33 | #include 34 | 35 | #ifdef _WIN32 36 | 37 | typedef CRITICAL_SECTION pthread_mutex_t; 38 | typedef void pthread_mutexattr_t; 39 | typedef void pthread_attr_t; 40 | typedef void pthread_condattr_t; 41 | typedef void pthread_rwlockattr_t; 42 | typedef HANDLE pthread_t; 43 | typedef CONDITION_VARIABLE pthread_cond_t; 44 | 45 | #ifdef __cplusplus 46 | extern "C" { 47 | #endif 48 | int pthread_create(pthread_t *thread, pthread_attr_t *attr, void *(*start_routine)(void *), void *arg); 49 | int pthread_join(pthread_t thread, void **value_ptr); 50 | int pthread_detach(pthread_t); 51 | 52 | int pthread_mutex_init(pthread_mutex_t *mutex, pthread_mutexattr_t *attr); 53 | int pthread_mutex_destroy(pthread_mutex_t *mutex); 54 | int pthread_mutex_lock(pthread_mutex_t *mutex); 55 | int pthread_mutex_unlock(pthread_mutex_t *mutex); 56 | 57 | int pthread_cond_init(pthread_cond_t *cond, pthread_condattr_t *attr); 58 | int pthread_cond_destroy(pthread_cond_t *cond); 59 | int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex); 60 | int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex, const struct timespec *abstime); 61 | int pthread_cond_signal(pthread_cond_t *cond); 62 | int pthread_cond_broadcast(pthread_cond_t *cond); 63 | 64 | int sched_yield(void); 65 | #ifdef __cplusplus 66 | } 67 | #endif 68 | #endif 69 | 70 | 71 | #ifdef __cplusplus 72 | extern "C" { 73 | #endif 74 | unsigned int pcthread_get_num_procs(); 75 | 76 | void ms_to_timespec(struct timespec *ts, unsigned int ms); 77 | unsigned int timespec_to_ms(const struct timespec *abstime); 78 | #ifdef __cplusplus 79 | } 80 | #endif 81 | 82 | #endif /* __CPTHREAD_H__ */ 83 | -------------------------------------------------------------------------------- /common/svd22.c: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #include 29 | 30 | /** SVD 2x2. 31 | 32 | Computes singular values and vectors without squaring the input 33 | matrix. With double precision math, results are accurate to about 34 | 1E-16. 35 | 36 | U = [ cos(theta) -sin(theta) ] 37 | [ sin(theta) cos(theta) ] 38 | 39 | S = [ e 0 ] 40 | [ 0 f ] 41 | 42 | V = [ cos(phi) -sin(phi) ] 43 | [ sin(phi) cos(phi) ] 44 | 45 | 46 | Our strategy is basically to analytically multiply everything out 47 | and then rearrange so that we can solve for theta, phi, e, and 48 | f. (Derivation by ebolson@umich.edu 5/2016) 49 | 50 | V' = [ CP SP ] 51 | [ -SP CP ] 52 | 53 | USV' = [ CT -ST ][ e*CP e*SP ] 54 | [ ST CT ][ -f*SP f*CP ] 55 | 56 | = [e*CT*CP + f*ST*SP e*CT*SP - f*ST*CP ] 57 | [e*ST*CP - f*SP*CT e*SP*ST + f*CP*CT ] 58 | 59 | A00+A11 = e*CT*CP + f*ST*SP + e*SP*ST + f*CP*CT 60 | = e*(CP*CT + SP*ST) + f*(SP*ST + CP*CT) 61 | = (e+f)(CP*CT + SP*ST) 62 | B0 = (e+f)*cos(P-T) 63 | 64 | A00-A11 = e*CT*CP + f*ST*SP - e*SP*ST - f*CP*CT 65 | = e*(CP*CT - SP*ST) - f*(-ST*SP + CP*CT) 66 | = (e-f)(CP*CT - SP*ST) 67 | B1 = (e-f)*cos(P+T) 68 | 69 | A01+A10 = e*CT*SP - f*ST*CP + e*ST*CP - f*SP*CT 70 | = e(CT*SP + ST*CP) - f*(ST*CP + SP*CT) 71 | = (e-f)*(CT*SP + ST*CP) 72 | B2 = (e-f)*sin(P+T) 73 | 74 | A01-A10 = e*CT*SP - f*ST*CP - e*ST*CP + f*SP*CT 75 | = e*(CT*SP - ST*CP) + f(SP*CT - ST*CP) 76 | = (e+f)*(CT*SP - ST*CP) 77 | B3 = (e+f)*sin(P-T) 78 | 79 | B0 = (e+f)*cos(P-T) 80 | B1 = (e-f)*cos(P+T) 81 | B2 = (e-f)*sin(P+T) 82 | B3 = (e+f)*sin(P-T) 83 | 84 | B3/B0 = tan(P-T) 85 | 86 | B2/B1 = tan(P+T) 87 | **/ 88 | void svd22(const double A[4], double U[4], double S[2], double V[4]) 89 | { 90 | double A00 = A[0]; 91 | double A01 = A[1]; 92 | double A10 = A[2]; 93 | double A11 = A[3]; 94 | 95 | double B0 = A00 + A11; 96 | double B1 = A00 - A11; 97 | double B2 = A01 + A10; 98 | double B3 = A01 - A10; 99 | 100 | double PminusT = atan2(B3, B0); 101 | double PplusT = atan2(B2, B1); 102 | 103 | double P = (PminusT + PplusT) / 2; 104 | double T = (-PminusT + PplusT) / 2; 105 | 106 | double CP = cos(P), SP = sin(P); 107 | double CT = cos(T), ST = sin(T); 108 | 109 | U[0] = CT; 110 | U[1] = -ST; 111 | U[2] = ST; 112 | U[3] = CT; 113 | 114 | V[0] = CP; 115 | V[1] = -SP; 116 | V[2] = SP; 117 | V[3] = CP; 118 | 119 | // C0 = e+f. There are two ways to compute C0; we pick the one 120 | // that is better conditioned. 121 | double CPmT = cos(P-T), SPmT = sin(P-T); 122 | double C0 = 0; 123 | if (fabs(CPmT) > fabs(SPmT)) 124 | C0 = B0 / CPmT; 125 | else 126 | C0 = B3 / SPmT; 127 | 128 | // C1 = e-f. There are two ways to compute C1; we pick the one 129 | // that is better conditioned. 130 | double CPpT = cos(P+T), SPpT = sin(P+T); 131 | double C1 = 0; 132 | if (fabs(CPpT) > fabs(SPpT)) 133 | C1 = B1 / CPpT; 134 | else 135 | C1 = B2 / SPpT; 136 | 137 | // e and f are the singular values 138 | double e = (C0 + C1) / 2; 139 | double f = (C0 - C1) / 2; 140 | 141 | if (e < 0) { 142 | e = -e; 143 | U[0] = -U[0]; 144 | U[2] = -U[2]; 145 | } 146 | 147 | if (f < 0) { 148 | f = -f; 149 | U[1] = -U[1]; 150 | U[3] = -U[3]; 151 | } 152 | 153 | // sort singular values. 154 | if (e > f) { 155 | // already in big-to-small order. 156 | S[0] = e; 157 | S[1] = f; 158 | } else { 159 | // Curiously, this code never seems to get invoked. Why is it 160 | // that S[0] always ends up the dominant vector? However, 161 | // this code has been tested (flipping the logic forces us to 162 | // sort the singular values in ascending order). 163 | // 164 | // P = [ 0 1 ; 1 0 ] 165 | // USV' = (UP)(PSP)(PV') 166 | // = (UP)(PSP)(VP)' 167 | // = (UP)(PSP)(P'V')' 168 | S[0] = f; 169 | S[1] = e; 170 | 171 | // exchange columns of U and V 172 | double tmp[2]; 173 | tmp[0] = U[0]; 174 | tmp[1] = U[2]; 175 | U[0] = U[1]; 176 | U[2] = U[3]; 177 | U[1] = tmp[0]; 178 | U[3] = tmp[1]; 179 | 180 | tmp[0] = V[0]; 181 | tmp[1] = V[2]; 182 | V[0] = V[1]; 183 | V[2] = V[3]; 184 | V[1] = tmp[0]; 185 | V[3] = tmp[1]; 186 | } 187 | 188 | /* 189 | double SM[4] = { S[0], 0, 0, S[1] }; 190 | 191 | doubles_print_mat(U, 2, 2, "%20.10g"); 192 | doubles_print_mat(SM, 2, 2, "%20.10g"); 193 | doubles_print_mat(V, 2, 2, "%20.10g"); 194 | printf("A:\n"); 195 | doubles_print_mat(A, 2, 2, "%20.10g"); 196 | 197 | double SVt[4]; 198 | doubles_mat_ABt(SM, 2, 2, V, 2, 2, SVt, 2, 2); 199 | double USVt[4]; 200 | doubles_mat_AB(U, 2, 2, SVt, 2, 2, USVt, 2, 2); 201 | 202 | printf("USVt\n"); 203 | doubles_print_mat(USVt, 2, 2, "%20.10g"); 204 | 205 | double diff[4]; 206 | for (int i = 0; i < 4; i++) 207 | diff[i] = A[i] - USVt[i]; 208 | 209 | printf("diff\n"); 210 | doubles_print_mat(diff, 2, 2, "%20.10g"); 211 | 212 | */ 213 | 214 | } 215 | 216 | 217 | // for the matrix [a b; b d] 218 | void svd_sym_singular_values(double A00, double A01, double A11, 219 | double *Lmin, double *Lmax) 220 | { 221 | double A10 = A01; 222 | 223 | double B0 = A00 + A11; 224 | double B1 = A00 - A11; 225 | double B2 = A01 + A10; 226 | double B3 = A01 - A10; 227 | 228 | double PminusT = atan2(B3, B0); 229 | double PplusT = atan2(B2, B1); 230 | 231 | double P = (PminusT + PplusT) / 2; 232 | double T = (-PminusT + PplusT) / 2; 233 | 234 | // C0 = e+f. There are two ways to compute C0; we pick the one 235 | // that is better conditioned. 236 | double CPmT = cos(P-T), SPmT = sin(P-T); 237 | double C0 = 0; 238 | if (fabs(CPmT) > fabs(SPmT)) 239 | C0 = B0 / CPmT; 240 | else 241 | C0 = B3 / SPmT; 242 | 243 | // C1 = e-f. There are two ways to compute C1; we pick the one 244 | // that is better conditioned. 245 | double CPpT = cos(P+T), SPpT = sin(P+T); 246 | double C1 = 0; 247 | if (fabs(CPpT) > fabs(SPpT)) 248 | C1 = B1 / CPpT; 249 | else 250 | C1 = B2 / SPpT; 251 | 252 | // e and f are the singular values 253 | double e = (C0 + C1) / 2; 254 | double f = (C0 - C1) / 2; 255 | 256 | *Lmin = fmin(e, f); 257 | *Lmax = fmax(e, f); 258 | } 259 | -------------------------------------------------------------------------------- /common/svd22.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #pragma once 29 | 30 | void svd22(const double A[4], double U[4], double S[2], double V[4]); 31 | 32 | // for the matrix [a b; b d] 33 | void svd_sym_singular_values(double A00, double A01, double A11, 34 | double *Lmin, double *Lmax); 35 | -------------------------------------------------------------------------------- /common/time_util.c: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #include 29 | #include 30 | #include "time_util.h" 31 | 32 | struct timeutil_rest 33 | { 34 | int64_t acc_time; 35 | int64_t start_time; 36 | }; 37 | 38 | timeutil_rest_t *timeutil_rest_create() 39 | { 40 | timeutil_rest_t *rest = calloc(1, sizeof(timeutil_rest_t)); 41 | return rest; 42 | } 43 | 44 | void timeutil_rest_destroy(timeutil_rest_t *rest) 45 | { 46 | free(rest); 47 | } 48 | 49 | int64_t utime_now() // blacklist-ignore 50 | { 51 | struct timeval tv; 52 | gettimeofday (&tv, NULL); // blacklist-ignore 53 | return (int64_t) tv.tv_sec * 1000000 + tv.tv_usec; 54 | } 55 | 56 | int64_t utime_get_seconds(int64_t v) 57 | { 58 | return v/1000000; 59 | } 60 | 61 | int64_t utime_get_useconds(int64_t v) 62 | { 63 | return v%1000000; 64 | } 65 | 66 | void utime_to_timeval(int64_t v, struct timeval *tv) 67 | { 68 | tv->tv_sec = (time_t) utime_get_seconds(v); 69 | tv->tv_usec = (suseconds_t) utime_get_useconds(v); 70 | } 71 | 72 | void utime_to_timespec(int64_t v, struct timespec *ts) 73 | { 74 | ts->tv_sec = (time_t) utime_get_seconds(v); 75 | ts->tv_nsec = (suseconds_t) utime_get_useconds(v)*1000; 76 | } 77 | 78 | int32_t timeutil_usleep(int64_t useconds) 79 | { 80 | #ifdef _WIN32 81 | Sleep(useconds/1000); 82 | return 0; 83 | #else 84 | // unistd.h function, but usleep is obsoleted in POSIX.1-2008. 85 | // TODO: Eventually, rewrite this to use nanosleep 86 | return usleep(useconds); 87 | #endif 88 | } 89 | 90 | uint32_t timeutil_sleep(unsigned int seconds) 91 | { 92 | #ifdef _WIN32 93 | Sleep(seconds*1000); 94 | return 0; 95 | #else 96 | // unistd.h function 97 | return sleep(seconds); 98 | #endif 99 | } 100 | 101 | int32_t timeutil_sleep_hz(timeutil_rest_t *rest, double hz) 102 | { 103 | int64_t max_delay = 1000000L/hz; 104 | int64_t curr_time = utime_now(); 105 | int64_t diff = curr_time - rest->start_time; 106 | int64_t delay = max_delay - diff; 107 | if (delay < 0) delay = 0; 108 | 109 | int32_t ret = timeutil_usleep(delay); 110 | rest->start_time = utime_now(); 111 | 112 | return ret; 113 | } 114 | 115 | void timeutil_timer_reset(timeutil_rest_t *rest) 116 | { 117 | rest->start_time = utime_now(); 118 | rest->acc_time = 0; 119 | } 120 | 121 | void timeutil_timer_start(timeutil_rest_t *rest) 122 | { 123 | rest->start_time = utime_now(); 124 | } 125 | 126 | void timeutil_timer_stop(timeutil_rest_t *rest) 127 | { 128 | int64_t curr_time = utime_now(); 129 | int64_t diff = curr_time - rest->start_time; 130 | 131 | rest->acc_time += diff; 132 | } 133 | 134 | bool timeutil_timer_timeout(timeutil_rest_t *rest, double timeout_s) 135 | { 136 | int64_t timeout_us = (int64_t)(1000000L*timeout_s); 137 | return rest->acc_time > timeout_us; 138 | } 139 | 140 | int64_t time_util_hhmmss_ss_to_utime(double time) 141 | { 142 | int64_t utime = 0; 143 | 144 | int itime = ((int) time); 145 | 146 | double seconds = fmod(time, 100.0); 147 | uint8_t minutes = (itime % 10000) / 100; 148 | uint8_t hours = itime / 10000; 149 | 150 | utime += seconds * 100; 151 | utime += minutes * 6000; 152 | utime += hours *360000; 153 | 154 | utime *= 10000; 155 | 156 | return utime; 157 | } 158 | 159 | int64_t timeutil_ms_to_us(int32_t ms) 160 | { 161 | return ((int64_t) ms) * 1000; 162 | } 163 | -------------------------------------------------------------------------------- /common/time_util.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #ifdef _WIN32 35 | #include 36 | typedef long long suseconds_t; 37 | #endif 38 | 39 | #ifdef _MSC_VER 40 | 41 | inline int gettimeofday(struct timeval* tp, void* tzp) 42 | { 43 | (void)tzp; 44 | 45 | unsigned long t; 46 | t = time(NULL); 47 | tp->tv_sec = t / 1000; 48 | tp->tv_usec = t % 1000; 49 | return 0; 50 | } 51 | #else 52 | #include 53 | #include 54 | #endif 55 | 56 | #ifdef __cplusplus 57 | extern "C" { 58 | #endif 59 | 60 | typedef struct timeutil_rest timeutil_rest_t; 61 | timeutil_rest_t *timeutil_rest_create(); 62 | void timeutil_rest_destroy(timeutil_rest_t * rest); 63 | 64 | int64_t utime_now(); // blacklist-ignore 65 | int64_t utime_get_seconds(int64_t v); 66 | int64_t utime_get_useconds(int64_t v); 67 | void utime_to_timeval(int64_t v, struct timeval *tv); 68 | void utime_to_timespec(int64_t v, struct timespec *ts); 69 | 70 | int32_t timeutil_usleep(int64_t useconds); 71 | uint32_t timeutil_sleep(unsigned int seconds); 72 | int32_t timeutil_sleep_hz(timeutil_rest_t *rest, double hz); 73 | 74 | void timeutil_timer_reset(timeutil_rest_t *rest); 75 | void timeutil_timer_start(timeutil_rest_t *rest); 76 | void timeutil_timer_stop(timeutil_rest_t *rest); 77 | bool timeutil_timer_timeout(timeutil_rest_t *rest, double timeout_s); 78 | 79 | int64_t time_util_hhmmss_ss_to_utime(double time); 80 | 81 | int64_t timeutil_ms_to_us(int32_t ms); 82 | 83 | #ifdef __cplusplus 84 | } 85 | #endif 86 | -------------------------------------------------------------------------------- /common/timeprofile.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #pragma once 29 | 30 | #ifdef __cplusplus 31 | extern "C" { 32 | #endif 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | #include "time_util.h" 39 | #include "zarray.h" 40 | 41 | struct timeprofile_entry 42 | { 43 | char name[32]; 44 | int64_t utime; 45 | }; 46 | 47 | typedef struct timeprofile timeprofile_t; 48 | struct timeprofile 49 | { 50 | int64_t utime; 51 | zarray_t *stamps; 52 | }; 53 | 54 | static inline timeprofile_t *timeprofile_create() 55 | { 56 | timeprofile_t *tp = (timeprofile_t*) calloc(1, sizeof(timeprofile_t)); 57 | tp->stamps = zarray_create(sizeof(struct timeprofile_entry)); 58 | 59 | tp->utime = utime_now(); 60 | 61 | return tp; 62 | } 63 | 64 | static inline void timeprofile_destroy(timeprofile_t *tp) 65 | { 66 | zarray_destroy(tp->stamps); 67 | free(tp); 68 | } 69 | 70 | static inline void timeprofile_clear(timeprofile_t *tp) 71 | { 72 | zarray_clear(tp->stamps); 73 | tp->utime = utime_now(); 74 | } 75 | 76 | static inline void timeprofile_stamp(timeprofile_t *tp, const char *name) 77 | { 78 | struct timeprofile_entry tpe; 79 | 80 | strncpy(tpe.name, name, sizeof(tpe.name)); 81 | tpe.name[sizeof(tpe.name)-1] = 0; 82 | tpe.utime = utime_now(); 83 | 84 | zarray_add(tp->stamps, &tpe); 85 | } 86 | 87 | static inline void timeprofile_display(timeprofile_t *tp) 88 | { 89 | int64_t lastutime = tp->utime; 90 | 91 | for (int i = 0; i < zarray_size(tp->stamps); i++) { 92 | struct timeprofile_entry *stamp; 93 | 94 | zarray_get_volatile(tp->stamps, i, &stamp); 95 | 96 | double cumtime = (stamp->utime - tp->utime)/1000000.0; 97 | 98 | double parttime = (stamp->utime - lastutime)/1000000.0; 99 | 100 | printf("%2d %32s %15f ms %15f ms\n", i, stamp->name, parttime*1000, cumtime*1000); 101 | 102 | lastutime = stamp->utime; 103 | } 104 | } 105 | 106 | static inline uint64_t timeprofile_total_utime(timeprofile_t *tp) 107 | { 108 | if (zarray_size(tp->stamps) == 0) 109 | return 0; 110 | 111 | struct timeprofile_entry *first, *last; 112 | zarray_get_volatile(tp->stamps, 0, &first); 113 | zarray_get_volatile(tp->stamps, zarray_size(tp->stamps) - 1, &last); 114 | 115 | return last->utime - first->utime; 116 | } 117 | 118 | #ifdef __cplusplus 119 | } 120 | #endif 121 | -------------------------------------------------------------------------------- /common/unionfind.c: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #include "unionfind.h" 29 | #include 30 | #include 31 | -------------------------------------------------------------------------------- /common/unionfind.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | typedef struct unionfind unionfind_t; 35 | 36 | struct unionfind 37 | { 38 | uint32_t maxid; 39 | 40 | // Parent node for each. Initialized to 0xffffffff 41 | uint32_t *parent; 42 | 43 | // The size of the tree excluding the root 44 | uint32_t *size; 45 | }; 46 | 47 | static inline unionfind_t *unionfind_create(uint32_t maxid) 48 | { 49 | unionfind_t *uf = (unionfind_t*) calloc(1, sizeof(unionfind_t)); 50 | uf->maxid = maxid; 51 | uf->parent = (uint32_t *) malloc((maxid+1) * sizeof(uint32_t) * 2); 52 | memset(uf->parent, 0xff, (maxid+1) * sizeof(uint32_t)); 53 | uf->size = uf->parent + (maxid+1); 54 | memset(uf->size, 0, (maxid+1) * sizeof(uint32_t)); 55 | return uf; 56 | } 57 | 58 | static inline void unionfind_destroy(unionfind_t *uf) 59 | { 60 | free(uf->parent); 61 | free(uf); 62 | } 63 | 64 | /* 65 | static inline uint32_t unionfind_get_representative(unionfind_t *uf, uint32_t id) 66 | { 67 | // base case: a node is its own parent 68 | if (uf->parent[id] == id) 69 | return id; 70 | 71 | // otherwise, recurse 72 | uint32_t root = unionfind_get_representative(uf, uf->parent[id]); 73 | 74 | // short circuit the path. [XXX This write prevents tail recursion] 75 | uf->parent[id] = root; 76 | 77 | return root; 78 | } 79 | */ 80 | 81 | // this one seems to be every-so-slightly faster than the recursive 82 | // version above. 83 | static inline uint32_t unionfind_get_representative(unionfind_t *uf, uint32_t id) 84 | { 85 | uint32_t root = uf->parent[id]; 86 | // unititialized node, so set to self 87 | if (root == 0xffffffff) { 88 | uf->parent[id] = id; 89 | return id; 90 | } 91 | 92 | // chase down the root 93 | while (uf->parent[root] != root) { 94 | root = uf->parent[root]; 95 | } 96 | 97 | // go back and collapse the tree. 98 | while (uf->parent[id] != root) { 99 | uint32_t tmp = uf->parent[id]; 100 | uf->parent[id] = root; 101 | id = tmp; 102 | } 103 | 104 | return root; 105 | } 106 | 107 | static inline uint32_t unionfind_get_set_size(unionfind_t *uf, uint32_t id) 108 | { 109 | uint32_t repid = unionfind_get_representative(uf, id); 110 | return uf->size[repid] + 1; 111 | } 112 | 113 | static inline uint32_t unionfind_connect(unionfind_t *uf, uint32_t aid, uint32_t bid) 114 | { 115 | uint32_t aroot = unionfind_get_representative(uf, aid); 116 | uint32_t broot = unionfind_get_representative(uf, bid); 117 | 118 | if (aroot == broot) 119 | return aroot; 120 | 121 | // we don't perform "union by rank", but we perform a similar 122 | // operation (but probably without the same asymptotic guarantee): 123 | // We join trees based on the number of *elements* (as opposed to 124 | // rank) contained within each tree. I.e., we use size as a proxy 125 | // for rank. In my testing, it's often *faster* to use size than 126 | // rank, perhaps because the rank of the tree isn't that critical 127 | // if there are very few nodes in it. 128 | uint32_t asize = uf->size[aroot] + 1; 129 | uint32_t bsize = uf->size[broot] + 1; 130 | 131 | // optimization idea: We could shortcut some or all of the tree 132 | // that is grafted onto the other tree. Pro: those nodes were just 133 | // read and so are probably in cache. Con: it might end up being 134 | // wasted effort -- the tree might be grafted onto another tree in 135 | // a moment! 136 | if (asize > bsize) { 137 | uf->parent[broot] = aroot; 138 | uf->size[aroot] += bsize; 139 | return aroot; 140 | } else { 141 | uf->parent[aroot] = broot; 142 | uf->size[broot] += asize; 143 | return broot; 144 | } 145 | } 146 | -------------------------------------------------------------------------------- /common/workerpool.c: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | #include 28 | 29 | #define _GNU_SOURCE // Possible fix for 16.04 30 | #define __USE_GNU 31 | #include "common/pthreads_cross.h" 32 | #include 33 | #include 34 | #include 35 | #include 36 | #ifdef _WIN32 37 | #include 38 | #else 39 | #include 40 | #endif 41 | 42 | #include "workerpool.h" 43 | #include "debug_print.h" 44 | 45 | struct workerpool { 46 | int nthreads; 47 | zarray_t *tasks; 48 | int taskspos; 49 | 50 | pthread_t *threads; 51 | int *status; 52 | 53 | pthread_mutex_t mutex; 54 | pthread_cond_t startcond; // used to signal the availability of work 55 | bool start_predicate; // predicate that prevents spurious wakeups on startcond 56 | pthread_cond_t endcond; // used to signal completion of all work 57 | 58 | int end_count; // how many threads are done? 59 | }; 60 | 61 | struct task 62 | { 63 | void (*f)(void *p); 64 | void *p; 65 | }; 66 | 67 | void *worker_thread(void *p) 68 | { 69 | workerpool_t *wp = (workerpool_t*) p; 70 | 71 | while (1) { 72 | struct task *task; 73 | 74 | pthread_mutex_lock(&wp->mutex); 75 | while (wp->taskspos == zarray_size(wp->tasks) || !wp->start_predicate) { 76 | wp->end_count++; 77 | pthread_cond_broadcast(&wp->endcond); 78 | pthread_cond_wait(&wp->startcond, &wp->mutex); 79 | } 80 | 81 | zarray_get_volatile(wp->tasks, wp->taskspos, &task); 82 | wp->taskspos++; 83 | pthread_mutex_unlock(&wp->mutex); 84 | sched_yield(); 85 | 86 | // we've been asked to exit. 87 | if (task->f == NULL) 88 | return NULL; 89 | 90 | task->f(task->p); 91 | } 92 | 93 | return NULL; 94 | } 95 | 96 | workerpool_t *workerpool_create(int nthreads) 97 | { 98 | assert(nthreads > 0); 99 | 100 | workerpool_t *wp = calloc(1, sizeof(workerpool_t)); 101 | wp->nthreads = nthreads; 102 | wp->tasks = zarray_create(sizeof(struct task)); 103 | wp->start_predicate = false; 104 | 105 | if (nthreads > 1) { 106 | wp->threads = calloc(wp->nthreads, sizeof(pthread_t)); 107 | 108 | pthread_mutex_init(&wp->mutex, NULL); 109 | pthread_cond_init(&wp->startcond, NULL); 110 | pthread_cond_init(&wp->endcond, NULL); 111 | 112 | for (int i = 0; i < nthreads; i++) { 113 | int res = pthread_create(&wp->threads[i], NULL, worker_thread, wp); 114 | if (res != 0) { 115 | debug_print("Insufficient system resources to create workerpool threads\n"); 116 | // errno already set to EAGAIN by pthread_create() failure 117 | return NULL; 118 | } 119 | } 120 | 121 | // Wait for the worker threads to be ready 122 | pthread_mutex_lock(&wp->mutex); 123 | while (wp->end_count < wp->nthreads) { 124 | pthread_cond_wait(&wp->endcond, &wp->mutex); 125 | } 126 | pthread_mutex_unlock(&wp->mutex); 127 | } 128 | 129 | return wp; 130 | } 131 | 132 | void workerpool_destroy(workerpool_t *wp) 133 | { 134 | if (wp == NULL) 135 | return; 136 | 137 | // force all worker threads to exit. 138 | if (wp->nthreads > 1) { 139 | for (int i = 0; i < wp->nthreads; i++) 140 | workerpool_add_task(wp, NULL, NULL); 141 | 142 | pthread_mutex_lock(&wp->mutex); 143 | wp->start_predicate = true; 144 | pthread_cond_broadcast(&wp->startcond); 145 | pthread_mutex_unlock(&wp->mutex); 146 | 147 | for (int i = 0; i < wp->nthreads; i++) 148 | pthread_join(wp->threads[i], NULL); 149 | 150 | pthread_mutex_destroy(&wp->mutex); 151 | pthread_cond_destroy(&wp->startcond); 152 | pthread_cond_destroy(&wp->endcond); 153 | free(wp->threads); 154 | } 155 | 156 | zarray_destroy(wp->tasks); 157 | free(wp); 158 | } 159 | 160 | int workerpool_get_nthreads(workerpool_t *wp) 161 | { 162 | return wp->nthreads; 163 | } 164 | 165 | void workerpool_add_task(workerpool_t *wp, void (*f)(void *p), void *p) 166 | { 167 | struct task t; 168 | t.f = f; 169 | t.p = p; 170 | 171 | if (wp->nthreads > 1) { 172 | pthread_mutex_lock(&wp->mutex); 173 | zarray_add(wp->tasks, &t); 174 | pthread_mutex_unlock(&wp->mutex); 175 | } else { 176 | zarray_add(wp->tasks, &t); 177 | } 178 | } 179 | 180 | void workerpool_run_single(workerpool_t *wp) 181 | { 182 | for (int i = 0; i < zarray_size(wp->tasks); i++) { 183 | struct task *task; 184 | zarray_get_volatile(wp->tasks, i, &task); 185 | task->f(task->p); 186 | } 187 | 188 | zarray_clear(wp->tasks); 189 | } 190 | 191 | // runs all added tasks, waits for them to complete. 192 | void workerpool_run(workerpool_t *wp) 193 | { 194 | if (wp->nthreads > 1) { 195 | pthread_mutex_lock(&wp->mutex); 196 | wp->end_count = 0; 197 | wp->start_predicate = true; 198 | pthread_cond_broadcast(&wp->startcond); 199 | 200 | while (wp->end_count < wp->nthreads) { 201 | // printf("caught %d\n", wp->end_count); 202 | pthread_cond_wait(&wp->endcond, &wp->mutex); 203 | } 204 | 205 | wp->taskspos = 0; 206 | wp->start_predicate = false; 207 | pthread_mutex_unlock(&wp->mutex); 208 | 209 | zarray_clear(wp->tasks); 210 | 211 | } else { 212 | workerpool_run_single(wp); 213 | } 214 | } 215 | 216 | int workerpool_get_nprocs() 217 | { 218 | #ifdef _WIN32 219 | SYSTEM_INFO sysinfo; 220 | GetSystemInfo(&sysinfo); 221 | return sysinfo.dwNumberOfProcessors; 222 | #else 223 | return sysconf (_SC_NPROCESSORS_ONLN); 224 | #endif 225 | } 226 | -------------------------------------------------------------------------------- /common/workerpool.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #pragma once 29 | 30 | #include "zarray.h" 31 | 32 | typedef struct workerpool workerpool_t; 33 | 34 | // as a special case, if nthreads==1, no additional threads are 35 | // created, and workerpool_run will run synchronously. 36 | workerpool_t *workerpool_create(int nthreads); 37 | void workerpool_destroy(workerpool_t *wp); 38 | 39 | void workerpool_add_task(workerpool_t *wp, void (*f)(void *p), void *p); 40 | 41 | // runs all added tasks, waits for them to complete. 42 | void workerpool_run(workerpool_t *wp); 43 | 44 | // same as workerpool_run, except always single threaded. (mostly for debugging). 45 | void workerpool_run_single(workerpool_t *wp); 46 | 47 | int workerpool_get_nthreads(workerpool_t *wp); 48 | 49 | int workerpool_get_nprocs(); 50 | -------------------------------------------------------------------------------- /common/zarray.c: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #include 29 | #include 30 | 31 | #include "zarray.h" 32 | 33 | int zstrcmp(const void * a_pp, const void * b_pp) 34 | { 35 | assert(a_pp != NULL); 36 | assert(b_pp != NULL); 37 | 38 | char * a = *(void**)a_pp; 39 | char * b = *(void**)b_pp; 40 | 41 | return strcmp(a,b); 42 | } 43 | 44 | void zarray_vmap(zarray_t *za, void (*f)(void*)) 45 | { 46 | assert(za != NULL); 47 | assert(f != NULL); 48 | assert(za->el_sz == sizeof(void*)); 49 | 50 | for (int idx = 0; idx < za->size; idx++) { 51 | void *pp = &za->data[idx*za->el_sz]; 52 | void *p = *(void**) pp; 53 | f(p); 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /common/zmaxheap.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | typedef struct zmaxheap zmaxheap_t; 33 | 34 | typedef struct zmaxheap_iterator zmaxheap_iterator_t; 35 | struct zmaxheap_iterator { 36 | zmaxheap_t *heap; 37 | int in, out; 38 | }; 39 | 40 | zmaxheap_t *zmaxheap_create(size_t el_sz); 41 | 42 | void zmaxheap_vmap(zmaxheap_t *heap, void (*f)(void *)); 43 | 44 | void zmaxheap_destroy(zmaxheap_t *heap); 45 | 46 | void zmaxheap_add(zmaxheap_t *heap, void *p, float v); 47 | 48 | int zmaxheap_size(zmaxheap_t *heap); 49 | 50 | // returns 0 if the heap is empty, so you can do 51 | // while (zmaxheap_remove_max(...)) { } 52 | int zmaxheap_remove_max(zmaxheap_t *heap, void *p, float *v); 53 | 54 | //////////////////////////////////////////// 55 | // This is a peculiar iterator intended to support very specific (and 56 | // unusual) applications, and the heap is not necessarily in a valid 57 | // state until zmaxheap_iterator_finish is called. Consequently, do 58 | // not call any other methods on the heap while iterating through. 59 | 60 | // you must provide your own storage for the iterator, and pass in a 61 | // pointer. 62 | void zmaxheap_iterator_init(zmaxheap_t *heap, zmaxheap_iterator_t *it); 63 | 64 | // Traverses the heap in top-down/left-right order. makes a copy of 65 | // the content into memory (p) that you provide. 66 | int zmaxheap_iterator_next(zmaxheap_iterator_t *it, void *p, float *v); 67 | 68 | // will set p to be a pointer to the heap's internal copy of the dfata. 69 | int zmaxheap_iterator_next_volatile(zmaxheap_iterator_t *it, void *p, float *v); 70 | 71 | // remove the current element. 72 | void zmaxheap_iterator_remove(zmaxheap_iterator_t *it); 73 | 74 | // call after all iterator operations are done. After calling this, 75 | // the iterator should no longer be used, but the heap methods can be. 76 | void zmaxheap_iterator_finish(zmaxheap_iterator_t *it); 77 | -------------------------------------------------------------------------------- /example/opencv_demo.cc: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #include 29 | #include 30 | 31 | #include "opencv2/opencv.hpp" 32 | 33 | extern "C" { 34 | #include "apriltag.h" 35 | #include "tag36h11.h" 36 | #include "tag25h9.h" 37 | #include "tag16h5.h" 38 | #include "tagCircle21h7.h" 39 | #include "tagCircle49h12.h" 40 | #include "tagCustom48h12.h" 41 | #include "tagStandard41h12.h" 42 | #include "tagStandard52h13.h" 43 | #include "common/getopt.h" 44 | } 45 | 46 | using namespace std; 47 | using namespace cv; 48 | 49 | 50 | int main(int argc, char *argv[]) 51 | { 52 | getopt_t *getopt = getopt_create(); 53 | 54 | getopt_add_bool(getopt, 'h', "help", 0, "Show this help"); 55 | getopt_add_int(getopt, 'c', "camera", "0", "camera ID"); 56 | getopt_add_bool(getopt, 'd', "debug", 0, "Enable debugging output (slow)"); 57 | getopt_add_bool(getopt, 'q', "quiet", 0, "Reduce output"); 58 | getopt_add_string(getopt, 'f', "family", "tag36h11", "Tag family to use"); 59 | getopt_add_int(getopt, 't', "threads", "1", "Use this many CPU threads"); 60 | getopt_add_double(getopt, 'x', "decimate", "2.0", "Decimate input image by this factor"); 61 | getopt_add_double(getopt, 'b', "blur", "0.0", "Apply low-pass blur to input"); 62 | getopt_add_bool(getopt, '0', "refine-edges", 1, "Spend more time trying to align edges of tags"); 63 | 64 | if (!getopt_parse(getopt, argc, argv, 1) || 65 | getopt_get_bool(getopt, "help")) { 66 | printf("Usage: %s [options]\n", argv[0]); 67 | getopt_do_usage(getopt); 68 | exit(0); 69 | } 70 | 71 | cout << "Enabling video capture" << endl; 72 | 73 | TickMeter meter; 74 | meter.start(); 75 | 76 | // Initialize camera 77 | VideoCapture cap(getopt_get_int(getopt, "camera")); 78 | if (!cap.isOpened()) { 79 | cerr << "Couldn't open video capture device" << endl; 80 | return -1; 81 | } 82 | 83 | // Initialize tag detector with options 84 | apriltag_family_t *tf = NULL; 85 | const char *famname = getopt_get_string(getopt, "family"); 86 | if (!strcmp(famname, "tag36h11")) { 87 | tf = tag36h11_create(); 88 | } else if (!strcmp(famname, "tag25h9")) { 89 | tf = tag25h9_create(); 90 | } else if (!strcmp(famname, "tag16h5")) { 91 | tf = tag16h5_create(); 92 | } else if (!strcmp(famname, "tagCircle21h7")) { 93 | tf = tagCircle21h7_create(); 94 | } else if (!strcmp(famname, "tagCircle49h12")) { 95 | tf = tagCircle49h12_create(); 96 | } else if (!strcmp(famname, "tagStandard41h12")) { 97 | tf = tagStandard41h12_create(); 98 | } else if (!strcmp(famname, "tagStandard52h13")) { 99 | tf = tagStandard52h13_create(); 100 | } else if (!strcmp(famname, "tagCustom48h12")) { 101 | tf = tagCustom48h12_create(); 102 | } else { 103 | printf("Unrecognized tag family name. Use e.g. \"tag36h11\".\n"); 104 | exit(-1); 105 | } 106 | 107 | 108 | apriltag_detector_t *td = apriltag_detector_create(); 109 | apriltag_detector_add_family(td, tf); 110 | 111 | if (errno == ENOMEM) { 112 | printf("Unable to add family to detector due to insufficient memory to allocate the tag-family decoder with the default maximum hamming value of 2. Try choosing an alternative tag family.\n"); 113 | exit(-1); 114 | } 115 | 116 | td->quad_decimate = getopt_get_double(getopt, "decimate"); 117 | td->quad_sigma = getopt_get_double(getopt, "blur"); 118 | td->nthreads = getopt_get_int(getopt, "threads"); 119 | td->debug = getopt_get_bool(getopt, "debug"); 120 | td->refine_edges = getopt_get_bool(getopt, "refine-edges"); 121 | 122 | meter.stop(); 123 | cout << "Detector " << famname << " initialized in " 124 | << std::fixed << std::setprecision(3) << meter.getTimeSec() << " seconds" << endl; 125 | #if CV_MAJOR_VERSION > 3 126 | cout << " " << cap.get(CAP_PROP_FRAME_WIDTH ) << "x" << 127 | cap.get(CAP_PROP_FRAME_HEIGHT ) << " @" << 128 | cap.get(CAP_PROP_FPS) << "FPS" << endl; 129 | #else 130 | cout << " " << cap.get(CV_CAP_PROP_FRAME_WIDTH ) << "x" << 131 | cap.get(CV_CAP_PROP_FRAME_HEIGHT ) << " @" << 132 | cap.get(CV_CAP_PROP_FPS) << "FPS" << endl; 133 | #endif 134 | meter.reset(); 135 | 136 | Mat frame, gray; 137 | while (true) { 138 | errno = 0; 139 | cap >> frame; 140 | cvtColor(frame, gray, COLOR_BGR2GRAY); 141 | 142 | // Make an image_u8_t header for the Mat data 143 | image_u8_t im = {gray.cols, gray.rows, gray.cols, gray.data}; 144 | 145 | zarray_t *detections = apriltag_detector_detect(td, &im); 146 | 147 | if (errno == EAGAIN) { 148 | printf("Unable to create the %d threads requested.\n",td->nthreads); 149 | exit(-1); 150 | } 151 | 152 | // Draw detection outlines 153 | for (int i = 0; i < zarray_size(detections); i++) { 154 | apriltag_detection_t *det; 155 | zarray_get(detections, i, &det); 156 | line(frame, Point(det->p[0][0], det->p[0][1]), 157 | Point(det->p[1][0], det->p[1][1]), 158 | Scalar(0, 0xff, 0), 2); 159 | line(frame, Point(det->p[0][0], det->p[0][1]), 160 | Point(det->p[3][0], det->p[3][1]), 161 | Scalar(0, 0, 0xff), 2); 162 | line(frame, Point(det->p[1][0], det->p[1][1]), 163 | Point(det->p[2][0], det->p[2][1]), 164 | Scalar(0xff, 0, 0), 2); 165 | line(frame, Point(det->p[2][0], det->p[2][1]), 166 | Point(det->p[3][0], det->p[3][1]), 167 | Scalar(0xff, 0, 0), 2); 168 | 169 | stringstream ss; 170 | ss << det->id; 171 | String text = ss.str(); 172 | int fontface = FONT_HERSHEY_SCRIPT_SIMPLEX; 173 | double fontscale = 1.0; 174 | int baseline; 175 | Size textsize = getTextSize(text, fontface, fontscale, 2, 176 | &baseline); 177 | putText(frame, text, Point(det->c[0]-textsize.width/2, 178 | det->c[1]+textsize.height/2), 179 | fontface, fontscale, Scalar(0xff, 0x99, 0), 2); 180 | } 181 | apriltag_detections_destroy(detections); 182 | 183 | imshow("Tag Detections", frame); 184 | if (waitKey(30) >= 0) 185 | break; 186 | } 187 | 188 | apriltag_detector_destroy(td); 189 | 190 | if (!strcmp(famname, "tag36h11")) { 191 | tag36h11_destroy(tf); 192 | } else if (!strcmp(famname, "tag25h9")) { 193 | tag25h9_destroy(tf); 194 | } else if (!strcmp(famname, "tag16h5")) { 195 | tag16h5_destroy(tf); 196 | } else if (!strcmp(famname, "tagCircle21h7")) { 197 | tagCircle21h7_destroy(tf); 198 | } else if (!strcmp(famname, "tagCircle49h12")) { 199 | tagCircle49h12_destroy(tf); 200 | } else if (!strcmp(famname, "tagStandard41h12")) { 201 | tagStandard41h12_destroy(tf); 202 | } else if (!strcmp(famname, "tagStandard52h13")) { 203 | tagStandard52h13_destroy(tf); 204 | } else if (!strcmp(famname, "tagCustom48h12")) { 205 | tagCustom48h12_destroy(tf); 206 | } 207 | 208 | 209 | getopt_destroy(getopt); 210 | 211 | return 0; 212 | } 213 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | apriltag 5 | 3.4.3 6 | AprilTag detector library 7 | 8 | Max Krogius 9 | Wolfgang Merkt 10 | Christian Rauch 11 | 12 | BSD 13 | https://april.eecs.umich.edu/software/apriltag.html 14 | 15 | Edwin Olson 16 | Max Krogius 17 | 18 | cmake 19 | python3-dev 20 | python3-numpy 21 | 22 | 23 | libopencv-dev 24 | 25 | 26 | cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /tag16h5.c: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #include 29 | #include "tag16h5.h" 30 | 31 | static uint64_t codedata[30] = { 32 | 0x00000000000027c8UL, 33 | 0x00000000000031b6UL, 34 | 0x0000000000003859UL, 35 | 0x000000000000569cUL, 36 | 0x0000000000006c76UL, 37 | 0x0000000000007ddbUL, 38 | 0x000000000000af09UL, 39 | 0x000000000000f5a1UL, 40 | 0x000000000000fb8bUL, 41 | 0x0000000000001cb9UL, 42 | 0x00000000000028caUL, 43 | 0x000000000000e8dcUL, 44 | 0x0000000000001426UL, 45 | 0x0000000000005770UL, 46 | 0x0000000000009253UL, 47 | 0x000000000000b702UL, 48 | 0x000000000000063aUL, 49 | 0x0000000000008f34UL, 50 | 0x000000000000b4c0UL, 51 | 0x00000000000051ecUL, 52 | 0x000000000000e6f0UL, 53 | 0x0000000000005fa4UL, 54 | 0x000000000000dd43UL, 55 | 0x0000000000001aaaUL, 56 | 0x000000000000e62fUL, 57 | 0x0000000000006dbcUL, 58 | 0x000000000000b6ebUL, 59 | 0x000000000000de10UL, 60 | 0x000000000000154dUL, 61 | 0x000000000000b57aUL, 62 | }; 63 | apriltag_family_t *tag16h5_create() 64 | { 65 | apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t)); 66 | tf->name = strdup("tag16h5"); 67 | tf->h = 5; 68 | tf->ncodes = 30; 69 | tf->codes = codedata; 70 | tf->nbits = 16; 71 | tf->bit_x = calloc(16, sizeof(uint32_t)); 72 | tf->bit_y = calloc(16, sizeof(uint32_t)); 73 | tf->bit_x[0] = 1; 74 | tf->bit_y[0] = 1; 75 | tf->bit_x[1] = 2; 76 | tf->bit_y[1] = 1; 77 | tf->bit_x[2] = 3; 78 | tf->bit_y[2] = 1; 79 | tf->bit_x[3] = 2; 80 | tf->bit_y[3] = 2; 81 | tf->bit_x[4] = 4; 82 | tf->bit_y[4] = 1; 83 | tf->bit_x[5] = 4; 84 | tf->bit_y[5] = 2; 85 | tf->bit_x[6] = 4; 86 | tf->bit_y[6] = 3; 87 | tf->bit_x[7] = 3; 88 | tf->bit_y[7] = 2; 89 | tf->bit_x[8] = 4; 90 | tf->bit_y[8] = 4; 91 | tf->bit_x[9] = 3; 92 | tf->bit_y[9] = 4; 93 | tf->bit_x[10] = 2; 94 | tf->bit_y[10] = 4; 95 | tf->bit_x[11] = 3; 96 | tf->bit_y[11] = 3; 97 | tf->bit_x[12] = 1; 98 | tf->bit_y[12] = 4; 99 | tf->bit_x[13] = 1; 100 | tf->bit_y[13] = 3; 101 | tf->bit_x[14] = 1; 102 | tf->bit_y[14] = 2; 103 | tf->bit_x[15] = 2; 104 | tf->bit_y[15] = 3; 105 | tf->width_at_border = 6; 106 | tf->total_width = 8; 107 | tf->reversed_border = false; 108 | return tf; 109 | } 110 | 111 | void tag16h5_destroy(apriltag_family_t *tf) 112 | { 113 | free(tf->bit_x); 114 | free(tf->bit_y); 115 | free(tf->name); 116 | free(tf); 117 | } 118 | -------------------------------------------------------------------------------- /tag16h5.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #ifndef _TAG16H5 29 | #define _TAG16H5 30 | 31 | #include "apriltag.h" 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | apriltag_family_t *tag16h5_create(); 38 | void tag16h5_destroy(apriltag_family_t *tf); 39 | 40 | #ifdef __cplusplus 41 | } 42 | #endif 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /tag25h9.c: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #include 29 | #include "tag25h9.h" 30 | 31 | static uint64_t codedata[35] = { 32 | 0x000000000156f1f4UL, 33 | 0x0000000001f28cd5UL, 34 | 0x00000000016ce32cUL, 35 | 0x0000000001ea379cUL, 36 | 0x0000000001390f89UL, 37 | 0x000000000034fad0UL, 38 | 0x00000000007dcdb5UL, 39 | 0x000000000119ba95UL, 40 | 0x0000000001ae9daaUL, 41 | 0x0000000000df02aaUL, 42 | 0x000000000082fc15UL, 43 | 0x0000000000465123UL, 44 | 0x0000000000ceee98UL, 45 | 0x0000000001f17260UL, 46 | 0x00000000014429cdUL, 47 | 0x00000000017248a8UL, 48 | 0x00000000016ad452UL, 49 | 0x00000000009670adUL, 50 | 0x00000000016f65b2UL, 51 | 0x0000000000b8322bUL, 52 | 0x00000000005d715bUL, 53 | 0x0000000001a1c7e7UL, 54 | 0x0000000000d7890dUL, 55 | 0x0000000001813522UL, 56 | 0x0000000001c9c611UL, 57 | 0x000000000099e4a4UL, 58 | 0x0000000000855234UL, 59 | 0x00000000017b81c0UL, 60 | 0x0000000000c294bbUL, 61 | 0x000000000089fae3UL, 62 | 0x000000000044df5fUL, 63 | 0x0000000001360159UL, 64 | 0x0000000000ec31e8UL, 65 | 0x0000000001bcc0f6UL, 66 | 0x0000000000a64f8dUL, 67 | }; 68 | apriltag_family_t *tag25h9_create() 69 | { 70 | apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t)); 71 | tf->name = strdup("tag25h9"); 72 | tf->h = 9; 73 | tf->ncodes = 35; 74 | tf->codes = codedata; 75 | tf->nbits = 25; 76 | tf->bit_x = calloc(25, sizeof(uint32_t)); 77 | tf->bit_y = calloc(25, sizeof(uint32_t)); 78 | tf->bit_x[0] = 1; 79 | tf->bit_y[0] = 1; 80 | tf->bit_x[1] = 2; 81 | tf->bit_y[1] = 1; 82 | tf->bit_x[2] = 3; 83 | tf->bit_y[2] = 1; 84 | tf->bit_x[3] = 4; 85 | tf->bit_y[3] = 1; 86 | tf->bit_x[4] = 2; 87 | tf->bit_y[4] = 2; 88 | tf->bit_x[5] = 3; 89 | tf->bit_y[5] = 2; 90 | tf->bit_x[6] = 5; 91 | tf->bit_y[6] = 1; 92 | tf->bit_x[7] = 5; 93 | tf->bit_y[7] = 2; 94 | tf->bit_x[8] = 5; 95 | tf->bit_y[8] = 3; 96 | tf->bit_x[9] = 5; 97 | tf->bit_y[9] = 4; 98 | tf->bit_x[10] = 4; 99 | tf->bit_y[10] = 2; 100 | tf->bit_x[11] = 4; 101 | tf->bit_y[11] = 3; 102 | tf->bit_x[12] = 5; 103 | tf->bit_y[12] = 5; 104 | tf->bit_x[13] = 4; 105 | tf->bit_y[13] = 5; 106 | tf->bit_x[14] = 3; 107 | tf->bit_y[14] = 5; 108 | tf->bit_x[15] = 2; 109 | tf->bit_y[15] = 5; 110 | tf->bit_x[16] = 4; 111 | tf->bit_y[16] = 4; 112 | tf->bit_x[17] = 3; 113 | tf->bit_y[17] = 4; 114 | tf->bit_x[18] = 1; 115 | tf->bit_y[18] = 5; 116 | tf->bit_x[19] = 1; 117 | tf->bit_y[19] = 4; 118 | tf->bit_x[20] = 1; 119 | tf->bit_y[20] = 3; 120 | tf->bit_x[21] = 1; 121 | tf->bit_y[21] = 2; 122 | tf->bit_x[22] = 2; 123 | tf->bit_y[22] = 4; 124 | tf->bit_x[23] = 2; 125 | tf->bit_y[23] = 3; 126 | tf->bit_x[24] = 3; 127 | tf->bit_y[24] = 3; 128 | tf->width_at_border = 7; 129 | tf->total_width = 9; 130 | tf->reversed_border = false; 131 | return tf; 132 | } 133 | 134 | void tag25h9_destroy(apriltag_family_t *tf) 135 | { 136 | free(tf->bit_x); 137 | free(tf->bit_y); 138 | free(tf->name); 139 | free(tf); 140 | } 141 | -------------------------------------------------------------------------------- /tag25h9.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #ifndef _TAG25H9 29 | #define _TAG25H9 30 | 31 | #include "apriltag.h" 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | apriltag_family_t *tag25h9_create(); 38 | void tag25h9_destroy(apriltag_family_t *tf); 39 | 40 | #ifdef __cplusplus 41 | } 42 | #endif 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /tag36h10.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #ifndef _TAG36H10 29 | #define _TAG36H10 30 | 31 | #include "apriltag.h" 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | apriltag_family_t *tag36h10_create(); 38 | void tag36h10_destroy(apriltag_family_t *tf); 39 | 40 | #ifdef __cplusplus 41 | } 42 | #endif 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /tag36h11.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #ifndef _TAG36H11 29 | #define _TAG36H11 30 | 31 | #include "apriltag.h" 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | apriltag_family_t *tag36h11_create(); 38 | void tag36h11_destroy(apriltag_family_t *tf); 39 | 40 | #ifdef __cplusplus 41 | } 42 | #endif 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /tagCircle21h7.c: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #include 29 | #include "tagCircle21h7.h" 30 | 31 | static uint64_t codedata[38] = { 32 | 0x0000000000157863UL, 33 | 0x0000000000047e28UL, 34 | 0x00000000001383edUL, 35 | 0x000000000000953cUL, 36 | 0x00000000000da68bUL, 37 | 0x00000000001cac50UL, 38 | 0x00000000000bb215UL, 39 | 0x000000000016ceeeUL, 40 | 0x000000000005d4b3UL, 41 | 0x00000000001ff751UL, 42 | 0x00000000000efd16UL, 43 | 0x0000000000072b3eUL, 44 | 0x0000000000163103UL, 45 | 0x0000000000106e56UL, 46 | 0x00000000001996b9UL, 47 | 0x00000000000c0234UL, 48 | 0x00000000000624d2UL, 49 | 0x00000000001fa985UL, 50 | 0x00000000000344a5UL, 51 | 0x00000000000762fbUL, 52 | 0x000000000019e92bUL, 53 | 0x0000000000043755UL, 54 | 0x000000000001a4f4UL, 55 | 0x000000000010fad8UL, 56 | 0x0000000000001b52UL, 57 | 0x000000000017e59fUL, 58 | 0x00000000000e6f70UL, 59 | 0x00000000000ed47aUL, 60 | 0x00000000000c9931UL, 61 | 0x0000000000014df2UL, 62 | 0x00000000000a06f1UL, 63 | 0x00000000000e5041UL, 64 | 0x000000000012ec03UL, 65 | 0x000000000016724eUL, 66 | 0x00000000000af1a5UL, 67 | 0x000000000008a8acUL, 68 | 0x0000000000015b39UL, 69 | 0x00000000001ec1e3UL, 70 | }; 71 | apriltag_family_t *tagCircle21h7_create() 72 | { 73 | apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t)); 74 | tf->name = strdup("tagCircle21h7"); 75 | tf->h = 7; 76 | tf->ncodes = 38; 77 | tf->codes = codedata; 78 | tf->nbits = 21; 79 | tf->bit_x = calloc(21, sizeof(uint32_t)); 80 | tf->bit_y = calloc(21, sizeof(uint32_t)); 81 | tf->bit_x[0] = 1; 82 | tf->bit_y[0] = -2; 83 | tf->bit_x[1] = 2; 84 | tf->bit_y[1] = -2; 85 | tf->bit_x[2] = 3; 86 | tf->bit_y[2] = -2; 87 | tf->bit_x[3] = 1; 88 | tf->bit_y[3] = 1; 89 | tf->bit_x[4] = 2; 90 | tf->bit_y[4] = 1; 91 | tf->bit_x[5] = 6; 92 | tf->bit_y[5] = 1; 93 | tf->bit_x[6] = 6; 94 | tf->bit_y[6] = 2; 95 | tf->bit_x[7] = 6; 96 | tf->bit_y[7] = 3; 97 | tf->bit_x[8] = 3; 98 | tf->bit_y[8] = 1; 99 | tf->bit_x[9] = 3; 100 | tf->bit_y[9] = 2; 101 | tf->bit_x[10] = 3; 102 | tf->bit_y[10] = 6; 103 | tf->bit_x[11] = 2; 104 | tf->bit_y[11] = 6; 105 | tf->bit_x[12] = 1; 106 | tf->bit_y[12] = 6; 107 | tf->bit_x[13] = 3; 108 | tf->bit_y[13] = 3; 109 | tf->bit_x[14] = 2; 110 | tf->bit_y[14] = 3; 111 | tf->bit_x[15] = -2; 112 | tf->bit_y[15] = 3; 113 | tf->bit_x[16] = -2; 114 | tf->bit_y[16] = 2; 115 | tf->bit_x[17] = -2; 116 | tf->bit_y[17] = 1; 117 | tf->bit_x[18] = 1; 118 | tf->bit_y[18] = 3; 119 | tf->bit_x[19] = 1; 120 | tf->bit_y[19] = 2; 121 | tf->bit_x[20] = 2; 122 | tf->bit_y[20] = 2; 123 | tf->width_at_border = 5; 124 | tf->total_width = 9; 125 | tf->reversed_border = true; 126 | return tf; 127 | } 128 | 129 | void tagCircle21h7_destroy(apriltag_family_t *tf) 130 | { 131 | free(tf->bit_x); 132 | free(tf->bit_y); 133 | free(tf->name); 134 | free(tf); 135 | } 136 | -------------------------------------------------------------------------------- /tagCircle21h7.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #ifndef _TAGCircle21H7 29 | #define _TAGCircle21H7 30 | 31 | #include "apriltag.h" 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | apriltag_family_t *tagCircle21h7_create(); 38 | void tagCircle21h7_destroy(apriltag_family_t *tf); 39 | 40 | #ifdef __cplusplus 41 | } 42 | #endif 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /tagCircle49h12.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #ifndef _TAGCircle49H12 29 | #define _TAGCircle49H12 30 | 31 | #include "apriltag.h" 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | apriltag_family_t *tagCircle49h12_create(); 38 | void tagCircle49h12_destroy(apriltag_family_t *tf); 39 | 40 | #ifdef __cplusplus 41 | } 42 | #endif 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /tagCustom48h12.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #ifndef _TAGCustom48H12 29 | #define _TAGCustom48H12 30 | 31 | #include "apriltag.h" 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | apriltag_family_t *tagCustom48h12_create(); 38 | void tagCustom48h12_destroy(apriltag_family_t *tf); 39 | 40 | #ifdef __cplusplus 41 | } 42 | #endif 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /tagStandard41h12.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #ifndef _TAGStandard41H12 29 | #define _TAGStandard41H12 30 | 31 | #include "apriltag.h" 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | apriltag_family_t *tagStandard41h12_create(); 38 | void tagStandard41h12_destroy(apriltag_family_t *tf); 39 | 40 | #ifdef __cplusplus 41 | } 42 | #endif 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /tagStandard52h13.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2013-2016, The Regents of The University of Michigan. 2 | All rights reserved. 3 | This software was developed in the APRIL Robotics Lab under the 4 | direction of Edwin Olson, ebolson@umich.edu. This software may be 5 | available under alternative licensing terms; contact the address above. 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | The views and conclusions contained in the software and documentation are those 24 | of the authors and should not be interpreted as representing official policies, 25 | either expressed or implied, of the Regents of The University of Michigan. 26 | */ 27 | 28 | #ifndef _TAGStandard52H13 29 | #define _TAGStandard52H13 30 | 31 | #include "apriltag.h" 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | apriltag_family_t *tagStandard52h13_create(); 38 | void tagStandard52h13_destroy(apriltag_family_t *tf); 39 | 40 | #ifdef __cplusplus 41 | } 42 | #endif 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /tag_size_48h12.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AprilRobotics/apriltag/f11c949c75217f824dd3240ea68a635bb97f0090/tag_size_48h12.png -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(getline OBJECT getline.c) 2 | 3 | add_executable(test_detection test_detection.c) 4 | target_link_libraries(test_detection ${PROJECT_NAME} getline) 5 | 6 | # test images with true detection 7 | set(TEST_IMAGE_NAMES 8 | "33369213973_9d9bb4cc96_c" 9 | "34085369442_304b6bafd9_c" 10 | "34139872896_defdb2f8d9_c" 11 | ) 12 | 13 | foreach(IMG IN LISTS TEST_IMAGE_NAMES) 14 | add_test(NAME test_detection_${IMG} 15 | COMMAND $ data/${IMG} 16 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} 17 | ) 18 | endforeach() 19 | -------------------------------------------------------------------------------- /test/data/33369213973_9d9bb4cc96_c.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AprilRobotics/apriltag/f11c949c75217f824dd3240ea68a635bb97f0090/test/data/33369213973_9d9bb4cc96_c.jpg -------------------------------------------------------------------------------- /test/data/33369213973_9d9bb4cc96_c.txt: -------------------------------------------------------------------------------- 1 | 0, (277.9247 327.4884), (250.3570 329.7265), (251.6721 357.3356), (279.4720 354.6839) 2 | 0, (352.8467 346.8030), (328.9969 344.6012), (330.2313 372.6704), (354.3267 375.5784) 3 | 0, (414.9603 338.8232), (403.0023 333.3730), (404.3122 361.6505), (414.9944 366.4854) 4 | 0, (449.0000 335.4156), (421.7478 338.2029), (422.8918 366.6338), (449.0000 363.4273) 5 | 0, (451.8521 324.5121), (452.9211 352.3356), (462.6942 357.3213), (461.5424 329.8951) 6 | 0, (465.7551 341.7513), (466.5209 370.4993), (480.3520 375.3039), (478.8691 346.5728) 7 | 0, (512.0000 371.4803), (512.0000 342.4403), (485.9187 345.2694), (486.2385 374.6548) 8 | 0, (535.6610 364.6997), (524.0469 359.3780), (523.9808 387.9674), (537.1108 394.5279) 9 | 0, (571.5667 361.6513), (543.9484 364.7314), (544.2009 394.8912), (572.1226 391.7314) 10 | 0, (620.9534 374.3775), (621.1776 405.3268), (652.4188 403.1609), (650.3995 373.0815) 11 | 0, (639.7830 425.7310), (641.0399 458.6032), (669.9742 462.4909), (670.0141 429.6474) 12 | 0, (760.8949 429.0904), (726.9314 428.9567), (727.1409 461.2701), (761.0534 462.3109) 13 | -------------------------------------------------------------------------------- /test/data/34085369442_304b6bafd9_c.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AprilRobotics/apriltag/f11c949c75217f824dd3240ea68a635bb97f0090/test/data/34085369442_304b6bafd9_c.jpg -------------------------------------------------------------------------------- /test/data/34085369442_304b6bafd9_c.txt: -------------------------------------------------------------------------------- 1 | 0, (7.1998 367.7382), (8.2826 387.0433), (21.8467 388.8047), (22.3012 369.3906) 2 | 0, (40.0000 385.2895), (40.0000 366.5399), (26.9912 368.8071), (27.7584 388.3069) 3 | 0, (50.2235 357.2749), (62.8278 355.8416), (64.3150 337.5952), (49.2334 339.3408) 4 | 0, (56.2142 395.9775), (75.7085 395.7057), (76.4279 376.0304), (55.7100 376.1390) 5 | 0, (136.5859 405.9852), (136.6868 386.9614), (117.9318 385.6759), (119.3466 405.7574) 6 | 0, (136.8157 349.8532), (137.4467 331.6211), (119.2479 332.8495), (120.3091 350.9236) 7 | 0, (214.1390 345.1158), (231.0276 343.9631), (230.9505 326.2049), (213.9404 327.2949) 8 | 0, (216.2888 412.8570), (234.8337 413.0699), (236.6666 393.5693), (215.5450 393.1030) 9 | 0, (311.3437 343.7850), (328.7296 343.6689), (328.3899 326.1203), (310.4015 325.7319) 10 | 0, (319.0783 390.0601), (300.7925 391.4020), (300.1619 411.2209), (318.8514 409.6298) 11 | 0, (357.8598 346.5026), (372.8396 347.3288), (374.1022 329.4759), (356.7228 328.0721) 12 | 0, (400.0000 282.9315), (400.0000 300.0000), (417.1274 300.0000), (416.7581 283.1546) 13 | 0, (416.5377 352.6791), (430.8029 354.3849), (431.4457 336.2527), (415.8975 334.5414) 14 | 0, (437.0000 353.3342), (446.9166 350.9822), (446.7385 333.0705), (437.0000 335.3143) 15 | 0, (466.8379 288.1093), (467.5887 271.7813), (456.9818 269.5371), (457.0370 285.9229) 16 | 0, (472.0643 272.4668), (472.1009 288.5338), (484.8149 287.5498), (485.0922 270.7832) 17 | 0, (492.0000 247.0000), (492.0000 263.0000), (507.0938 263.0000), (507.0838 247.0000) 18 | 0, (542.8355 351.0359), (543.3651 333.1548), (525.1558 332.4807), (525.6393 350.9261) 19 | 0, (615.1451 276.3273), (614.9308 293.1076), (629.6509 292.7443), (630.5632 275.8616) 20 | 0, (630.5645 330.1998), (630.6873 348.0871), (646.7246 347.8124), (647.3389 329.9061) 21 | 0, (678.0500 302.4676), (677.9111 285.4095), (666.0000 283.6531), (666.0000 300.7083) 22 | 0, (683.0000 303.2291), (694.9082 301.7316), (695.1923 285.0323), (683.0000 286.0783) 23 | 0, (696.0000 107.8969), (696.0000 121.2508), (707.5752 120.6790), (708.6246 108.2199) 24 | 0, (712.3666 321.4679), (695.3271 319.9513), (695.3467 337.6513), (711.9149 339.4104) 25 | 0, (734.1375 326.0000), (750.7946 326.0000), (751.4586 308.0991), (733.6561 308.1078) 26 | -------------------------------------------------------------------------------- /test/data/34139872896_defdb2f8d9_c.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AprilRobotics/apriltag/f11c949c75217f824dd3240ea68a635bb97f0090/test/data/34139872896_defdb2f8d9_c.jpg -------------------------------------------------------------------------------- /test/data/34139872896_defdb2f8d9_c.txt: -------------------------------------------------------------------------------- 1 | 0, (329.0657 399.3667), (285.7438 402.8478), (286.5317 446.8641), (330.4509 443.1603) 2 | 0, (422.3676 450.0573), (421.1896 405.5042), (376.7516 407.5195), (378.1724 452.5206) 3 | 0, (449.6035 294.2907), (408.7995 289.9406), (394.2166 316.1437), (435.2841 319.5461) 4 | 0, (450.7480 281.4591), (444.8973 246.3509), (404.0564 242.6216), (409.2780 277.5639) 5 | 0, (585.6249 383.3414), (587.1271 427.5599), (608.1880 435.4088), (606.5087 391.2607) 6 | 0, (658.3193 429.8612), (656.9769 385.1598), (616.7130 389.3761), (618.0273 434.5546) 7 | 0, (695.4741 419.9608), (676.5197 411.5248), (677.6697 456.8612), (697.3136 465.9787) 8 | 0, (708.7199 356.2537), (723.1980 347.7975), (681.8147 345.8170), (666.7383 354.4589) 9 | 0, (711.3130 364.1447), (678.2820 360.0336), (652.4395 366.2831), (685.0097 370.6662) 10 | 0, (751.8777 416.2984), (708.5916 420.4094), (709.7285 466.8840), (753.3184 462.5622) 11 | -------------------------------------------------------------------------------- /test/data/README.md: -------------------------------------------------------------------------------- 1 | Test image from NASA Swarmathon competition at KSC. Licenced under "CC BY-SA 2.0 DEED". 2 | 3 | Sources: 4 | - https://www.flickr.com/photos/nasakennedy/33369213973/ 5 | - https://www.flickr.com/photos/nasakennedy/34085369442/ 6 | - https://www.flickr.com/photos/nasakennedy/34139872896/ 7 | -------------------------------------------------------------------------------- /test/getline.c: -------------------------------------------------------------------------------- 1 | #include "getline.h" 2 | #include 3 | #include 4 | 5 | // https://stackoverflow.com/a/47229318/8144672 6 | /* The original code is public domain -- Will Hartung 4/9/09 */ 7 | /* Modifications, public domain as well, by Antti Haapala, 11/10/17 - Switched to getc on 5/23/19 */ 8 | 9 | ssize_t apriltag_test_getline(char **lineptr, size_t *n, FILE *stream) { 10 | size_t pos; 11 | int c; 12 | 13 | if (lineptr == NULL || stream == NULL || n == NULL) { 14 | errno = EINVAL; 15 | return -1; 16 | } 17 | 18 | c = getc(stream); 19 | if (c == EOF) { 20 | return -1; 21 | } 22 | 23 | if (*lineptr == NULL) { 24 | *lineptr = malloc(128); 25 | if (*lineptr == NULL) { 26 | return -1; 27 | } 28 | *n = 128; 29 | } 30 | 31 | pos = 0; 32 | while(c != EOF) { 33 | if (pos + 1 >= *n) { 34 | size_t new_size = *n + (*n >> 2); 35 | if (new_size < 128) { 36 | new_size = 128; 37 | } 38 | char *new_ptr = realloc(*lineptr, new_size); 39 | if (new_ptr == NULL) { 40 | return -1; 41 | } 42 | *n = new_size; 43 | *lineptr = new_ptr; 44 | } 45 | 46 | ((unsigned char *)(*lineptr))[pos ++] = c; 47 | if (c == '\n') { 48 | break; 49 | } 50 | c = getc(stream); 51 | } 52 | 53 | (*lineptr)[pos] = '\0'; 54 | return pos; 55 | } 56 | -------------------------------------------------------------------------------- /test/getline.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | typedef intptr_t ssize_t; 8 | 9 | ssize_t apriltag_test_getline(char **lineptr, size_t *n, FILE *stream); 10 | -------------------------------------------------------------------------------- /test/test_detection.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include "getline.h" 8 | 9 | 10 | char* 11 | format(const char* fmt, ...) 12 | { 13 | va_list args1; 14 | va_list args2; 15 | 16 | va_start(args1, fmt); 17 | va_copy(args2, args1); 18 | 19 | const int N = vsnprintf(NULL, 0, fmt, args1); 20 | va_end(args1); 21 | 22 | if (N < 0) { 23 | va_end(args2); 24 | return NULL; 25 | } 26 | 27 | char* res_fmt = calloc(N+1, sizeof(char)); 28 | const int n = vsnprintf(res_fmt, N+1, fmt, args2); 29 | va_end(args2); 30 | 31 | if (n != N) { 32 | free(res_fmt); 33 | return NULL; 34 | } 35 | 36 | // caller has to free returned string 37 | return res_fmt; 38 | } 39 | 40 | int 41 | detection_compare_function(const apriltag_detection_t *a, const apriltag_detection_t *b) 42 | { 43 | if (a->id != b->id) { 44 | return copysign(1, a->id - b->id); 45 | } 46 | 47 | for (int e = 0; e<4; e++) { 48 | for (int c = 0; c<2; c++) { 49 | const double d = a->p[e][c] - b->p[e][c]; 50 | if (fabs(d) > 1e-1) { 51 | return copysign(1, d); 52 | } 53 | } 54 | } 55 | 56 | return 0; 57 | } 58 | 59 | int 60 | detection_array_element_compare_function(const void *_a, const void *_b) 61 | { 62 | const apriltag_detection_t * const a = *(apriltag_detection_t**) _a; 63 | const apriltag_detection_t * const b = *(apriltag_detection_t**) _b; 64 | 65 | return detection_compare_function(a, b); 66 | } 67 | 68 | int 69 | main(int argc, char *argv[]) 70 | { 71 | if (argc!=2) { 72 | return EXIT_FAILURE; 73 | } 74 | 75 | // load image 76 | char* const path_img = format("%s.jpg", argv[1]); 77 | pjpeg_t *pjpeg = pjpeg_create_from_file(path_img, 0, NULL); 78 | image_u8_t *im = pjpeg_to_u8_baseline(pjpeg); 79 | free(path_img); 80 | 81 | // load true detection 82 | char* const path_det_true = format("%s.txt", argv[1]); 83 | FILE *fp = fopen(path_det_true, "r"); 84 | if (fp == NULL) { 85 | return EXIT_FAILURE; 86 | } 87 | free(path_det_true); 88 | 89 | apriltag_detector_t *td = apriltag_detector_create(); 90 | td->quad_decimate = 1; 91 | td->refine_edges = false; 92 | apriltag_family_t *tf = tag36h11_create(); 93 | apriltag_detector_add_family(td, tf); 94 | 95 | const char fmt_det[] = "%i, (%.4lf %.4lf), (%.4lf %.4lf), (%.4lf %.4lf), (%.4lf %.4lf)"; 96 | const char fmt_ref_parse[] = "%i, (%lf %lf), (%lf %lf), (%lf %lf), (%lf %lf)"; 97 | 98 | bool ok = true; 99 | 100 | zarray_t *detections = apriltag_detector_detect(td, im); 101 | 102 | // sort detections by detected corners for deterministic sorting order 103 | zarray_sort(detections, detection_array_element_compare_function); 104 | 105 | int i = 0; 106 | for (; i < zarray_size(detections); i++) { 107 | apriltag_detection_t *det; 108 | zarray_get(detections, i, &det); 109 | 110 | char* const det_fmt = format(fmt_det, 111 | det->id, 112 | det->p[0][0], det->p[0][1], det->p[1][0], det->p[1][1], 113 | det->p[2][0], det->p[2][1], det->p[3][0], det->p[3][1]); 114 | 115 | char* line = NULL; 116 | size_t len = 0; 117 | const ssize_t nread = apriltag_test_getline(&line, &len, fp); 118 | if (nread == -1) { 119 | free(line); 120 | return EXIT_FAILURE; 121 | } 122 | 123 | printf("Got: %s\n", det_fmt); 124 | printf("Expected: %s\n", line); 125 | 126 | // parse reference detection 127 | apriltag_detection_t ref; 128 | const int nparsed = sscanf( 129 | line, fmt_ref_parse, 130 | &ref.id, 131 | &ref.p[0][0], &ref.p[0][1], &ref.p[1][0], &ref.p[1][1], 132 | &ref.p[2][0], &ref.p[2][1], &ref.p[3][0], &ref.p[3][1]); 133 | 134 | (void) nparsed; 135 | assert(nparsed == 9); 136 | 137 | // compare detections 138 | const bool equ = detection_compare_function(det, &ref) == 0; 139 | 140 | if (!equ || det->id != ref.id) { 141 | fprintf(stderr, "Mismatch.\nGot:\n %s\nExpected:\n %s\n", det_fmt, line); 142 | ok = false; 143 | } 144 | 145 | free(det_fmt); 146 | free(line); 147 | } 148 | 149 | // check that we compared the expected amount of detections 150 | // if there are no "true" detections left, we should be at the end of the file 151 | // if there are no "detected" detections left, we should be at the end of the array 152 | if ((fgetc(fp) != -1) && (i != zarray_size(detections))) { 153 | return EXIT_FAILURE; 154 | } 155 | 156 | fclose(fp); 157 | 158 | apriltag_detections_destroy(detections); 159 | image_u8_destroy(im); 160 | pjpeg_destroy(pjpeg); 161 | 162 | apriltag_detector_destroy(td); 163 | tag36h11_destroy(tf); 164 | 165 | if (!ok) { 166 | return EXIT_FAILURE; 167 | } 168 | 169 | return EXIT_SUCCESS; 170 | } 171 | --------------------------------------------------------------------------------