From eda63dc1628af2e9ea7495eae758e974a4483c48 Mon Sep 17 00:00:00 2001 From: Jade Date: Sun, 21 Jul 2024 13:12:49 +0800 Subject: [PATCH] [upstream_utils] Add AprilTag library (#6816) Co-authored-by: Tyler Veness --- .github/workflows/upstream-utils.yml | 5 + apriltag/CMakeLists.txt | 78 +- apriltag/build.gradle | 58 +- .../src/main/native/cpp/AprilTagDetector.cpp | 30 - .../thirdparty/apriltag/include/apriltag.h | 276 +++ .../apriltag/include/apriltag_math.h | 101 + .../apriltag/include/apriltag_pose.h | 81 + .../apriltag/include/common/debug_print.h | 41 + .../apriltag/include/common/doubles.h | 32 + .../include/common/doubles_floats_impl.h | 1036 +++++++++ .../apriltag/include/common/floats.h | 32 + .../thirdparty/apriltag/include/common/g2d.h | 124 + .../apriltag/include/common/homography.h | 183 ++ .../apriltag/include/common/image_types.h | 84 + .../apriltag/include/common/image_u8.h | 90 + .../apriltag/include/common/image_u8x3.h | 64 + .../apriltag/include/common/image_u8x4.h | 65 + .../thirdparty/apriltag/include/common/matd.h | 446 ++++ .../apriltag/include/common/math_util.h | 216 ++ .../thirdparty/apriltag/include/common/pam.h | 54 + .../apriltag/include/common/pjpeg.h | 103 + .../thirdparty/apriltag/include/common/pnm.h | 58 + .../apriltag/include/common/pthreads_cross.h | 81 + .../apriltag/include/common/string_util.h | 465 ++++ .../apriltag/include/common/svd22.h | 34 + .../apriltag/include/common/time_util.h | 83 + .../apriltag/include/common/timeprofile.h | 120 + .../apriltag/include/common/unionfind.h | 145 ++ .../apriltag/include/common/workerpool.h | 49 + .../apriltag/include/common/zarray.h | 465 ++++ .../apriltag/include/common/zhash.h | 432 ++++ .../apriltag/include/common/zmaxheap.h | 76 + .../thirdparty/apriltag/include/tag16h5.h | 44 + .../thirdparty/apriltag/include/tag36h11.h | 44 + .../native/thirdparty/apriltag/src/apriltag.c | 1448 ++++++++++++ .../thirdparty/apriltag/src/apriltag_pose.c | 559 +++++ .../apriltag/src/apriltag_quad_thresh.c | 2017 ++++++++++++++++ .../thirdparty/apriltag/src/common/g2d.c | 919 ++++++++ .../apriltag/src/common/homography.c | 479 ++++ .../thirdparty/apriltag/src/common/image_u8.c | 557 +++++ .../apriltag/src/common/image_u8x3.c | 274 +++ .../apriltag/src/common/image_u8x4.c | 233 ++ .../thirdparty/apriltag/src/common/matd.c | 2029 +++++++++++++++++ .../thirdparty/apriltag/src/common/pam.c | 256 +++ .../apriltag/src/common/pjpeg-idct.c | 388 ++++ .../thirdparty/apriltag/src/common/pjpeg.c | 894 ++++++++ .../thirdparty/apriltag/src/common/pnm.c | 154 ++ .../apriltag/src/common/pthreads_cross.cpp | 259 +++ .../apriltag/src/common/string_util.c | 772 +++++++ .../thirdparty/apriltag/src/common/svd22.c | 258 +++ .../apriltag/src/common/time_util.c | 162 ++ .../apriltag/src/common/unionfind.c | 30 + .../apriltag/src/common/workerpool.c | 215 ++ .../thirdparty/apriltag/src/common/zarray.c | 55 + .../thirdparty/apriltag/src/common/zhash.c | 562 +++++ .../thirdparty/apriltag/src/common/zmaxheap.c | 424 ++++ .../native/thirdparty/apriltag/src/tag16h5.c | 117 + .../native/thirdparty/apriltag/src/tag36h11.c | 714 ++++++ .../first/apriltag/AprilTagDetectorTest.java | 5 - .../test/native/cpp/AprilTagDetectorTest.cpp | 5 - docs/build.gradle | 3 + shared/apriltaglib.gradle | 13 - upstream_utils/apriltag.py | 94 + ...Set-NULL-when-second-solution-could-.patch | 22 + ...pify-Avoid-return-of-void-expression.patch | 23 + ...-variable-warnings-in-release-builds.patch | 70 + ...iteration-exit-early-upon-convergenc.patch | 141 ++ .../0005-Fix-signed-left-shift-warning.patch | 31 + ...6-Avoid-incompatible-pointer-warning.patch | 24 + .../0007-Fix-GCC-14-calloc-warning.patch | 32 + ...008-Remove-calls-to-postscript_image.patch | 61 + .../0009-Fix-clang-16-warnings.patch | 691 ++++++ 72 files changed, 20189 insertions(+), 96 deletions(-) create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/apriltag.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/apriltag_math.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/apriltag_pose.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/debug_print.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/doubles.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/doubles_floats_impl.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/floats.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/g2d.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/homography.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/image_types.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/image_u8.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/image_u8x3.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/image_u8x4.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/matd.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/math_util.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/pam.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/pjpeg.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/pnm.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/pthreads_cross.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/string_util.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/svd22.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/time_util.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/timeprofile.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/unionfind.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/workerpool.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/zarray.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/zhash.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/common/zmaxheap.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/tag16h5.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/include/tag36h11.h create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/apriltag.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/apriltag_pose.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/apriltag_quad_thresh.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/common/g2d.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/common/homography.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/common/image_u8.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/common/image_u8x3.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/common/image_u8x4.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/common/matd.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/common/pam.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/common/pjpeg-idct.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/common/pjpeg.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/common/pnm.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/common/pthreads_cross.cpp create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/common/string_util.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/common/svd22.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/common/time_util.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/common/unionfind.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/common/workerpool.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/common/zarray.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/common/zhash.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/common/zmaxheap.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/tag16h5.c create mode 100644 apriltag/src/main/native/thirdparty/apriltag/src/tag36h11.c delete mode 100644 shared/apriltaglib.gradle create mode 100755 upstream_utils/apriltag.py create mode 100644 upstream_utils/apriltag_patches/0001-apriltag_pose.c-Set-NULL-when-second-solution-could-.patch create mode 100644 upstream_utils/apriltag_patches/0002-zmaxheapify-Avoid-return-of-void-expression.patch create mode 100644 upstream_utils/apriltag_patches/0003-Avoid-unused-variable-warnings-in-release-builds.patch create mode 100644 upstream_utils/apriltag_patches/0004-Make-orthogonal_iteration-exit-early-upon-convergenc.patch create mode 100644 upstream_utils/apriltag_patches/0005-Fix-signed-left-shift-warning.patch create mode 100644 upstream_utils/apriltag_patches/0006-Avoid-incompatible-pointer-warning.patch create mode 100644 upstream_utils/apriltag_patches/0007-Fix-GCC-14-calloc-warning.patch create mode 100644 upstream_utils/apriltag_patches/0008-Remove-calls-to-postscript_image.patch create mode 100644 upstream_utils/apriltag_patches/0009-Fix-clang-16-warnings.patch diff --git a/.github/workflows/upstream-utils.yml b/.github/workflows/upstream-utils.yml index aa45fd8717..2c05509682 100644 --- a/.github/workflows/upstream-utils.yml +++ b/.github/workflows/upstream-utils.yml @@ -30,6 +30,11 @@ jobs: run: | git config --global user.email "you@example.com" git config --global user.name "Your Name" + - name: Run apriltag.py + run: | + cd upstream_utils + ./apriltag.py clone + ./apriltag.py copy-upstream-to-thirdparty - name: Run eigen.py run: | cd upstream_utils diff --git a/apriltag/CMakeLists.txt b/apriltag/CMakeLists.txt index 0287f21d7c..dcaca5f00d 100644 --- a/apriltag/CMakeLists.txt +++ b/apriltag/CMakeLists.txt @@ -2,28 +2,35 @@ project(apriltag) include(CompileWarnings) include(GenResources) -include(FetchContent) -fetchcontent_declare( - apriltaglib - GIT_REPOSITORY https://github.com/wpilibsuite/apriltag.git - GIT_TAG da208cc38c1b78fe89861616d44c0692e76b6b8b +file( + GLOB_RECURSE apriltaglib_src + src/main/native/thirdparty/apriltag/src/*.c + src/main/native/thirdparty/apriltag/src/*.cpp ) -# Don't use apriltag's CMakeLists.txt due to conflicting naming and JNI -fetchcontent_getproperties(apriltaglib) -if(NOT apriltaglib_POPULATED) - fetchcontent_populate(apriltaglib) +# Disable apriltag C library warnings +if(MSVC) + set_source_files_properties( + ${apriltaglib_src} + PROPERTIES COMPILE_FLAGS "/wd2220 /wd4005 /wd4018 /wd4244 /wd4267 /wd4996" + ) +elseif(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + set_source_files_properties( + ${apriltaglib_src} + PROPERTIES + COMPILE_FLAGS + "-Wno-format-nonliteral -Wno-gnu-zero-variadic-macro-arguments -Wno-uninitialized -Wno-sign-compare -Wno-type-limits" + ) +else() + set_source_files_properties( + ${apriltaglib_src} + PROPERTIES + COMPILE_FLAGS + "-Wno-format-nonliteral -Wno-gnu-zero-variadic-macro-arguments -Wno-maybe-uninitialized -Wno-sign-compare -Wno-type-limits" + ) endif() -aux_source_directory(${apriltaglib_SOURCE_DIR}/common APRILTAGLIB_COMMON_SRC) -file(GLOB TAG_FILES ${apriltaglib_SOURCE_DIR}/tag*.c) -set(APRILTAGLIB_SRCS - ${apriltaglib_SOURCE_DIR}/apriltag.c - ${apriltaglib_SOURCE_DIR}/apriltag_pose.c - ${apriltaglib_SOURCE_DIR}/apriltag_quad_thresh.c -) - file(GLOB apriltag_jni_src src/main/native/cpp/jni/AprilTagJNI.cpp) if(WITH_JAVA) @@ -72,7 +79,7 @@ if(WITH_JAVA) install_jar(apriltag_jar DESTINATION ${java_lib_dest}) install_jar_exports(TARGETS apriltag_jar FILE apriltag_jar.cmake DESTINATION share/apriltag) - add_library(apriltagjni ${apriltag_jni_src}) + add_library(apriltagjni ${apriltag_jni_src} ${apriltaglib_src}) wpilib_target_warnings(apriltagjni) target_link_libraries(apriltagjni PUBLIC apriltag) @@ -109,32 +116,12 @@ generate_resources( file(GLOB apriltag_native_src src/main/native/cpp/*.cpp) -add_library( - apriltag - ${apriltag_native_src} - ${apriltag_resources_src} - ${APRILTAGLIB_SRCS} - ${APRILTAGLIB_COMMON_SRC} - ${TAG_FILES} -) +add_library(apriltag ${apriltag_native_src} ${apriltag_resources_src} ${apriltaglib_src}) set_target_properties(apriltag PROPERTIES DEBUG_POSTFIX "d") set_property(TARGET apriltag PROPERTY FOLDER "libraries") target_compile_features(apriltag PUBLIC cxx_std_20) wpilib_target_warnings(apriltag) -# disable warnings that apriltaglib can't handle -if(MSVC) - target_compile_options(apriltag PRIVATE /wd4018 /wd4005 /wd4996) -else() - target_compile_options( - apriltag - PRIVATE - -Wno-sign-compare - -Wno-gnu-zero-variadic-macro-arguments - -Wno-type-limits - -Wno-format-nonliteral - ) -endif() target_link_libraries(apriltag wpimath) @@ -146,6 +133,19 @@ target_include_directories( $ ) +install( + DIRECTORY src/main/native/thirdparty/apriltag/include/ + DESTINATION "${include_dest}/apriltag" +) +target_include_directories( + apriltag + SYSTEM + PUBLIC + $ + $ + $ +) + install(TARGETS apriltag EXPORT apriltag) export(TARGETS apriltag FILE apriltag.cmake NAMESPACE apriltag::) install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/apriltag") diff --git a/apriltag/build.gradle b/apriltag/build.gradle index 5fb7b3490e..67e2d655d3 100644 --- a/apriltag/build.gradle +++ b/apriltag/build.gradle @@ -1,5 +1,7 @@ apply from: "${rootDir}/shared/resources.gradle" +apply plugin: 'c' + ext { nativeName = 'apriltag' devMain = 'edu.wpi.first.apriltag.DevMain' @@ -23,6 +25,17 @@ ext { include '*.cpp' } } + apriltagC(CSourceSet) { + source { + srcDirs 'src/main/native/thirdparty/apriltag/src' + include '**/*.c', '**/*.cpp' + } + exportedHeaders { + srcDirs 'src/main/native/thirdparty/apriltag/include', + 'src/main/native/thirdparty/apriltag/include/common', + 'src/main/native/thirdparty/apriltag/src' + } + } } } } @@ -30,7 +43,6 @@ ext { evaluationDependsOn(':wpimath') apply from: "${rootDir}/shared/jni/setupBuild.gradle" -apply from: "${rootDir}/shared/apriltaglib.gradle" apply from: "${rootDir}/shared/opencv.gradle" dependencies { @@ -45,8 +57,48 @@ sourceSets { } } +cppHeadersZip { + from('src/main/native/thirdparty/apriltag/include') { + into '/' + } +} + +// Suppress sign-compare warnings +nativeUtils.platformConfigs.each { + if (it.name.contains('windows')) { + it.cCompiler.args.add("/wd2220") + it.cCompiler.args.add("/wd4005") + it.cCompiler.args.add("/wd4018") + it.cCompiler.args.add("/wd4244") + it.cCompiler.args.add("/wd4267") + it.cCompiler.args.add("/wd4996") + } else if (it.name.contains('osx')) { + it.cCompiler.args.add("-Wno-format-nonliteral") + it.cCompiler.args.add("-Wno-gnu-zero-variadic-macro-arguments") + it.cCompiler.args.add("-Wno-uninitialized") + it.cCompiler.args.add("-Wno-sign-compare") + it.cCompiler.args.add("-Wno-type-limits") + } else { + it.cCompiler.args.add("-Wno-format-nonliteral") + it.cCompiler.args.add("-Wno-gnu-zero-variadic-macro-arguments") + it.cCompiler.args.add("-Wno-maybe-uninitialized") + it.cCompiler.args.add("-Wno-sign-compare") + it.cCompiler.args.add("-Wno-type-limits") + } +} + model { - components {} + components { + all { + it.sources.each { + it.exportedHeaders { + srcDirs 'src/main/native/include', + 'src/main/native/thirdparty/apriltag/include', + 'src/main/native/thirdparty/apriltag/include/common' + } + } + } + } binaries { all { if (!it.buildable || !(it instanceof NativeBinarySpec)) { @@ -61,8 +113,6 @@ model { lib project: ':wpimath', library: 'wpimath', linkage: 'shared' lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' } - - nativeUtils.useRequiredLibrary(it, 'apriltaglib') } } tasks { diff --git a/apriltag/src/main/native/cpp/AprilTagDetector.cpp b/apriltag/src/main/native/cpp/AprilTagDetector.cpp index 036dc20431..ca06c2b947 100644 --- a/apriltag/src/main/native/cpp/AprilTagDetector.cpp +++ b/apriltag/src/main/native/cpp/AprilTagDetector.cpp @@ -17,13 +17,7 @@ #include "apriltag.h" #include "tag16h5.h" -#include "tag25h9.h" #include "tag36h11.h" -#include "tagCircle21h7.h" -#include "tagCircle49h12.h" -#include "tagCustom48h12.h" -#include "tagStandard41h12.h" -#include "tagStandard52h13.h" using namespace frc; @@ -117,20 +111,8 @@ bool AprilTagDetector::AddFamily(std::string_view fam, int bitsCorrected) { // create the family if (fam == "tag16h5") { data = tag16h5_create(); - } else if (fam == "tag25h9") { - data = tag25h9_create(); } else if (fam == "tag36h11") { data = tag36h11_create(); - } else if (fam == "tagCircle21h7") { - data = tagCircle21h7_create(); - } else if (fam == "tagCircle49h12") { - data = tagCircle49h12_create(); - } else if (fam == "tagStandard41h12") { - data = tagStandard41h12_create(); - } else if (fam == "tagStandard52h13") { - data = tagStandard52h13_create(); - } else if (fam == "tagCustom48h12") { - data = tagCustom48h12_create(); } if (!data) { m_families.erase(fam); // don't keep null value @@ -184,19 +166,7 @@ void AprilTagDetector::DestroyFamily(std::string_view name, void* data) { auto fam = static_cast(data); if (name == "tag16h5") { tag16h5_destroy(fam); - } else if (name == "tag25h9") { - tag25h9_destroy(fam); } else if (name == "tag36h11") { tag36h11_destroy(fam); - } else if (name == "tagCircle21h7") { - tagCircle21h7_destroy(fam); - } else if (name == "tagCircle49h12") { - tagCircle49h12_destroy(fam); - } else if (name == "tagStandard41h12") { - tagStandard41h12_destroy(fam); - } else if (name == "tagStandard52h13") { - tagStandard52h13_destroy(fam); - } else if (name == "tagCustom48h12") { - tagCustom48h12_destroy(fam); } } diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/apriltag.h b/apriltag/src/main/native/thirdparty/apriltag/include/apriltag.h new file mode 100644 index 0000000000..eb58a71d97 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/apriltag.h @@ -0,0 +1,276 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#include "common/matd.h" +#include "common/image_u8.h" +#include "common/zarray.h" +#include "common/workerpool.h" +#include "common/timeprofile.h" +#include "common/pthreads_cross.h" + +#define APRILTAG_TASKS_PER_THREAD_TARGET 10 + +struct quad +{ + float p[4][2]; // corners + + bool reversed_border; + + // H: tag coordinates ([-1,1] at the black corners) to pixels + // Hinv: pixels to tag + matd_t *H, *Hinv; +}; + +// Represents a tag family. Every tag belongs to a tag family. Tag +// families are generated by the Java tool +// april.tag.TagFamilyGenerator and can be converted to C using +// april.tag.TagToC. +typedef struct apriltag_family apriltag_family_t; +struct apriltag_family +{ + // How many codes are there in this tag family? + uint32_t ncodes; + + // The codes in the family. + uint64_t *codes; + + int width_at_border; + int total_width; + bool reversed_border; + + // The bit locations. + uint32_t nbits; + uint32_t *bit_x; + uint32_t *bit_y; + + // minimum hamming distance between any two codes. (e.g. 36h11 => 11) + uint32_t h; + + // a human-readable name, e.g., "tag36h11" + char *name; + + // some detector implementations may preprocess codes in order to + // accelerate decoding. They put their data here. (Do not use the + // same apriltag_family instance in more than one implementation) + void *impl; +}; + + +struct apriltag_quad_thresh_params +{ + // reject quads containing too few pixels + int min_cluster_pixels; + + // how many corner candidates to consider when segmenting a group + // of pixels into a quad. + int max_nmaxima; + + // Reject quads where pairs of edges have angles that are close to + // straight or close to 180 degrees. Zero means that no quads are + // rejected. (In radians). + float critical_rad; + float cos_critical_rad; + + // When fitting lines to the contours, what is the maximum mean + // squared error allowed? This is useful in rejecting contours + // that are far from being quad shaped; rejecting these quads "early" + // saves expensive decoding processing. + float max_line_fit_mse; + + // When we build our model of black & white pixels, we add an + // extra check that the white model must be (overall) brighter + // than the black model. How much brighter? (in pixel values, + // [0,255]). . + int min_white_black_diff; + + // should the thresholded image be deglitched? Only useful for + // very noisy images + int deglitch; +}; + +// Represents a detector object. Upon creating a detector, all fields +// are set to reasonable values, but can be overridden by accessing +// these fields. +typedef struct apriltag_detector apriltag_detector_t; +struct apriltag_detector +{ + /////////////////////////////////////////////////////////////// + // User-configurable parameters. + + // How many threads should be used? + int nthreads; + + // detection of quads can be done on a lower-resolution image, + // improving speed at a cost of pose accuracy and a slight + // decrease in detection rate. Decoding the binary payload is + // still done at full resolution. . + float quad_decimate; + + // What Gaussian blur should be applied to the segmented image + // (used for quad detection?) Parameter is the standard deviation + // in pixels. Very noisy images benefit from non-zero values + // (e.g. 0.8). + float quad_sigma; + + // When true, the edges of the each quad are adjusted to "snap + // to" strong gradients nearby. This is useful when decimation is + // employed, as it can increase the quality of the initial quad + // estimate substantially. Generally recommended to be on (true). + // + // Very computationally inexpensive. Option is ignored if + // quad_decimate = 1. + bool refine_edges; + + // How much sharpening should be done to decoded images? This + // can help decode small tags but may or may not help in odd + // lighting conditions or low light conditions. + // + // The default value is 0.25. + double decode_sharpening; + + // When true, write a variety of debugging images to the + // current working directory at various stages through the + // detection process. (Somewhat slow). + bool debug; + + struct apriltag_quad_thresh_params qtp; + + /////////////////////////////////////////////////////////////// + // Statistics relating to last processed frame + timeprofile_t *tp; + + uint32_t nedges; + uint32_t nsegments; + uint32_t nquads; + + /////////////////////////////////////////////////////////////// + // Internal variables below + + // Not freed on apriltag_destroy; a tag family can be shared + // between multiple users. The user should ultimately destroy the + // tag family passed into the constructor. + zarray_t *tag_families; + + // Used to manage multi-threading. + workerpool_t *wp; + + // Used for thread safety. + pthread_mutex_t mutex; +}; + +// Represents the detection of a tag. These are returned to the user +// and must be individually destroyed by the user. +typedef struct apriltag_detection apriltag_detection_t; +struct apriltag_detection +{ + // a pointer for convenience. not freed by apriltag_detection_destroy. + apriltag_family_t *family; + + // The decoded ID of the tag + int id; + + // How many error bits were corrected? Note: accepting large numbers of + // corrected errors leads to greatly increased false positive rates. + // NOTE: As of this implementation, the detector cannot detect tags with + // a hamming distance greater than 2. + int hamming; + + // A measure of the quality of the binary decoding process: the + // average difference between the intensity of a data bit versus + // the decision threshold. Higher numbers roughly indicate better + // decodes. This is a reasonable measure of detection accuracy + // only for very small tags-- not effective for larger tags (where + // we could have sampled anywhere within a bit cell and still + // gotten a good detection.) + float decision_margin; + + // The 3x3 homography matrix describing the projection from an + // "ideal" tag (with corners at (-1,1), (1,1), (1,-1), and (-1, + // -1)) to pixels in the image. This matrix will be freed by + // apriltag_detection_destroy. + matd_t *H; + + // The center of the detection in image pixel coordinates. + double c[2]; + + // The corners of the tag in image pixel coordinates. These always + // wrap counter-clock wise around the tag. + double p[4][2]; +}; + +// don't forget to add a family! +apriltag_detector_t *apriltag_detector_create(void); + +// add a family to the apriltag detector. caller still "owns" the family. +// a single instance should only be provided to one apriltag detector instance. +void apriltag_detector_add_family_bits(apriltag_detector_t *td, apriltag_family_t *fam, int bits_corrected); + +// Tunable, but really, 2 is a good choice. Values of >=3 +// consume prohibitively large amounts of memory, and otherwise +// you want the largest value possible. +static inline void apriltag_detector_add_family(apriltag_detector_t *td, apriltag_family_t *fam) +{ + apriltag_detector_add_family_bits(td, fam, 2); +} + +// does not deallocate the family. +void apriltag_detector_remove_family(apriltag_detector_t *td, apriltag_family_t *fam); + +// unregister all families, but does not deallocate the underlying tag family objects. +void apriltag_detector_clear_families(apriltag_detector_t *td); + +// Destroy the april tag detector (but not the underlying +// apriltag_family_t used to initialize it.) +void apriltag_detector_destroy(apriltag_detector_t *td); + +// Detect tags from an image and return an array of +// apriltag_detection_t*. You can use apriltag_detections_destroy to +// free the array and the detections it contains, or call +// _detection_destroy and zarray_destroy yourself. +zarray_t *apriltag_detector_detect(apriltag_detector_t *td, image_u8_t *im_orig); + +// Call this method on each of the tags returned by apriltag_detector_detect +void apriltag_detection_destroy(apriltag_detection_t *det); + +// destroys the array AND the detections within it. +void apriltag_detections_destroy(zarray_t *detections); + +// Renders the apriltag. +// Caller is responsible for calling image_u8_destroy on the image +image_u8_t *apriltag_to_image(apriltag_family_t *fam, int idx); + +#ifdef __cplusplus +} +#endif diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/apriltag_math.h b/apriltag/src/main/native/thirdparty/apriltag/include/apriltag_math.h new file mode 100644 index 0000000000..60bf907367 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/apriltag_math.h @@ -0,0 +1,101 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#include + +// Computes the cholesky factorization of A, putting the lower +// triangular matrix into R. +static inline void mat33_chol(const double *A, + double *R) +{ + // A[0] = R[0]*R[0] + R[0] = sqrt(A[0]); + + // A[1] = R[0]*R[3]; + R[3] = A[1] / R[0]; + + // A[2] = R[0]*R[6]; + R[6] = A[2] / R[0]; + + // A[4] = R[3]*R[3] + R[4]*R[4] + R[4] = sqrt(A[4] - R[3]*R[3]); + + // A[5] = R[3]*R[6] + R[4]*R[7] + R[7] = (A[5] - R[3]*R[6]) / R[4]; + + // A[8] = R[6]*R[6] + R[7]*R[7] + R[8]*R[8] + R[8] = sqrt(A[8] - R[6]*R[6] - R[7]*R[7]); + + R[1] = 0; + R[2] = 0; + R[5] = 0; +} + +static inline void mat33_lower_tri_inv(const double *A, + double *R) +{ + // A[0]*R[0] = 1 + R[0] = 1 / A[0]; + + // A[3]*R[0] + A[4]*R[3] = 0 + R[3] = -A[3]*R[0] / A[4]; + + // A[4]*R[4] = 1 + R[4] = 1 / A[4]; + + // A[6]*R[0] + A[7]*R[3] + A[8]*R[6] = 0 + R[6] = (-A[6]*R[0] - A[7]*R[3]) / A[8]; + + // A[7]*R[4] + A[8]*R[7] = 0 + R[7] = -A[7]*R[4] / A[8]; + + // A[8]*R[8] = 1 + R[8] = 1 / A[8]; +} + + +static inline void mat33_sym_solve(const double *A, + const double *B, + double *R) +{ + double L[9]; + mat33_chol(A, L); + + double M[9]; + mat33_lower_tri_inv(L, M); + + double tmp[3]; + tmp[0] = M[0]*B[0]; + tmp[1] = M[3]*B[0] + M[4]*B[1]; + tmp[2] = M[6]*B[0] + M[7]*B[1] + M[8]*B[2]; + + R[0] = M[0]*tmp[0] + M[3]*tmp[1] + M[6]*tmp[2]; + R[1] = M[4]*tmp[1] + M[7]*tmp[2]; + R[2] = M[8]*tmp[2]; +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/apriltag_pose.h b/apriltag/src/main/native/thirdparty/apriltag/include/apriltag_pose.h new file mode 100644 index 0000000000..72993f1151 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/apriltag_pose.h @@ -0,0 +1,81 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + + +#include "apriltag.h" +#include "common/matd.h" + +typedef struct { + apriltag_detection_t* det; + double tagsize; // In meters. + double fx; // In pixels. + double fy; // In pixels. + double cx; // In pixels. + double cy; // In pixels. +} apriltag_detection_info_t; + +/** + * This struct holds the transformation from the camera optical frame to + * the April tag frame. The pose refers to the position of the tag within + * the camera frame. + */ +typedef struct { + matd_t* R; // Rotation matrix 3x3 of doubles. + matd_t* t; // Translation matrix 3x1 of doubles. +} apriltag_pose_t; + +/** + * Estimate pose of the tag using the homography method described in [1]. + * @outparam pose + */ +void estimate_pose_for_tag_homography( + apriltag_detection_info_t* info, + apriltag_pose_t* pose); + +/** + * Estimate pose of the tag. This returns one or two possible poses for the + * tag, along with the object-space error of each. + * + * This uses the homography method described in [1] for the initial estimate. + * Then Orthogonal Iteration [2] is used to refine this estimate. Then [3] is + * used to find a potential second local minima and Orthogonal Iteration is + * used to refine this second estimate. + * + * [1]: E. Olson, “Apriltag: A robust and flexible visual fiducial system,” in + * 2011 IEEE International Conference on Robotics and Automation, + * May 2011, pp. 3400–3407. + * [2]: Lu, G. D. Hager and E. Mjolsness, "Fast and globally convergent pose + * estimation from video images," in IEEE Transactions on Pattern Analysis + * and Machine Intelligence, vol. 22, no. 6, pp. 610-622, June 2000. + * doi: 10.1109/34.862199 + * [3]: Schweighofer and A. Pinz, "Robust Pose Estimation from a Planar Target," + * in IEEE Transactions on Pattern Analysis and Machine Intelligence, + * vol. 28, no. 12, pp. 2024-2030, Dec. 2006. doi: 10.1109/TPAMI.2006.252 + * + * @outparam err1, pose1, err2, pose2 + */ +void estimate_tag_pose_orthogonal_iteration( + apriltag_detection_info_t* info, + double* err1, + apriltag_pose_t* pose1, + double* err2, + apriltag_pose_t* pose2, + int nIters, + double min_improvement_per_iteration); + +/** + * Estimate tag pose. + * This method is an easier to use interface to estimate_tag_pose_orthogonal_iteration. + * + * @outparam pose + * @return Object-space error of returned pose. + */ +double estimate_tag_pose(apriltag_detection_info_t* info, apriltag_pose_t* pose); + +#ifdef __cplusplus +} +#endif + diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/debug_print.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/debug_print.h new file mode 100644 index 0000000000..77e0d780e4 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/debug_print.h @@ -0,0 +1,41 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ +#pragma once + +#if !defined(NDEBUG) || defined(_DEBUG) + +#include +#include +#define DEBUG 1 + +#else +#define DEBUG 0 +#endif + +#define debug_print(fmt, ...) \ + do { if (DEBUG) fprintf(stderr, "%s:%d:%s(): " fmt, strrchr("/"__FILE__,'/')+1, \ + __LINE__, __func__, ##__VA_ARGS__); fflush(stderr);} while (0) diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/doubles.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/doubles.h new file mode 100644 index 0000000000..fd5fc829cf --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/doubles.h @@ -0,0 +1,32 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#define TNAME double +#include "doubles_floats_impl.h" +#undef TNAME diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/doubles_floats_impl.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/doubles_floats_impl.h new file mode 100644 index 0000000000..1817e083d6 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/doubles_floats_impl.h @@ -0,0 +1,1036 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#include +#include +#include +#include + +#include "matd.h" +#include "math_util.h" + +// XXX Write unit tests for me! +// XXX Rewrite matd_coords in terms of this. + +/* + This file provides conversions between the following formats: + + quaternion (TNAME[4], { w, x, y, z}) + + xyt (translation in x, y, and rotation in radians.) + + xytcov (xyt as a TNAME[3] followed by covariance TNAME[9]) + + xy, xyz (translation in x, y, and z) + + mat44 (4x4 rigid-body transformation matrix, row-major + order. Conventions: We assume points are projected via right + multiplication. E.g., p' = Mp.) Note: some functions really do rely + on it being a RIGID, scale=1 transform. + + angleaxis (TNAME[4], { angle-rads, x, y, z } + + xyzrpy (translation x, y, z, euler angles) + + Roll Pitch Yaw are evaluated in the order: roll, pitch, then yaw. I.e., + rollPitchYawToMatrix(rpy) = rotateZ(rpy[2]) * rotateY(rpy[1]) * Rotatex(rpy[0]) +*/ + +#define TRRFN(root, suffix) root ## suffix +#define TRFN(root, suffix) TRRFN(root, suffix) +#define TFN(suffix) TRFN(TNAME, suffix) + +// if V is null, returns null. +static inline TNAME *TFN(s_dup)(const TNAME *v, int len) +{ + if (!v) + return NULL; + + TNAME *r = (TNAME*)malloc(len * sizeof(TNAME)); + memcpy(r, v, len * sizeof(TNAME)); + return r; +} + +static inline void TFN(s_print)(const TNAME *a, int len, const char *fmt) +{ + for (int i = 0; i < len; i++) + printf(fmt, a[i]); + printf("\n"); +} + +static inline void TFN(s_print_mat)(const TNAME *a, int nrows, int ncols, const char *fmt) +{ + for (int i = 0; i < nrows * ncols; i++) { + printf(fmt, a[i]); + if ((i % ncols) == (ncols - 1)) + printf("\n"); + } +} + +static inline void TFN(s_print_mat44)(const TNAME *a, const char *fmt) +{ + for (int i = 0; i < 4 * 4; i++) { + printf(fmt, a[i]); + if ((i % 4) == 3) + printf("\n"); + } +} + +static inline void TFN(s_add)(const TNAME *a, const TNAME *b, int len, TNAME *r) +{ + for (int i = 0; i < len; i++) + r[i] = a[i] + b[i]; +} + +static inline void TFN(s_subtract)(const TNAME *a, const TNAME *b, int len, TNAME *r) +{ + for (int i = 0; i < len; i++) + r[i] = a[i] - b[i]; +} + +static inline void TFN(s_scale)(TNAME s, const TNAME *v, int len, TNAME *r) +{ + for (int i = 0; i < len; i++) + r[i] = s * v[i]; +} + +static inline TNAME TFN(s_dot)(const TNAME *a, const TNAME *b, int len) +{ + TNAME acc = 0; + for (int i = 0; i < len; i++) + acc += a[i] * b[i]; + return acc; +} + +static inline TNAME TFN(s_distance)(const TNAME *a, const TNAME *b, int len) +{ + TNAME acc = 0; + for (int i = 0; i < len; i++) + acc += (a[i] - b[i])*(a[i] - b[i]); + return (TNAME)sqrt(acc); +} + +static inline TNAME TFN(s_squared_distance)(const TNAME *a, const TNAME *b, int len) +{ + TNAME acc = 0; + for (int i = 0; i < len; i++) + acc += (a[i] - b[i])*(a[i] - b[i]); + return acc; +} + +static inline TNAME TFN(s_squared_magnitude)(const TNAME *v, int len) +{ + TNAME acc = 0; + for (int i = 0; i < len; i++) + acc += v[i]*v[i]; + return acc; +} + +static inline TNAME TFN(s_magnitude)(const TNAME *v, int len) +{ + TNAME acc = 0; + for (int i = 0; i < len; i++) + acc += v[i]*v[i]; + return (TNAME)sqrt(acc); +} + +static inline void TFN(s_normalize)(const TNAME *v, int len, TNAME *r) +{ + TNAME mag = TFN(s_magnitude)(v, len); + for (int i = 0; i < len; i++) + r[i] = v[i] / mag; +} + +static inline void TFN(s_normalize_self)(TNAME *v, int len) +{ + TNAME mag = TFN(s_magnitude)(v, len); + for (int i = 0; i < len; i++) + v[i] /= mag; +} + +static inline void TFN(s_scale_self)(TNAME *v, int len, double scale) +{ + for (int i = 0; i < len; i++) + v[i] = (TNAME)(v[i] * scale); +} + +static inline void TFN(s_quat_rotate)(const TNAME q[4], const TNAME v[3], TNAME r[3]) +{ + TNAME t2, t3, t4, t5, t6, t7, t8, t9, t10; + + t2 = q[0]*q[1]; + t3 = q[0]*q[2]; + t4 = q[0]*q[3]; + t5 = -q[1]*q[1]; + t6 = q[1]*q[2]; + t7 = q[1]*q[3]; + t8 = -q[2]*q[2]; + t9 = q[2]*q[3]; + t10 = -q[3]*q[3]; + + r[0] = 2*((t8+t10)*v[0] + (t6-t4)*v[1] + (t3+t7)*v[2]) + v[0]; + r[1] = 2*((t4+t6)*v[0] + (t5+t10)*v[1] + (t9-t2)*v[2]) + v[1]; + r[2] = 2*((t7-t3)*v[0] + (t2+t9)*v[1] + (t5+t8)*v[2]) + v[2]; +} + +static inline void TFN(s_quat_multiply)(const TNAME a[4], const TNAME b[4], TNAME r[4]) +{ + r[0] = a[0]*b[0] - a[1]*b[1] - a[2]*b[2] - a[3]*b[3]; + r[1] = a[0]*b[1] + a[1]*b[0] + a[2]*b[3] - a[3]*b[2]; + r[2] = a[0]*b[2] - a[1]*b[3] + a[2]*b[0] + a[3]*b[1]; + r[3] = a[0]*b[3] + a[1]*b[2] - a[2]*b[1] + a[3]*b[0]; +} + +static inline void TFN(s_quat_inverse)(const TNAME q[4], TNAME r[4]) +{ + TNAME mag = TFN(s_magnitude)(q, 4); + r[0] = q[0]/mag; + r[1] = -q[1]/mag; + r[2] = -q[2]/mag; + r[3] = -q[3]/mag; +} + +static inline void TFN(s_copy)(const TNAME *src, TNAME *dst, int n) +{ + memcpy(dst, src, n * sizeof(TNAME)); +} + +static inline void TFN(s_xyt_copy)(const TNAME xyt[3], TNAME r[3]) +{ + TFN(s_copy)(xyt, r, 3); +} + +static inline void TFN(s_xyt_to_mat44)(const TNAME xyt[3], TNAME r[16]) +{ + TNAME s = (TNAME)sin(xyt[2]), c = (TNAME)cos(xyt[2]); + memset(r, 0, sizeof(TNAME)*16); + r[0] = c; + r[1] = -s; + r[3] = xyt[0]; + r[4] = s; + r[5] = c; + r[7] = xyt[1]; + r[10] = 1; + r[15] = 1; +} + +static inline void TFN(s_xyt_transform_xy)(const TNAME xyt[3], const TNAME xy[2], TNAME r[2]) +{ + TNAME s = (TNAME)sin(xyt[2]), c = (TNAME)cos(xyt[2]); + r[0] = c*xy[0] - s*xy[1] + xyt[0]; + r[1] = s*xy[0] + c*xy[1] + xyt[1]; +} + +static inline void TFN(s_mat_transform_xyz)(const TNAME M[16], const TNAME xyz[3], TNAME r[3]) +{ + r[0] = M[0]*xyz[0] + M[1]*xyz[1] + M[2]*xyz[2] + M[3]; + r[1] = M[4]*xyz[0] + M[5]*xyz[1] + M[6]*xyz[2] + M[7]; + r[2] = M[8]*xyz[0] + M[9]*xyz[1] + M[10]*xyz[2] + M[11]; +} + +static inline void TFN(s_quat_to_angleaxis)(const TNAME _q[4], TNAME r[4]) +{ + TNAME q[4]; + TFN(s_normalize)(_q, 4, q); + + // be polite: return an angle from [-pi, pi] + // use atan2 to be 4-quadrant safe + TNAME mag = TFN(s_magnitude)(&q[1], 3); + r[0] = (TNAME)mod2pi(2 * atan2(mag, q[0])); + if (mag != 0) { + r[1] = q[1] / mag; + r[2] = q[2] / mag; + r[3] = q[3] / mag; + } else { + r[1] = 1; + r[2] = 0; + r[3] = 0; + } +} + +static inline void TFN(s_angleaxis_to_quat)(const TNAME aa[4], TNAME q[4]) +{ + TNAME rad = aa[0]; + q[0] = (TNAME)cos(rad / 2.0); + TNAME s = (TNAME)sin(rad / 2.0); + + TNAME v[3] = { aa[1], aa[2], aa[3] }; + TFN(s_normalize)(v, 3, v); + + q[1] = s * v[0]; + q[2] = s * v[1]; + q[3] = s * v[2]; +} + +static inline void TFN(s_quat_to_mat44)(const TNAME q[4], TNAME r[16]) +{ + TNAME w = q[0], x = q[1], y = q[2], z = q[3]; + + r[0] = w*w + x*x - y*y - z*z; + r[1] = 2*x*y - 2*w*z; + r[2] = 2*x*z + 2*w*y; + r[3] = 0; + + r[4] = 2*x*y + 2*w*z; + r[5] = w*w - x*x + y*y - z*z; + r[6] = 2*y*z - 2*w*x; + r[7] = 0; + + r[8] = 2*x*z - 2*w*y; + r[9] = 2*y*z + 2*w*x; + r[10] = w*w - x*x - y*y + z*z; + r[11] = 0; + + r[12] = 0; + r[13] = 0; + r[14] = 0; + r[15] = 1; +} + +/* Returns the skew-symmetric matrix V such that V*w = v x w (cross product). + Sometimes denoted [v]_x or \hat{v}. + [ 0 -v3 v2 + v3 0 -v1 + -v2 v1 0] + */ +static inline void TFN(s_cross_matrix)(const TNAME v[3], TNAME V[9]) +{ + V[0] = 0; + V[1] = -v[2]; + V[2] = v[1]; + V[3] = v[2]; + V[4] = 0; + V[5] = -v[0]; + V[6] = -v[1]; + V[7] = v[0]; + V[8] = 0; +} + +static inline void TFN(s_angleaxis_to_mat44)(const TNAME aa[4], TNAME r[16]) +{ + TNAME q[4]; + + TFN(s_angleaxis_to_quat)(aa, q); + TFN(s_quat_to_mat44)(q, r); +} + +static inline void TFN(s_quat_xyz_to_mat44)(const TNAME q[4], const TNAME xyz[3], TNAME r[16]) +{ + TFN(s_quat_to_mat44)(q, r); + + if (xyz != NULL) { + r[3] = xyz[0]; + r[7] = xyz[1]; + r[11] = xyz[2]; + } +} + +static inline void TFN(s_rpy_to_quat)(const TNAME rpy[3], TNAME quat[4]) +{ + TNAME roll = rpy[0], pitch = rpy[1], yaw = rpy[2]; + + TNAME halfroll = roll / 2; + TNAME halfpitch = pitch / 2; + TNAME halfyaw = yaw / 2; + + TNAME sin_r2 = (TNAME)sin(halfroll); + TNAME sin_p2 = (TNAME)sin(halfpitch); + TNAME sin_y2 = (TNAME)sin(halfyaw); + + TNAME cos_r2 = (TNAME)cos(halfroll); + TNAME cos_p2 = (TNAME)cos(halfpitch); + TNAME cos_y2 = (TNAME)cos(halfyaw); + + quat[0] = cos_r2 * cos_p2 * cos_y2 + sin_r2 * sin_p2 * sin_y2; + quat[1] = sin_r2 * cos_p2 * cos_y2 - cos_r2 * sin_p2 * sin_y2; + quat[2] = cos_r2 * sin_p2 * cos_y2 + sin_r2 * cos_p2 * sin_y2; + quat[3] = cos_r2 * cos_p2 * sin_y2 - sin_r2 * sin_p2 * cos_y2; +} + +// Reference: "A tutorial on SE(3) transformation parameterizations and +// on-manifold optimization" by Jose-Luis Blanco +static inline void TFN(s_quat_to_rpy)(const TNAME q[4], TNAME rpy[3]) +{ + const TNAME qr = q[0]; + const TNAME qx = q[1]; + const TNAME qy = q[2]; + const TNAME qz = q[3]; + + TNAME disc = qr*qy - qx*qz; + + if (fabs(disc+0.5) < DBL_EPSILON) { // near -1/2 + rpy[0] = 0; + rpy[1] = (TNAME)(-M_PI/2); + rpy[2] = (TNAME)(2 * atan2(qx, qr)); + } + else if (fabs(disc-0.5) < DBL_EPSILON) { // near 1/2 + rpy[0] = 0; + rpy[1] = (TNAME)(M_PI/2); + rpy[2] = (TNAME)(-2 * atan2(qx, qr)); + } + else { + // roll + TNAME roll_a = 2 * (qr*qx + qy*qz); + TNAME roll_b = 1 - 2 * (qx*qx + qy*qy); + rpy[0] = (TNAME)atan2(roll_a, roll_b); + + // pitch + rpy[1] = (TNAME)asin(2*disc); + + // yaw + TNAME yaw_a = 2 * (qr*qz + qx*qy); + TNAME yaw_b = 1 - 2 * (qy*qy + qz*qz); + rpy[2] = (TNAME)atan2(yaw_a, yaw_b); + } +} + +static inline void TFN(s_rpy_to_mat44)(const TNAME rpy[3], TNAME M[16]) +{ + TNAME q[4]; + TFN(s_rpy_to_quat)(rpy, q); + TFN(s_quat_to_mat44)(q, M); +} + + +static inline void TFN(s_xyzrpy_to_mat44)(const TNAME xyzrpy[6], TNAME M[16]) +{ + TFN(s_rpy_to_mat44)(&xyzrpy[3], M); + M[3] = xyzrpy[0]; + M[7] = xyzrpy[1]; + M[11] = xyzrpy[2]; +} + +static inline void TFN(s_mat44_transform_xyz)(const TNAME M[16], const TNAME in[3], TNAME out[3]) +{ + for (int i = 0; i < 3; i++) + out[i] = M[4*i + 0]*in[0] + M[4*i + 1]*in[1] + M[4*i + 2]*in[2] + M[4*i + 3]; +} + +// out = (upper 3x3 of M) * in +static inline void TFN(s_mat44_rotate_vector)(const TNAME M[16], const TNAME in[3], TNAME out[3]) +{ + for (int i = 0; i < 3; i++) + out[i] = M[4*i + 0]*in[0] + M[4*i + 1]*in[1] + M[4*i + 2]*in[2]; +} + +static inline void TFN(s_mat44_to_xyt)(const TNAME M[16], TNAME xyt[3]) +{ + // c -s + // s c + xyt[0] = M[3]; + xyt[1] = M[7]; + xyt[2] = (TNAME)atan2(M[4], M[0]); +} + +static inline void TFN(s_mat_to_xyz)(const TNAME M[16], TNAME xyz[3]) +{ + xyz[0] = M[3]; + xyz[1] = M[7]; + xyz[2] = M[11]; +} + +static inline void TFN(s_mat_to_quat)(const TNAME M[16], TNAME q[4]) +{ + double T = M[0] + M[5] + M[10] + 1.0; + double S; + + if (T > 0.0000001) { + S = sqrt(T) * 2; + q[0] = (TNAME)(0.25 * S); + q[1] = (TNAME)((M[9] - M[6]) / S); + q[2] = (TNAME)((M[2] - M[8]) / S); + q[3] = (TNAME)((M[4] - M[1]) / S); + } else if (M[0] > M[5] && M[0] > M[10]) { // Column 0: + S = sqrt(1.0 + M[0] - M[5] - M[10]) * 2; + q[0] = (TNAME)((M[9] - M[6]) / S); + q[1] = (TNAME)(0.25 * S); + q[2] = (TNAME)((M[4] + M[1]) / S); + q[3] = (TNAME)((M[2] + M[8]) / S); + } else if (M[5] > M[10]) { // Column 1: + S = sqrt(1.0 + M[5] - M[0] - M[10]) * 2; + q[0] = (TNAME)((M[2] - M[8]) / S); + q[1] = (TNAME)((M[4] + M[1]) / S); + q[2] = (TNAME)(0.25 * S); + q[3] = (TNAME)((M[9] + M[6]) / S); + } else { // Column 2: + S = sqrt(1.0 + M[10] - M[0] - M[5]); + q[0] = (TNAME)((M[4] - M[1]) / S); + q[1] = (TNAME)((M[2] + M[8]) / S); + q[2] = (TNAME)((M[9] + M[6]) / S); + q[3] = (TNAME)(0.25 * S); + } + + TFN(s_normalize)(q, 4, q); +} + +static inline void TFN(s_quat_xyz_to_xyt)(const TNAME q[4], const TNAME xyz[3], TNAME xyt[3]) +{ + TNAME M[16]; + TFN(s_quat_xyz_to_mat44)(q, xyz, M); + TFN(s_mat44_to_xyt)(M, xyt); +} + +// xytr = xyta * xytb; +static inline void TFN(s_xyt_mul)(const TNAME xyta[3], const TNAME xytb[3], TNAME xytr[3]) +{ + TNAME xa = xyta[0], ya = xyta[1], ta = xyta[2]; + TNAME s = (TNAME)sin(ta), c = (TNAME)cos(ta); + + xytr[0] = c*xytb[0] - s*xytb[1] + xa; + xytr[1] = s*xytb[0] + c*xytb[1] + ya; + xytr[2] = ta + xytb[2]; +} + +static inline void TFN(s_xytcov_copy)(const TNAME xyta[3], const TNAME Ca[9], + TNAME xytr[3], TNAME Cr[9]) +{ + memcpy(xytr, xyta, 3 * sizeof(TNAME)); + memcpy(Cr, Ca, 9 * sizeof(TNAME)); +} + +static inline void TFN(s_xytcov_mul)(const TNAME xyta[3], const TNAME Ca[9], + const TNAME xytb[3], const TNAME Cb[9], + TNAME xytr[3], TNAME Cr[9]) +{ + TNAME xa = xyta[0], ya = xyta[1], ta = xyta[2]; + TNAME xb = xytb[0], yb = xytb[1]; + + TNAME sa = (TNAME)sin(ta), ca = (TNAME)cos(ta); + + TNAME P11 = Ca[0], P12 = Ca[1], P13 = Ca[2]; + TNAME P22 = Ca[4], P23 = Ca[5]; + TNAME P33 = Ca[8]; + + TNAME Q11 = Cb[0], Q12 = Cb[1], Q13 = Cb[2]; + TNAME Q22 = Cb[4], Q23 = Cb[5]; + TNAME Q33 = Cb[8]; + + TNAME JA13 = -sa*xb - ca*yb; + TNAME JA23 = ca*xb - sa*yb; + TNAME JB11 = ca; + TNAME JB12 = -sa; + TNAME JB21 = sa; + TNAME JB22 = ca; + + Cr[0] = P33*JA13*JA13 + 2*P13*JA13 + Q11*JB11*JB11 + 2*Q12*JB11*JB12 + Q22*JB12*JB12 + P11; + Cr[1] = P12 + JA23*(P13 + JA13*P33) + JA13*P23 + JB21*(JB11*Q11 + JB12*Q12) + JB22*(JB11*Q12 + JB12*Q22); + Cr[2] = P13 + JA13*P33 + JB11*Q13 + JB12*Q23; + Cr[3] = Cr[1]; + Cr[4] = P33*JA23*JA23 + 2*P23*JA23 + Q11*JB21*JB21 + 2*Q12*JB21*JB22 + Q22*JB22*JB22 + P22; + Cr[5] = P23 + JA23*P33 + JB21*Q13 + JB22*Q23; + Cr[6] = Cr[2]; + Cr[7] = Cr[5]; + Cr[8] = P33 + Q33; + + xytr[0] = ca*xb - sa*yb + xa; + xytr[1] = sa*xb + ca*yb + ya; + xytr[2] = xyta[2] + xytb[2]; + +/* + // the code above is just an unrolling of the following: + + TNAME JA[][] = new TNAME[][] { { 1, 0, -sa*xb - ca*yb }, + { 0, 1, ca*xb - sa*yb }, + { 0, 0, 1 } }; + TNAME JB[][] = new TNAME[][] { { ca, -sa, 0 }, + { sa, ca, 0 }, + { 0, 0, 1 } }; + + newge.P = LinAlg.add(LinAlg.matrixABCt(JA, P, JA), + LinAlg.matrixABCt(JB, ge.P, JB)); +*/ +} + + +static inline void TFN(s_xyt_inv)(const TNAME xyta[3], TNAME xytr[3]) +{ + TNAME s = (TNAME)sin(xyta[2]), c = (TNAME)cos(xyta[2]); + xytr[0] = -s*xyta[1] - c*xyta[0]; + xytr[1] = -c*xyta[1] + s*xyta[0]; + xytr[2] = -xyta[2]; +} + +static inline void TFN(s_xytcov_inv)(const TNAME xyta[3], const TNAME Ca[9], + TNAME xytr[3], TNAME Cr[9]) +{ + TNAME x = xyta[0], y = xyta[1], theta = xyta[2]; + TNAME s = (TNAME)sin(theta), c = (TNAME)cos(theta); + + TNAME J11 = -c, J12 = -s, J13 = -c*y + s*x; + TNAME J21 = s, J22 = -c, J23 = s*y + c*x; + + TNAME P11 = Ca[0], P12 = Ca[1], P13 = Ca[2]; + TNAME P22 = Ca[4], P23 = Ca[5]; + TNAME P33 = Ca[8]; + + Cr[0] = P11*J11*J11 + 2*P12*J11*J12 + 2*P13*J11*J13 + + P22*J12*J12 + 2*P23*J12*J13 + P33*J13*J13; + Cr[1] = J21*(J11*P11 + J12*P12 + J13*P13) + + J22*(J11*P12 + J12*P22 + J13*P23) + + J23*(J11*P13 + J12*P23 + J13*P33); + Cr[2] = - J11*P13 - J12*P23 - J13*P33; + Cr[3] = Cr[1]; + Cr[4] = P11*J21*J21 + 2*P12*J21*J22 + 2*P13*J21*J23 + + P22*J22*J22 + 2*P23*J22*J23 + P33*J23*J23; + Cr[5] = - J21*P13 - J22*P23 - J23*P33; + Cr[6] = Cr[2]; + Cr[7] = Cr[5]; + Cr[8] = P33; + + /* + // the code above is just an unrolling of the following: + + TNAME J[][] = new TNAME[][] { { -c, -s, -c*y + s*x }, + { s, -c, s*y + c*x }, + { 0, 0, -1 } }; + ge.P = LinAlg.matrixABCt(J, P, J); + */ + + xytr[0] = -s*y - c*x; + xytr[1] = -c*y + s*x; + xytr[2] = -xyta[2]; +} + +// xytr = inv(xyta) * xytb +static inline void TFN(s_xyt_inv_mul)(const TNAME xyta[3], const TNAME xytb[3], TNAME xytr[3]) +{ + TNAME theta = xyta[2]; + TNAME ca = (TNAME)cos(theta); + TNAME sa = (TNAME)sin(theta); + TNAME dx = xytb[0] - xyta[0]; + TNAME dy = xytb[1] - xyta[1]; + + xytr[0] = ca*dx + sa*dy; + xytr[1] = -sa*dx + ca*dy; + xytr[2]= xytb[2] - xyta[2]; +} + +static inline void TFN(s_mat_add)(const TNAME *A, int Arows, int Acols, + const TNAME *B, int Brows, int Bcols, + TNAME *R, int Rrows, int Rcols) +{ + assert(Arows == Brows); + assert(Arows == Rrows); + assert(Bcols == Bcols); + assert(Bcols == Rcols); + + for (int i = 0; i < Arows; i++) + for (int j = 0; j < Bcols; j++) + R[i*Acols + j] = A[i*Acols + j] + B[i*Acols + j]; +} + +// matrix should be in row-major order, allocated in a single packed +// array. (This is compatible with matd.) +static inline void TFN(s_mat_AB)(const TNAME *A, int Arows, int Acols, + const TNAME *B, int Brows, int Bcols, + TNAME *R, int Rrows, int Rcols) +{ + assert(Acols == Brows); + assert(Rrows == Arows); + assert(Bcols == Rcols); + + for (int Rrow = 0; Rrow < Rrows; Rrow++) { + for (int Rcol = 0; Rcol < Rcols; Rcol++) { + TNAME acc = 0; + for (int i = 0; i < Acols; i++) + acc += A[Rrow*Acols + i] * B[i*Bcols + Rcol]; + R[Rrow*Rcols + Rcol] = acc; + } + } +} + +// matrix should be in row-major order, allocated in a single packed +// array. (This is compatible with matd.) +static inline void TFN(s_mat_ABt)(const TNAME *A, int Arows, int Acols, + const TNAME *B, int Brows, int Bcols, + TNAME *R, int Rrows, int Rcols) +{ + assert(Acols == Bcols); + assert(Rrows == Arows); + assert(Brows == Rcols); + + for (int Rrow = 0; Rrow < Rrows; Rrow++) { + for (int Rcol = 0; Rcol < Rcols; Rcol++) { + TNAME acc = 0; + for (int i = 0; i < Acols; i++) + acc += A[Rrow*Acols + i] * B[Rcol*Bcols + i]; + R[Rrow*Rcols + Rcol] = acc; + } + } +} + +static inline void TFN(s_mat_ABC)(const TNAME *A, int Arows, int Acols, + const TNAME *B, int Brows, int Bcols, + const TNAME *C, int Crows, int Ccols, + TNAME *R, int Rrows, int Rcols) +{ + TNAME *tmp = malloc(sizeof(TNAME)*Arows*Bcols); + + TFN(s_mat_AB)(A, Arows, Acols, B, Brows, Bcols, tmp, Arows, Bcols); + TFN(s_mat_AB)(tmp, Arows, Bcols, C, Crows, Ccols, R, Rrows, Rcols); + free(tmp); +} + +static inline void TFN(s_mat_Ab)(const TNAME *A, int Arows, int Acols, + const TNAME *B, int Blength, + TNAME *R, int Rlength) +{ + assert(Acols == Blength); + assert(Arows == Rlength); + + for (int Ridx = 0; Ridx < Rlength; Ridx++) { + TNAME acc = 0; + for (int i = 0; i < Blength; i++) + acc += A[Ridx*Acols + i] * B[i]; + R[Ridx] = acc; + } +} + +static inline void TFN(s_mat_AtB)(const TNAME *A, int Arows, int Acols, + const TNAME *B, int Brows, int Bcols, + TNAME *R, int Rrows, int Rcols) +{ + assert(Arows == Brows); + assert(Rrows == Acols); + assert(Bcols == Rcols); + + for (int Rrow = 0; Rrow < Rrows; Rrow++) { + for (int Rcol = 0; Rcol < Rcols; Rcol++) { + TNAME acc = 0; + for (int i = 0; i < Acols; i++) + acc += A[i*Acols + Rrow] * B[i*Bcols + Rcol]; + R[Rrow*Rcols + Rcol] = acc; + } + } +} + +static inline void TFN(s_quat_slerp)(const TNAME q0[4], const TNAME _q1[4], TNAME r[4], TNAME w) +{ + TNAME dot = TFN(s_dot)(q0, _q1, 4); + + TNAME q1[4]; + memcpy(q1, _q1, sizeof(TNAME) * 4); + + if (dot < 0) { + // flip sign on one of them so we don't spin the "wrong + // way" around. This doesn't change the rotation that the + // quaternion represents. + dot = -dot; + for (int i = 0; i < 4; i++) + q1[i] *= -1; + } + + // if large dot product (1), slerp will scale both q0 and q1 + // by 0, and normalization will blow up. + if (dot > 0.95) { + + for (int i = 0; i < 4; i++) + r[i] = q0[i]*(1-w) + q1[i]*w; + + } else { + TNAME angle = (TNAME)acos(dot); + + TNAME w0 = (TNAME)sin(angle*(1-w)), w1 = (TNAME)sin(angle*w); + + for (int i = 0; i < 4; i++) + r[i] = q0[i]*w0 + q1[i]*w1; + + TFN(s_normalize)(r, 4, r); + } +} + +static inline void TFN(s_cross_product)(const TNAME v1[3], const TNAME v2[3], TNAME r[3]) +{ + r[0] = v1[1]*v2[2] - v1[2]*v2[1]; + r[1] = v1[2]*v2[0] - v1[0]*v2[2]; + r[2] = v1[0]*v2[1] - v1[1]*v2[0]; +} + +//////////////////// +static inline void TFN(s_mat44_identity)(TNAME out[16]) +{ + memset(out, 0, 16 * sizeof(TNAME)); + out[0] = 1; + out[5] = 1; + out[10] = 1; + out[15] = 1; +} + +static inline void TFN(s_mat44_translate)(const TNAME txyz[3], TNAME out[16]) +{ + TFN(s_mat44_identity)(out); + + for (int i = 0; i < 3; i++) + out[4*i + 3] += txyz[i]; +} + +static inline void TFN(s_mat44_scale)(const TNAME sxyz[3], TNAME out[16]) +{ + TFN(s_mat44_identity)(out); + + for (int i = 0; i < 3; i++) + out[4*i + i] = sxyz[i]; +} + +static inline void TFN(s_mat44_rotate_z)(TNAME rad, TNAME out[16]) +{ + TFN(s_mat44_identity)(out); + TNAME s = (TNAME)sin(rad), c = (TNAME)cos(rad); + out[0*4 + 0] = c; + out[0*4 + 1] = -s; + out[1*4 + 0] = s; + out[1*4 + 1] = c; +} + +static inline void TFN(s_mat44_rotate_y)(TNAME rad, TNAME out[16]) +{ + TFN(s_mat44_identity)(out); + TNAME s = (TNAME)sin(rad), c = (TNAME)cos(rad); + out[0*4 + 0] = c; + out[0*4 + 2] = s; + out[2*4 + 0] = -s; + out[2*4 + 2] = c; +} + +static inline void TFN(s_mat44_rotate_x)(TNAME rad, TNAME out[16]) +{ + TFN(s_mat44_identity)(out); + TNAME s = (TNAME)sin(rad), c = (TNAME)cos(rad); + out[1*4 + 1] = c; + out[1*4 + 2] = -s; + out[2*4 + 1] = s; + out[2*4 + 2] = c; +} + +// out = out * translate(txyz) +static inline void TFN(s_mat44_translate_self)(const TNAME txyz[3], TNAME out[16]) +{ + TNAME tmp[16], prod[16]; + TFN(s_mat44_translate(txyz, tmp)); + TFN(s_mat_AB)(out, 4, 4, tmp, 4, 4, prod, 4, 4); + memcpy(out, prod, sizeof(TNAME)*16); +} + +static inline void TFN(s_mat44_scale_self)(const TNAME sxyz[3], TNAME out[16]) +{ + TNAME tmp[16], prod[16]; + TFN(s_mat44_scale(sxyz, tmp)); + TFN(s_mat_AB)(out, 4, 4, tmp, 4, 4, prod, 4, 4); + memcpy(out, prod, sizeof(TNAME)*16); +} + +static inline void TFN(s_mat44_rotate_z_self)(TNAME rad, TNAME out[16]) +{ + TNAME tmp[16], prod[16]; + TFN(s_mat44_rotate_z(rad, tmp)); + TFN(s_mat_AB)(out, 4, 4, tmp, 4, 4, prod, 4, 4); + memcpy(out, prod, sizeof(TNAME)*16); +} + +// out = inv(M)*in. Note: this assumes that mat44 is a rigid-body transformation. +static inline void TFN(s_mat44_inv)(const TNAME M[16], TNAME out[16]) +{ +// NB: M = T*R, inv(M) = inv(R) * inv(T) + + // transpose of upper-left corner + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + out[4*i + j] = M[4*j + i]; + + out[4*0 + 3] = 0; + out[4*1 + 3] = 0; + out[4*2 + 3] = 0; + + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + out[4*i + 3] -= out[4*i + j] * M[4*j + 3]; + + out[4*3 + 0] = 0; + out[4*3 + 1] = 0; + out[4*3 + 2] = 0; + out[4*3 + 3] = 1; + +/* TNAME tmp[16]; + TFN(s_mat_AB)(M, 4, 4, out, 4, 4, tmp, 4, 4); + printf("identity: "); + TFN(s_print_mat)(tmp, 4, 4, "%15f"); */ +} + +// out = inv(M)*in +static inline void TFN(s_mat44_inv_transform_xyz)(const TNAME M[16], const TNAME in[3], TNAME out[3]) +{ + TNAME T[16]; + TFN(s_mat44_inv)(M, T); + + TFN(s_mat44_transform_xyz)(T, in, out); +} + +// out = (upper 3x3 of inv(M)) * in +static inline void TFN(s_mat44_inv_rotate_vector)(const TNAME M[16], const TNAME in[3], TNAME out[3]) +{ + TNAME T[16]; + TFN(s_mat44_inv)(M, T); + + TFN(s_mat44_rotate_vector)(T, in, out); +} + +static inline void TFN(s_elu_to_mat44)(const TNAME eye[3], const TNAME lookat[3], const TNAME _up[3], + TNAME M[16]) +{ + TNAME f[3]; + TFN(s_subtract)(lookat, eye, 3, f); + TFN(s_normalize)(f, 3, f); + + TNAME up[3]; + + // remove any component of 'up' that isn't perpendicular to the look direction. + TFN(s_normalize)(_up, 3, up); + + TNAME up_dot = TFN(s_dot)(f, up, 3); + for (int i = 0; i < 3; i++) + up[i] -= up_dot*f[i]; + + TFN(s_normalize_self)(up, 3); + + TNAME s[3], u[3]; + TFN(s_cross_product)(f, up, s); + TFN(s_cross_product)(s, f, u); + + TNAME R[16] = { s[0], s[1], s[2], 0, + u[0], u[1], u[2], 0, + -f[0], -f[1], -f[2], 0, + 0, 0, 0, 1}; + + TNAME T[16] = {1, 0, 0, -eye[0], + 0, 1, 0, -eye[1], + 0, 0, 1, -eye[2], + 0, 0, 0, 1}; + + // M is the extrinsics matrix [R | t] where t = -R*c + TNAME tmp[16]; + TFN(s_mat_AB)(R, 4, 4, T, 4, 4, tmp, 4, 4); + TFN(s_mat44_inv)(tmp, M); +} + +// Computes the cholesky factorization of A, putting the lower +// triangular matrix into R. +static inline void TFN(s_mat33_chol)(const TNAME *A, int Arows, int Acols, + TNAME *R, int Brows, int Bcols) +{ + assert(Arows == Brows); + assert(Bcols == Bcols); + + // A[0] = R[0]*R[0] + R[0] = (TNAME)sqrt(A[0]); + + // A[1] = R[0]*R[3]; + R[3] = A[1] / R[0]; + + // A[2] = R[0]*R[6]; + R[6] = A[2] / R[0]; + + // A[4] = R[3]*R[3] + R[4]*R[4] + R[4] = (TNAME)sqrt(A[4] - R[3]*R[3]); + + // A[5] = R[3]*R[6] + R[4]*R[7] + R[7] = (A[5] - R[3]*R[6]) / R[4]; + + // A[8] = R[6]*R[6] + R[7]*R[7] + R[8]*R[8] + R[8] = (TNAME)sqrt(A[8] - R[6]*R[6] - R[7]*R[7]); + + R[1] = 0; + R[2] = 0; + R[5] = 0; +} + +static inline void TFN(s_mat33_lower_tri_inv)(const TNAME *A, int Arows, int Acols, + TNAME *R, int Rrows, int Rcols) +{ + // A[0]*R[0] = 1 + R[0] = 1 / A[0]; + + // A[3]*R[0] + A[4]*R[3] = 0 + R[3] = -A[3]*R[0] / A[4]; + + // A[4]*R[4] = 1 + R[4] = 1 / A[4]; + + // A[6]*R[0] + A[7]*R[3] + A[8]*R[6] = 0 + R[6] = (-A[6]*R[0] - A[7]*R[3]) / A[8]; + + // A[7]*R[4] + A[8]*R[7] = 0 + R[7] = -A[7]*R[4] / A[8]; + + // A[8]*R[8] = 1 + R[8] = 1 / A[8]; +} + + +static inline void TFN(s_mat33_sym_solve)(const TNAME *A, int Arows, int Acols, + const TNAME *B, int Brows, int Bcols, + TNAME *R, int Rrows, int Rcols) +{ + assert(Arows == Acols); + assert(Acols == 3); + assert(Brows == 3); + assert(Bcols == 1); + assert(Rrows == 3); + assert(Rcols == 1); + + TNAME L[9]; + TFN(s_mat33_chol)(A, 3, 3, L, 3, 3); + + TNAME M[9]; + TFN(s_mat33_lower_tri_inv)(L, 3, 3, M, 3, 3); + + double tmp[3]; + tmp[0] = M[0]*B[0]; + tmp[1] = M[3]*B[0] + M[4]*B[1]; + tmp[2] = M[6]*B[0] + M[7]*B[1] + M[8]*B[2]; + + R[0] = (TNAME)(M[0]*tmp[0] + M[3]*tmp[1] + M[6]*tmp[2]); + R[1] = (TNAME)(M[4]*tmp[1] + M[7]*tmp[2]); + R[2] = (TNAME)(M[8]*tmp[2]); +} + +/* +// solve Ax = B. Assumes A is symmetric; uses cholesky factorization +static inline void TFN(s_mat_solve_chol)(const TNAME *A, int Arows, int Acols, + const TNAME *B, int Brows, int Bcols, + TNAME *R, int Rrows, int Rcols) +{ + assert(Arows == Acols); + assert(Arows == Brows); + assert(Acols == Rrows); + assert(Bcols == Rcols); + + // +} +*/ +#undef TRRFN +#undef TRFN +#undef TFN diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/floats.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/floats.h new file mode 100644 index 0000000000..22f839fa9b --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/floats.h @@ -0,0 +1,32 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#define TNAME float +#include "doubles_floats_impl.h" +#undef TNAME diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/g2d.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/g2d.h new file mode 100644 index 0000000000..5a0dd0b598 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/g2d.h @@ -0,0 +1,124 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "zarray.h" + +// This library tries to avoid needless proliferation of types. +// +// A point is a double[2]. (Note that when passing a double[2] as an +// argument, it is passed by pointer, not by value.) +// +// A polygon is a zarray_t of double[2]. (Note that in this case, the +// zarray contains the actual vertex data, and not merely a pointer to +// some other data. IMPORTANT: A polygon must be specified in CCW +// order. It is implicitly closed (do not list the same point at the +// beginning at the end. +// +// Where sensible, it is assumed that objects should be allocated +// sparingly; consequently "init" style methods, rather than "create" +// methods are used. + +//////////////////////////////////////////////////////////////////// +// Lines + +typedef struct +{ + // Internal representation: a point that the line goes through (p) and + // the direction of the line (u). + double p[2]; + double u[2]; // always a unit vector +} g2d_line_t; + +// initialize a line object. +void g2d_line_init_from_points(g2d_line_t *line, const double p0[2], const double p1[2]); + +// The line defines a one-dimensional coordinate system whose origin +// is p. Where is q? (If q is not on the line, the point nearest q is +// returned. +double g2d_line_get_coordinate(const g2d_line_t *line, const double q[2]); + +// Intersect two lines. The intersection, if it exists, is written to +// p (if not NULL), and 1 is returned. Else, zero is returned. +int g2d_line_intersect_line(const g2d_line_t *linea, const g2d_line_t *lineb, double *p); + +//////////////////////////////////////////////////////////////////// +// Line Segments. line.p is always one endpoint; p1 is the other +// endpoint. +typedef struct +{ + g2d_line_t line; + double p1[2]; +} g2d_line_segment_t; + +void g2d_line_segment_init_from_points(g2d_line_segment_t *seg, const double p0[2], const double p1[2]); + +// Intersect two segments. The intersection, if it exists, is written +// to p (if not NULL), and 1 is returned. Else, zero is returned. +int g2d_line_segment_intersect_segment(const g2d_line_segment_t *sega, const g2d_line_segment_t *segb, double *p); + +void g2d_line_segment_closest_point(const g2d_line_segment_t *seg, const double *q, double *p); +double g2d_line_segment_closest_point_distance(const g2d_line_segment_t *seg, const double *q); + +//////////////////////////////////////////////////////////////////// +// Polygons + +zarray_t *g2d_polygon_create_data(double v[][2], int sz); + +zarray_t *g2d_polygon_create_zeros(int sz); + +zarray_t *g2d_polygon_create_empty(void); + +void g2d_polygon_add(zarray_t *poly, double v[2]); + +// Takes a polygon in either CW or CCW and modifies it (if necessary) +// to be CCW. +void g2d_polygon_make_ccw(zarray_t *poly); + +// Return 1 if point q lies within poly. +int g2d_polygon_contains_point(const zarray_t *poly, double q[2]); + +// Do the edges of the polygons cross? (Does not test for containment). +int g2d_polygon_intersects_polygon(const zarray_t *polya, const zarray_t *polyb); + +// Does polya completely contain polyb? +int g2d_polygon_contains_polygon(const zarray_t *polya, const zarray_t *polyb); + +// Is there some point which is in both polya and polyb? +int g2d_polygon_overlaps_polygon(const zarray_t *polya, const zarray_t *polyb); + +// returns the number of points written to x. see comments. +int g2d_polygon_rasterize(const zarray_t *poly, double y, double *x); + +#ifdef __cplusplus +} +#endif diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/homography.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/homography.h new file mode 100644 index 0000000000..b1560c3729 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/homography.h @@ -0,0 +1,183 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#include "matd.h" +#include "zarray.h" + +#ifdef __cplusplus +extern "C" { +#endif + + /** Given a 3x3 homography matrix and the focal lengths of the + * camera, compute the pose of the tag. The focal lengths should + * be given in pixels. For example, if the camera's focal length + * is twice the width of the sensor, and the sensor is 600 pixels + * across, the focal length in pixels is 2*600. Note that the + * focal lengths in the fx and fy direction will be approximately + * equal for most lenses, and is not a function of aspect ratio. + * + * Theory: The homography matrix is the product of the camera + * projection matrix and the tag's pose matrix (the matrix that + * projects points from the tag's local coordinate system to the + * camera's coordinate frame). + * + * [ h00 h01 h02 h03] = [ fx 0 cx 0 ] [ R00 R01 R02 TX ] + * [ h10 h11 h12 h13] = [ 0 fy cy 0 ] [ R10 R11 R12 TY ] + * [ h20 h21 h22 h23] = [ 0 0 s 0 ] [ R20 R21 R22 TZ ] + * [ 0 0 0 1 ] + * + * fx is the focal length in the x direction of the camera + * (typically measured in pixels), fy is the focal length. cx and + * cy give the focal center (usually the middle of the image), and + * s is either +1 or -1, depending on the conventions you use. (We + * use 1.) + + * When observing a tag, the points we project in world space all + * have z=0, so we can form a 3x3 matrix by eliminating the 3rd + * column of the pose matrix. + * + * [ h00 h01 h02 ] = [ fx 0 cx 0 ] [ R00 R01 TX ] + * [ h10 h11 h12 ] = [ 0 fy cy 0 ] [ R10 R11 TY ] + * [ h20 h21 h22 ] = [ 0 0 s 0 ] [ R20 R21 TZ ] + * [ 0 0 1 ] + * + * (note that these h's are different from the ones above.) + * + * We can multiply the right-hand side to yield a set of equations + * relating the values of h to the values of the pose matrix. + * + * There are two wrinkles. The first is that the homography matrix + * is known only up to scale. We recover the unknown scale by + * constraining the magnitude of the first two columns of the pose + * matrix to be 1. We use the geometric average scale. The sign of + * the scale factor is recovered by constraining the observed tag + * to be in front of the camera. Once scaled, we recover the first + * two colmuns of the rotation matrix. The third column is the + * cross product of these. + * + * The second wrinkle is that the computed rotation matrix might + * not be exactly orthogonal, so we perform a polar decomposition + * to find a good pure rotation approximation. + * + * Tagsize is the size of the tag in your desired units. I.e., if + * your tag measures 0.25m along the side, your tag size is + * 0.25. (The homography is computed in terms of *half* the tag + * size, i.e., that a tag is 2 units wide as it spans from -1 to + * +1, but this code makes the appropriate adjustment.) + * + * A note on signs: + * + * The code below incorporates no additional negative signs, but + * respects the sign of any parameters that you pass in. Flipping + * the signs allows you to modify the projection to suit a wide + * variety of conditions. + * + * In the "pure geometry" projection matrix, the image appears + * upside down; i.e., the x and y coordinates on the left hand + * side are the opposite of those on the right of the camera + * projection matrix. This would happen for all parameters + * positive: recall that points in front of the camera have + * negative Z values, which will cause the sign of all points to + * flip. + * + * However, most cameras flip things so that the image appears + * "right side up" as though you were looking through the lens + * directly. This means that the projected points should have the + * same sign as the points on the right of the camera projection + * matrix. To achieve this, flip fx and fy. + * + * One further complication: cameras typically put y=0 at the top + * of the image, instead of the bottom. Thus you generally want to + * flip y yet again (so it's now positive again). + * + * General advice: you probably want fx negative, fy positive, cx + * and cy positive, and s=1. + **/ + +// correspondences is a list of float[4]s, consisting of the points x +// and y concatenated. We will compute a homography such that y = Hx +// Specifically, float [] { a, b, c, d } where x = [a b], y = [c d]. + + +#define HOMOGRAPHY_COMPUTE_FLAG_INVERSE 1 +#define HOMOGRAPHY_COMPUTE_FLAG_SVD 0 + +matd_t *homography_compute(zarray_t *correspondences, int flags); + +//void homography_project(const matd_t *H, double x, double y, double *ox, double *oy); +static inline void homography_project(const matd_t *H, double x, double y, double *ox, double *oy) +{ + double xx = MATD_EL(H, 0, 0)*x + MATD_EL(H, 0, 1)*y + MATD_EL(H, 0, 2); + double yy = MATD_EL(H, 1, 0)*x + MATD_EL(H, 1, 1)*y + MATD_EL(H, 1, 2); + double zz = MATD_EL(H, 2, 0)*x + MATD_EL(H, 2, 1)*y + MATD_EL(H, 2, 2); + + *ox = xx / zz; + *oy = yy / zz; +} + +// assuming that the projection matrix is: +// [ fx 0 cx 0 ] +// [ 0 fy cy 0 ] +// [ 0 0 1 0 ] +// +// And that the homography is equal to the projection matrix times the model matrix, +// recover the model matrix (which is returned). Note that the third column of the model +// matrix is missing in the expression below, reflecting the fact that the homography assumes +// all points are at z=0 (i.e., planar) and that the element of z is thus omitted. +// (3x1 instead of 4x1). +// +// [ fx 0 cx 0 ] [ R00 R01 TX ] [ H00 H01 H02 ] +// [ 0 fy cy 0 ] [ R10 R11 TY ] = [ H10 H11 H12 ] +// [ 0 0 1 0 ] [ R20 R21 TZ ] = [ H20 H21 H22 ] +// [ 0 0 1 ] +// +// fx*R00 + cx*R20 = H00 (note, H only known up to scale; some additional adjustments required; see code.) +// fx*R01 + cx*R21 = H01 +// fx*TX + cx*TZ = H02 +// fy*R10 + cy*R20 = H10 +// fy*R11 + cy*R21 = H11 +// fy*TY + cy*TZ = H12 +// R20 = H20 +// R21 = H21 +// TZ = H22 +matd_t *homography_to_pose(const matd_t *H, double fx, double fy, double cx, double cy); + +// Similar to above +// Recover the model view matrix assuming that the projection matrix is: +// +// [ F 0 A 0 ] (see glFrustrum) +// [ 0 G B 0 ] +// [ 0 0 C D ] +// [ 0 0 -1 0 ] + +matd_t *homography_to_model_view(const matd_t *H, double F, double G, double A, double B, double C, double D); + +#ifdef __cplusplus +} +#endif diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/image_types.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/image_types.h new file mode 100644 index 0000000000..58e5258de3 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/image_types.h @@ -0,0 +1,84 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#include + +// to support conversions between different types, we define all image +// types at once. Type-specific implementations can then #include this +// file, assured that the basic types of each image are known. + +typedef struct image_u8 image_u8_t; +struct image_u8 +{ + const int32_t width; + const int32_t height; + const int32_t stride; + + uint8_t *buf; +}; + +typedef struct image_u8x3 image_u8x3_t; +struct image_u8x3 +{ + const int32_t width; + const int32_t height; + const int32_t stride; // bytes per line + + uint8_t *buf; +}; + +typedef struct image_u8x4 image_u8x4_t; +struct image_u8x4 +{ + const int32_t width; + const int32_t height; + const int32_t stride; // bytes per line + + uint8_t *buf; +}; + +typedef struct image_f32 image_f32_t; +struct image_f32 +{ + const int32_t width; + const int32_t height; + const int32_t stride; // floats per line + + float *buf; // indexed as buf[y*stride + x] +}; + +typedef struct image_u32 image_u32_t; +struct image_u32 +{ + const int32_t width; + const int32_t height; + const int32_t stride; // int32_ts per line + + uint32_t *buf; // indexed as buf[y*stride + x] +}; diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/image_u8.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/image_u8.h new file mode 100644 index 0000000000..a0e151f916 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/image_u8.h @@ -0,0 +1,90 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#include +#include "image_types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct image_u8_lut image_u8_lut_t; +struct image_u8_lut +{ + // When drawing, we compute the squared distance between a given pixel and a filled region. + // int idx = squared_distance * scale; + // We then index into values[idx] to obtain the color. (If we must index beyond nvalues, + // no drawing is performed.) + float scale; + + int nvalues; + uint8_t *values; +}; + + +// Create or load an image. returns NULL on failure. Uses default +// stride alignment. +image_u8_t *image_u8_create_stride(unsigned int width, unsigned int height, unsigned int stride); +image_u8_t *image_u8_create(unsigned int width, unsigned int height); +image_u8_t *image_u8_create_alignment(unsigned int width, unsigned int height, unsigned int alignment); +image_u8_t *image_u8_create_from_f32(image_f32_t *fim); + +image_u8_t *image_u8_create_from_pnm(const char *path); + image_u8_t *image_u8_create_from_pnm_alignment(const char *path, int alignment); + +image_u8_t *image_u8_copy(const image_u8_t *in); +void image_u8_draw_line(image_u8_t *im, float x0, float y0, float x1, float y1, int v, int width); +void image_u8_draw_circle(image_u8_t *im, float x0, float y0, float r, int v); +void image_u8_draw_annulus(image_u8_t *im, float x0, float y0, float r0, float r1, int v); + +void image_u8_fill_line_max(image_u8_t *im, const image_u8_lut_t *lut, const float *xy0, const float *xy1); + +void image_u8_clear(image_u8_t *im); +void image_u8_darken(image_u8_t *im); +void image_u8_convolve_2D(image_u8_t *im, const uint8_t *k, int ksz); +void image_u8_gaussian_blur(image_u8_t *im, double sigma, int k); + +// 1.5, 2, 3, 4, ... supported +image_u8_t *image_u8_decimate(image_u8_t *im, float factor); + +void image_u8_destroy(image_u8_t *im); + +// Write a pnm. Returns 0 on success +// Currently only supports GRAY and RGBA. Does not write out alpha for RGBA +int image_u8_write_pnm(const image_u8_t *im, const char *path); + +// rotate the image by 'rad' radians. (Rotated in the "intuitive +// sense", i.e., if Y were up. When input values are unavailable, the +// value 'pad' is inserted instead. The geometric center of the output +// image corresponds to the geometric center of the input image. +image_u8_t *image_u8_rotate(const image_u8_t *in, double rad, uint8_t pad); + +#ifdef __cplusplus +} +#endif diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/image_u8x3.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/image_u8x3.h new file mode 100644 index 0000000000..4d1713dca5 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/image_u8x3.h @@ -0,0 +1,64 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#include +#include "image_types.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +///////////////////////////////////// +// IMPORTANT NOTE ON BYTE ORDER +// +// Format conversion routines will (unless otherwise specified) assume +// R, G, B, ordering of bytes. This is consistent with GTK, PNM, etc. +// +///////////////////////////////////// + +// Create or load an image. returns NULL on failure +image_u8x3_t *image_u8x3_create(unsigned int width, unsigned int height); +image_u8x3_t *image_u8x3_create_alignment(unsigned int width, unsigned int height, unsigned int alignment); +image_u8x3_t *image_u8x3_create_from_pnm(const char *path); + +image_u8x3_t *image_u8x3_copy(const image_u8x3_t *in); + +void image_u8x3_gaussian_blur(image_u8x3_t *im, double sigma, int ksz); + +void image_u8x3_destroy(image_u8x3_t *im); + +int image_u8x3_write_pnm(const image_u8x3_t *im, const char *path); + +// only width 1 supported +void image_u8x3_draw_line(image_u8x3_t *im, float x0, float y0, float x1, float y1, uint8_t rgb[3], int width); + +#ifdef __cplusplus +} +#endif diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/image_u8x4.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/image_u8x4.h new file mode 100644 index 0000000000..89820a2a85 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/image_u8x4.h @@ -0,0 +1,65 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#include +#include "image_types.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +///////////////////////////////////// +// IMPORTANT NOTE ON BYTE ORDER +// +// Format conversion routines will (unless otherwise specified) assume +// R, G, B, A ordering of bytes. +// +///////////////////////////////////// + +// Create or load an image. returns NULL on failure +image_u8x4_t *image_u8x4_create(unsigned int width, unsigned int height); +image_u8x4_t *image_u8x4_create_alignment(unsigned int width, unsigned int height, unsigned int alignment); +image_u8x4_t *image_u8x4_create_from_pnm(const char *path); + +image_u8x4_t *image_u8x4_copy(const image_u8x4_t *in); + +void image_u8x4_destroy(image_u8x4_t *im); + +// Write a pnm. Return 0 on success. +// Currently supports GRAY and RGB +int image_u8x4_write_pnm(const image_u8x4_t *im, const char *path); + +image_u8x4_t *image_u8x4_create_from_pam(const char *path); + + void image_u8x4_write_pam(const image_u8x4_t *im, const char *path); + +#ifdef __cplusplus +} +#endif diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/matd.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/matd.h new file mode 100644 index 0000000000..3a79c56b05 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/matd.h @@ -0,0 +1,446 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Defines a matrix structure for holding double-precision values with + * data in row-major order (i.e. index = row*ncols + col). + * + * nrows and ncols are 1-based counts with the exception that a scalar (non-matrix) + * is represented with nrows=0 and/or ncols=0. + */ +typedef struct +{ + unsigned int nrows, ncols; + double data[]; +// double *data; +} matd_t; + +#define MATD_ALLOC(name, nrows, ncols) double name ## _storage [nrows*ncols]; matd_t name = { .nrows = nrows, .ncols = ncols, .data = &name ## _storage }; + +/** + * Defines a small value which can be used in place of zero for approximating + * calculations which are singular at zero values (i.e. inverting a matrix with + * a zero or near-zero determinant). + */ +#define MATD_EPS 1e-8 + +/** + * A macro to reference a specific matd_t data element given it's zero-based + * row and column indexes. Suitable for both retrieval and assignment. + */ +#define MATD_EL(m, row, col) (m)->data[((row)*(m)->ncols + (col))] + +/** + * Creates a double matrix with the given number of rows and columns (or a scalar + * in the case where rows=0 and/or cols=0). All data elements will be initialized + * to zero. It is the caller's responsibility to call matd_destroy() on the + * returned matrix. + */ +matd_t *matd_create(int rows, int cols); + +/** + * Creates a double matrix with the given number of rows and columns (or a scalar + * in the case where rows=0 and/or cols=0). All data elements will be initialized + * using the supplied array of data, which must contain at least rows*cols elements, + * arranged in row-major order (i.e. index = row*ncols + col). It is the caller's + * responsibility to call matd_destroy() on the returned matrix. + */ +matd_t *matd_create_data(int rows, int cols, const double *data); + +/** + * Creates a double matrix with the given number of rows and columns (or a scalar + * in the case where rows=0 and/or cols=0). All data elements will be initialized + * using the supplied array of float data, which must contain at least rows*cols elements, + * arranged in row-major order (i.e. index = row*ncols + col). It is the caller's + * responsibility to call matd_destroy() on the returned matrix. + */ +matd_t *matd_create_dataf(int rows, int cols, const float *data); + +/** + * Creates a square identity matrix with the given number of rows (and + * therefore columns), or a scalar with value 1 in the case where dim=0. + * It is the caller's responsibility to call matd_destroy() on the + * returned matrix. + */ +matd_t *matd_identity(int dim); + +/** + * Creates a scalar with the supplied value 'v'. It is the caller's responsibility + * to call matd_destroy() on the returned matrix. + * + * NOTE: Scalars are different than 1x1 matrices (implementation note: + * they are encoded as 0x0 matrices). For example: for matrices A*B, A + * and B must both have specific dimensions. However, if A is a + * scalar, there are no restrictions on the size of B. + */ +matd_t *matd_create_scalar(double v); + +/** + * Retrieves the cell value for matrix 'm' at the given zero-based row and column index. + * Performs more thorough validation checking than MATD_EL(). + */ +double matd_get(const matd_t *m, int row, int col); + +/** + * Assigns the given value to the matrix cell at the given zero-based row and + * column index. Performs more thorough validation checking than MATD_EL(). + */ +void matd_put(matd_t *m, int row, int col, double value); + +/** + * Retrieves the scalar value of the given element ('m' must be a scalar). + * Performs more thorough validation checking than MATD_EL(). + */ +double matd_get_scalar(const matd_t *m); + +/** + * Assigns the given value to the supplied scalar element ('m' must be a scalar). + * Performs more thorough validation checking than MATD_EL(). + */ +void matd_put_scalar(matd_t *m, double value); + +/** + * Creates an exact copy of the supplied matrix 'm'. It is the caller's + * responsibility to call matd_destroy() on the returned matrix. + */ +matd_t *matd_copy(const matd_t *m); + +/** + * Creates a copy of a subset of the supplied matrix 'a'. The subset will include + * rows 'r0' through 'r1', inclusive ('r1' >= 'r0'), and columns 'c0' through 'c1', + * inclusive ('c1' >= 'c0'). All parameters are zero-based (i.e. matd_select(a, 0, 0, 0, 0) + * will return only the first cell). Cannot be used on scalars or to extend + * beyond the number of rows/columns of 'a'. It is the caller's responsibility to + * call matd_destroy() on the returned matrix. + */ +matd_t *matd_select(const matd_t *a, int r0, int r1, int c0, int c1); + +/** + * Prints the supplied matrix 'm' to standard output by applying the supplied + * printf format specifier 'fmt' for each individual element. Each row will + * be printed on a separate newline. + */ +void matd_print(const matd_t *m, const char *fmt); + +/** + * Prints the transpose of the supplied matrix 'm' to standard output by applying + * the supplied printf format specifier 'fmt' for each individual element. Each + * row will be printed on a separate newline. + */ +void matd_print_transpose(const matd_t *m, const char *fmt); + +/** + * Adds the two supplied matrices together, cell-by-cell, and returns the results + * as a new matrix of the same dimensions. The supplied matrices must have + * identical dimensions. It is the caller's responsibility to call matd_destroy() + * on the returned matrix. + */ +matd_t *matd_add(const matd_t *a, const matd_t *b); + +/** + * Adds the values of 'b' to matrix 'a', cell-by-cell, and overwrites the + * contents of 'a' with the results. The supplied matrices must have + * identical dimensions. + */ +void matd_add_inplace(matd_t *a, const matd_t *b); + +/** + * Subtracts matrix 'b' from matrix 'a', cell-by-cell, and returns the results + * as a new matrix of the same dimensions. The supplied matrices must have + * identical dimensions. It is the caller's responsibility to call matd_destroy() + * on the returned matrix. + */ +matd_t *matd_subtract(const matd_t *a, const matd_t *b); + +/** + * Subtracts the values of 'b' from matrix 'a', cell-by-cell, and overwrites the + * contents of 'a' with the results. The supplied matrices must have + * identical dimensions. + */ +void matd_subtract_inplace(matd_t *a, const matd_t *b); + +/** + * Scales all cell values of matrix 'a' by the given scale factor 's' and + * returns the result as a new matrix of the same dimensions. It is the caller's + * responsibility to call matd_destroy() on the returned matrix. + */ +matd_t *matd_scale(const matd_t *a, double s); + +/** + * Scales all cell values of matrix 'a' by the given scale factor 's' and + * overwrites the contents of 'a' with the results. + */ +void matd_scale_inplace(matd_t *a, double s); + +/** + * Multiplies the two supplied matrices together (matrix product), and returns the + * results as a new matrix. The supplied matrices must have dimensions such that + * columns(a) = rows(b). The returned matrix will have a row count of rows(a) + * and a column count of columns(b). It is the caller's responsibility to call + * matd_destroy() on the returned matrix. + */ +matd_t *matd_multiply(const matd_t *a, const matd_t *b); + +/** + * Creates a matrix which is the transpose of the supplied matrix 'a'. It is the + * caller's responsibility to call matd_destroy() on the returned matrix. + */ +matd_t *matd_transpose(const matd_t *a); + +/** + * Calculates the determinant of the supplied matrix 'a'. + */ +double matd_det(const matd_t *a); + +/** + * Attempts to compute an inverse of the supplied matrix 'a' and return it as + * a new matrix. This is strictly only possible if the determinant of 'a' is + * non-zero (matd_det(a) != 0). + * + * If the determinant is zero, NULL is returned. It is otherwise the + * caller's responsibility to cope with the results caused by poorly + * conditioned matrices. (E.g.., if such a situation is likely to arise, compute + * the pseudo-inverse from the SVD.) + **/ +matd_t *matd_inverse(const matd_t *a); + +static inline void matd_set_data(matd_t *m, const double *data) +{ + memcpy(m->data, data, m->nrows * m->ncols * sizeof(double)); +} + +/** + * Determines whether the supplied matrix 'a' is a scalar (positive return) or + * not (zero return, indicating a matrix of dimensions at least 1x1). + */ +static inline int matd_is_scalar(const matd_t *a) +{ + assert(a != NULL); + return a->ncols <= 1 && a->nrows <= 1; +} + +/** + * Determines whether the supplied matrix 'a' is a row or column vector + * (positive return) or not (zero return, indicating either 'a' is a scalar or a + * matrix with at least one dimension > 1). + */ +static inline int matd_is_vector(const matd_t *a) +{ + assert(a != NULL); + return a->ncols == 1 || a->nrows == 1; +} + +/** + * Determines whether the supplied matrix 'a' is a row or column vector + * with a dimension of 'len' (positive return) or not (zero return). + */ +static inline int matd_is_vector_len(const matd_t *a, int len) +{ + assert(a != NULL); + return (a->ncols == 1 && a->nrows == (unsigned int)len) || (a->ncols == (unsigned int)len && a->nrows == 1); +} + +/** + * Calculates the magnitude of the supplied matrix 'a'. + */ +double matd_vec_mag(const matd_t *a); + +/** + * Calculates the magnitude of the distance between the points represented by + * matrices 'a' and 'b'. Both 'a' and 'b' must be vectors and have the same + * dimension (although one may be a row vector and one may be a column vector). + */ +double matd_vec_dist(const matd_t *a, const matd_t *b); + + +/** + * Same as matd_vec_dist, but only uses the first 'n' terms to compute distance + */ +double matd_vec_dist_n(const matd_t *a, const matd_t *b, int n); + +/** + * Calculates the dot product of two vectors. Both 'a' and 'b' must be vectors + * and have the same dimension (although one may be a row vector and one may be + * a column vector). + */ +double matd_vec_dot_product(const matd_t *a, const matd_t *b); + +/** + * Calculates the normalization of the supplied vector 'a' (i.e. a unit vector + * of the same dimension and orientation as 'a' with a magnitude of 1) and returns + * it as a new vector. 'a' must be a vector of any dimension and must have a + * non-zero magnitude. It is the caller's responsibility to call matd_destroy() + * on the returned matrix. + */ +matd_t *matd_vec_normalize(const matd_t *a); + +/** + * Calculates the cross product of supplied matrices 'a' and 'b' (i.e. a x b) + * and returns it as a new matrix. Both 'a' and 'b' must be vectors of dimension + * 3, but can be either row or column vectors. It is the caller's responsibility + * to call matd_destroy() on the returned matrix. + */ +matd_t *matd_crossproduct(const matd_t *a, const matd_t *b); + +double matd_err_inf(const matd_t *a, const matd_t *b); + +/** + * Creates a new matrix by applying a series of matrix operations, as expressed + * in 'expr', to the supplied list of matrices. Each matrix to be operated upon + * must be represented in the expression by a separate matrix placeholder, 'M', + * and there must be one matrix supplied as an argument for each matrix + * placeholder in the expression. All rules and caveats of the corresponding + * matrix operations apply to the operated-on matrices. It is the caller's + * responsibility to call matd_destroy() on the returned matrix. + * + * Available operators (in order of increasing precedence): + * M+M add two matrices together + * M-M subtract one matrix from another + * M*M multiply two matrices together (matrix product) + * MM multiply two matrices together (matrix product) + * -M negate a matrix + * M^-1 take the inverse of a matrix + * M' take the transpose of a matrix + * + * Expressions can be combined together and grouped by enclosing them in + * parenthesis, i.e.: + * -M(M+M+M)-(M*M)^-1 + * + * Scalar values can be generated on-the-fly, i.e.: + * M*2.2 scales M by 2.2 + * -2+M adds -2 to all elements of M + * + * All whitespace in the expression is ignored. + */ +matd_t *matd_op(const char *expr, ...); + +/** + * Frees the memory associated with matrix 'm', being the result of an earlier + * call to a matd_*() function, after which 'm' will no longer be usable. + */ +void matd_destroy(matd_t *m); + +typedef struct +{ + matd_t *U; + matd_t *S; + matd_t *V; +} matd_svd_t; + +/** Compute a complete SVD of a matrix. The SVD exists for all + * matrices. For a matrix MxN, we will have: + * + * A = U*S*V' + * + * where A is MxN, U is MxM (and is an orthonormal basis), S is MxN + * (and is diagonal up to machine precision), and V is NxN (and is an + * orthonormal basis). + * + * The caller is responsible for destroying U, S, and V. + **/ +matd_svd_t matd_svd(matd_t *A); + +#define MATD_SVD_NO_WARNINGS 1 + matd_svd_t matd_svd_flags(matd_t *A, int flags); + +//////////////////////////////// +// PLU Decomposition + +// All square matrices (even singular ones) have a partially-pivoted +// LU decomposition such that A = PLU, where P is a permutation +// matrix, L is a lower triangular matrix, and U is an upper +// triangular matrix. +// +typedef struct +{ + // was the input matrix singular? When a zero pivot is found, this + // flag is set to indicate that this has happened. + int singular; + + unsigned int *piv; // permutation indices + int pivsign; // either +1 or -1 + + // The matd_plu_t object returned "owns" the enclosed LU matrix. It + // is not expected that the returned object is itself useful to + // users: it contains the L and U information all smushed + // together. + matd_t *lu; // combined L and U matrices, permuted so they can be triangular. +} matd_plu_t; + +matd_plu_t *matd_plu(const matd_t *a); +void matd_plu_destroy(matd_plu_t *mlu); +double matd_plu_det(const matd_plu_t *lu); +matd_t *matd_plu_p(const matd_plu_t *lu); +matd_t *matd_plu_l(const matd_plu_t *lu); +matd_t *matd_plu_u(const matd_plu_t *lu); +matd_t *matd_plu_solve(const matd_plu_t *mlu, const matd_t *b); + +// uses LU decomposition internally. +matd_t *matd_solve(matd_t *A, matd_t *b); + +//////////////////////////////// +// Cholesky Factorization + +/** + * Creates a double matrix with the Cholesky lower triangular matrix + * of A. A must be symmetric, positive definite. It is the caller's + * responsibility to call matd_destroy() on the returned matrix. + */ +//matd_t *matd_cholesky(const matd_t *A); + +typedef struct +{ + int is_spd; + matd_t *u; +} matd_chol_t; + +matd_chol_t *matd_chol(matd_t *A); +matd_t *matd_chol_solve(const matd_chol_t *chol, const matd_t *b); +void matd_chol_destroy(matd_chol_t *chol); +// only sensible on PSD matrices +matd_t *matd_chol_inverse(matd_t *a); + +void matd_ltransposetriangle_solve(matd_t *u, const double *b, double *x); +void matd_ltriangle_solve(matd_t *u, const double *b, double *x); +void matd_utriangle_solve(matd_t *u, const double *b, double *x); + + +double matd_max(matd_t *m); + +#ifdef __cplusplus +} +#endif diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/math_util.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/math_util.h new file mode 100644 index 0000000000..5434355a51 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/math_util.h @@ -0,0 +1,216 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#include +#include +#include +#include +#include +#include // memcpy + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef M_TWOPI +# define M_TWOPI 6.2831853071795862319959 /* 2*pi */ +#endif + +#ifndef M_PI +# define M_PI 3.141592653589793238462643383279502884196 +#endif + +#define to_radians(x) ( (x) * (M_PI / 180.0 )) +#define to_degrees(x) ( (x) * (180.0 / M_PI )) + +#define max(A, B) (A < B ? B : A) +#define min(A, B) (A < B ? A : B) + + /* DEPRECATE, threshold meaningless without context. +static inline int dequals(double a, double b) +{ + double thresh = 1e-9; + return (fabs(a-b) < thresh); +} + */ + +static inline int dequals_mag(double a, double b, double thresh) +{ + return (fabs(a-b) < thresh); +} + +static inline int isq(int v) +{ + return v*v; +} + +static inline float fsq(float v) +{ + return v*v; +} + +static inline double sq(double v) +{ + return v*v; +} + +static inline double sgn(double v) +{ + return (v>=0) ? 1 : -1; +} + +// random number between [0, 1) +static inline float randf(void) +{ + return (float)(rand() / (RAND_MAX + 1.0)); +} + + +static inline float signed_randf(void) +{ + return randf()*2 - 1; +} + +// return a random integer between [0, bound) +static inline int irand(int bound) +{ + int v = (int) (randf()*bound); + if (v == bound) + return (bound-1); + //assert(v >= 0); + //assert(v < bound); + return v; +} + +/** Map vin to [0, 2*PI) **/ +static inline double mod2pi_positive(double vin) +{ + return vin - M_TWOPI * floor(vin / M_TWOPI); +} + +/** Map vin to [-PI, PI) **/ +static inline double mod2pi(double vin) +{ + return mod2pi_positive(vin + M_PI) - M_PI; +} + +/** Return vin such that it is within PI degrees of ref **/ +static inline double mod2pi_ref(double ref, double vin) +{ + return ref + mod2pi(vin - ref); +} + +/** Map vin to [0, 360) **/ +static inline double mod360_positive(double vin) +{ + return vin - 360 * floor(vin / 360); +} + +/** Map vin to [-180, 180) **/ +static inline double mod360(double vin) +{ + return mod360_positive(vin + 180) - 180; +} + +static inline int mod_positive(int vin, int mod) { + return (vin % mod + mod) % mod; +} + +static inline int theta_to_int(double theta, int max) +{ + theta = mod2pi_ref(M_PI, theta); + int v = (int) (theta / M_TWOPI * max); + + if (v == max) + v = 0; + + assert (v >= 0 && v < max); + + return v; +} + +static inline int imin(int a, int b) +{ + return (a < b) ? a : b; +} + +static inline int imax(int a, int b) +{ + return (a > b) ? a : b; +} + +static inline int64_t imin64(int64_t a, int64_t b) +{ + return (a < b) ? a : b; +} + +static inline int64_t imax64(int64_t a, int64_t b) +{ + return (a > b) ? a : b; +} + +static inline int iclamp(int v, int minv, int maxv) +{ + return imax(minv, imin(v, maxv)); +} + +static inline double dclamp(double a, double min, double max) +{ + if (a < min) + return min; + if (a > max) + return max; + return a; +} + +static inline int fltcmp (float f1, float f2) +{ + float epsilon = f1-f2; + if (epsilon < 0.0) + return -1; + else if (epsilon > 0.0) + return 1; + else + return 0; +} + +static inline int dblcmp (double d1, double d2) +{ + double epsilon = d1-d2; + if (epsilon < 0.0) + return -1; + else if (epsilon > 0.0) + return 1; + else + return 0; +} + +#ifdef __cplusplus +} +#endif diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/pam.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/pam.h new file mode 100644 index 0000000000..67fff6748d --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/pam.h @@ -0,0 +1,54 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#include + +enum { PAM_GRAYSCALE_ALPHA = 5000, PAM_RGB_ALPHA, PAM_RGB, PAM_GRAYSCALE }; + +typedef struct pam pam_t; +struct pam +{ + int type; // one of PAM_* + + int width, height; // note, stride always width. + int depth; // bytes per pixel + int maxval; // maximum value per channel, e.g. 255 for 8bpp + + int datalen; // in bytes + uint8_t *data; +}; + +pam_t *pam_create_from_file(const char *inpath); +int pam_write_file(pam_t *pam, const char *outpath); +void pam_destroy(pam_t *pam); + +pam_t *pam_copy(pam_t *pam); + +// NB doesn't handle many conversions yet. +pam_t *pam_convert(pam_t *in, int type); diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/pjpeg.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/pjpeg.h new file mode 100644 index 0000000000..c9ba9c8b7a --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/pjpeg.h @@ -0,0 +1,103 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#include "image_u8.h" +#include "image_u8x3.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct pjpeg_component pjpeg_component_t; +struct pjpeg_component +{ + // resolution of this component (which is smaller than the + // dimensions of the image if the channel has been sub-sampled.) + uint32_t width, height; + + // number of bytes per row. May be larger than width for alignment + // reasons. + uint32_t stride; + + // data[y*stride + x] + uint8_t *data; + + //////////////////////////////////////////////////////////////// + // These items probably not of great interest to most + // applications. + uint8_t id; // the identifier associated with this component + uint8_t hv; // horiz scale (high 4 bits) / vert scale (low 4 bits) + uint8_t scalex, scaley; // derived from hv above + uint8_t tq; // quantization table index + + // this filled in at the last moment by SOS + uint8_t tda; // which huff tables will we use for DC (high 4 bits) and AC (low 4 bits) +}; + +typedef struct pjpeg pjpeg_t; +struct pjpeg +{ + // status of the decode is put here. Non-zero means error. + int error; + + uint32_t width, height; // pixel dimensions + + int ncomponents; + pjpeg_component_t *components; +}; + +enum PJPEG_FLAGS { + PJPEG_STRICT = 1, // Don't try to recover from errors. + PJPEG_MJPEG = 2, // Support JPGs with missing DHT segments. +}; + +enum PJPEG_ERROR { + PJPEG_OKAY = 0, + PJPEG_ERR_FILE, // something wrong reading file + PJPEG_ERR_DQT, // something wrong with DQT marker + PJPEG_ERR_SOF, // something wrong with SOF marker + PJPEG_ERR_DHT, // something wrong with DHT marker + PJPEG_ERR_SOS, // something wrong with SOS marker + PJPEG_ERR_MISSING_DHT, // missing a necessary huffman table + PJPEG_ERR_DRI, // something wrong with DRI marker + PJPEG_ERR_RESET, // didn't get a reset marker where we expected. Corruption? + PJPEG_ERR_EOF, // ran out of bytes while decoding + PJEPG_ERR_UNSUPPORTED, // an unsupported format +}; + +pjpeg_t *pjpeg_create_from_file(const char *path, uint32_t flags, int *error); +pjpeg_t *pjpeg_create_from_buffer(uint8_t *buf, int buflen, uint32_t flags, int *error); +void pjpeg_destroy(pjpeg_t *pj); + +image_u8_t *pjpeg_to_u8_baseline(pjpeg_t *pj); +image_u8x3_t *pjpeg_to_u8x3_baseline(pjpeg_t *pj); + +#ifdef __cplusplus +} +#endif diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/pnm.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/pnm.h new file mode 100644 index 0000000000..4d8f46e2c6 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/pnm.h @@ -0,0 +1,58 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define PNM_FORMAT_BINARY 4 +#define PNM_FORMAT_GRAY 5 +#define PNM_FORMAT_RGB 6 + +// supports ppm, pnm, pgm + +typedef struct pnm pnm_t; +struct pnm +{ + int width, height; + int format; + int max; // 1 = binary, 255 = one byte, 65535 = two bytes + + uint32_t buflen; + uint8_t *buf; // if max=65535, in big endian +}; + +pnm_t *pnm_create_from_file(const char *path); +void pnm_destroy(pnm_t *pnm); + +#ifdef __cplusplus +} +#endif diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/pthreads_cross.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/pthreads_cross.h new file mode 100644 index 0000000000..54f1865848 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/pthreads_cross.h @@ -0,0 +1,81 @@ +/** +Copyright John Schember + +Permission is hereby granted, free of charge, to any person obtaining a copy of +this software and associated documentation files (the "Software"), to deal in +the Software without restriction, including without limitation the rights to +use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies +of the Software, and to permit persons to whom the Software is furnished to do +so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +#ifndef __CPTHREAD_H__ +#define __CPTHREAD_H__ + +#ifdef _WIN32 +#include +#include +#else +#include +#include +#endif + +#ifdef _WIN32 + +typedef CRITICAL_SECTION pthread_mutex_t; +typedef void pthread_mutexattr_t; +typedef void pthread_attr_t; +typedef void pthread_condattr_t; +typedef void pthread_rwlockattr_t; +typedef HANDLE pthread_t; +typedef CONDITION_VARIABLE pthread_cond_t; + +#ifdef __cplusplus +extern "C" { +#endif +int pthread_create(pthread_t *thread, pthread_attr_t *attr, void *(*start_routine)(void *), void *arg); +int pthread_join(pthread_t thread, void **value_ptr); +int pthread_detach(pthread_t); + +int pthread_mutex_init(pthread_mutex_t *mutex, pthread_mutexattr_t *attr); +int pthread_mutex_destroy(pthread_mutex_t *mutex); +int pthread_mutex_lock(pthread_mutex_t *mutex); +int pthread_mutex_unlock(pthread_mutex_t *mutex); + +int pthread_cond_init(pthread_cond_t *cond, pthread_condattr_t *attr); +int pthread_cond_destroy(pthread_cond_t *cond); +int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex); +int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex, const struct timespec *abstime); +int pthread_cond_signal(pthread_cond_t *cond); +int pthread_cond_broadcast(pthread_cond_t *cond); + +int sched_yield(void); +#ifdef __cplusplus +} +#endif +#endif + + +#ifdef __cplusplus +extern "C" { +#endif +unsigned int pcthread_get_num_procs(void); + +void ms_to_timespec(struct timespec *ts, unsigned int ms); +unsigned int timespec_to_ms(const struct timespec *abstime); +#ifdef __cplusplus +} +#endif + +#endif /* __CPTHREAD_H__ */ diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/string_util.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/string_util.h new file mode 100644 index 0000000000..d0363b03f2 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/string_util.h @@ -0,0 +1,465 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#include +#include +#include +#include + +#include "zarray.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +typedef struct string_buffer string_buffer_t; + +typedef struct string_feeder string_feeder_t; +struct string_feeder +{ + char *s; + size_t len; + size_t pos; + + int line, col; +}; + +/** + * Similar to sprintf(), except that it will malloc() enough space for the + * formatted string which it returns. It is the caller's responsibility to call + * free() on the returned string when it is no longer needed. + */ +char *sprintf_alloc(const char *fmt, ...) +#ifndef _MSC_VER +__attribute__ ((format (printf, 1, 2))) +#endif +; + +/** + * Similar to vsprintf(), except that it will malloc() enough space for the + * formatted string which it returns. It is the caller's responsibility to call + * free() on the returned string when it is no longer needed. + */ +char *vsprintf_alloc(const char *fmt, va_list args); + +/** + * Concatenates 1 or more strings together and returns the result, which will be a + * newly allocated string which it is the caller's responsibility to free. + */ +#define str_concat(...) _str_concat_private(__VA_ARGS__, NULL) +char *_str_concat_private(const char *first, ...); + + +// Returns the index of the first character that differs: +int str_diff_idx(const char * a, const char * b); + +/** + * Splits the supplied string into an array of strings by subdividing it at + * each occurrence of the supplied delimiter string. The split strings will not + * contain the delimiter. The original string will remain unchanged. + * If str is composed of all delimiters, an empty array will be returned. + * + * It is the caller's responsibility to free the returned zarray, as well as + * the strings contained within it, e.g.: + * + * zarray_t *za = str_split("this is a haystack", " "); + * => ["this", "is", "a", "haystack"] + * zarray_vmap(za, free); + * zarray_destroy(za); + */ +zarray_t *str_split(const char *str, const char *delim); + +zarray_t *str_split_spaces(const char *str); + +void str_split_destroy(zarray_t *s); + +/* + * Determines if str1 exactly matches str2 (more efficient than strcmp(...) == 0) + */ +static inline bool streq(const char *str1, const char* str2) +{ + int i; + for (i = 0 ; str1[i] != '\0' ; i++) { + if (str1[i] != str2[i]) + return false; + } + + return str2[i] == '\0'; +} + +/** + * Determines if str1 exactly matches str2, ignoring case (more efficient than + * strcasecmp(...) == 0) + */ +static inline bool strcaseeq(const char *str1, const char* str2) +{ + int i; + for (i = 0 ; str1[i] != '\0' ; i++) { + if (str1[i] == str2[i]) + continue; + else if (islower(str1[i]) && (str1[i] - 32) == str2[i]) + continue; + else if (isupper(str1[i]) && (str1[i] + 32) == str2[i]) + continue; + + return false; + } + + return str2[i] == '\0'; +} + +/** + * Trims whitespace characters (i.e. matching isspace()) from the beginning and/or + * end of the supplied string. This change affects the supplied string in-place. + * The supplied/edited string is returned to enable chained reference. + * + * Note: do not pass a string literal to this function + */ +char *str_trim(char *str); + +/** + * Trims whitespace characters (i.e. matching isspace()) from the beginning + * of the supplied string. This change affects the supplied string in-place. + * The supplied/edited string is returned to enable chained reference. + * + * Note: do not pass a string literal to this function + */ +char *str_lstrip(char *str); + +/** + * Trims whitespace characters (i.e. matching isspace()) from the end of the + * supplied string. This change affects the supplied string in-place. + * The supplied/edited string is returned to enable chained reference. + * + * Note: do not pass a string literal to this function + */ +char *str_rstrip(char *str); + +/** + * Returns true if the end of string 'haystack' matches 'needle', else false. + * + * Note: An empty needle ("") will match any source. + */ +bool str_ends_with(const char *haystack, const char *needle); + +/** + * Returns true if the start of string 'haystack' matches 'needle', else false. + * + * Note: An empty needle ("") will match any source. + */ +bool str_starts_with(const char *haystack, const char *needle); + +/** + * Returns true if the start of string 'haystack' matches any needle, else false. + * + * Note: An empty needle ("") will match any source. + */ +bool str_starts_with_any(const char *haystack, const char **needles, int num_needles); + +/** + * Returns true if the string 'haystack' matches any needle, else false. + */ +bool str_matches_any(const char *haystack, const char **needles, int num_needles); + +/** + * Retrieves a (newly-allocated) substring of the given string, 'str', starting + * from character index 'startidx' through index 'endidx' - 1 (inclusive). + * An 'endidx' value -1 is equivalent to strlen(str). + * + * It is the caller's responsibility to free the returned string. + * + * Examples: + * str_substring("string", 1, 3) = "tr" + * str_substring("string", 2, -1) = "ring" + * str_substring("string", 3, 3) = "" + * + * Note: startidx must be >= endidx + */ +char *str_substring(const char *str, size_t startidx, long endidx); + +/** + * Retrieves the zero-based index of the beginning of the supplied substring + * (needle) within the search string (haystack) if it exists. + * + * Returns -1 if the supplied needle is not found within the haystack. + */ +int str_indexof(const char *haystack, const char *needle); + + static inline int str_contains(const char *haystack, const char *needle) { + return str_indexof(haystack, needle) >= 0; + } + +// same as above, but returns last match +int str_last_indexof(const char *haystack, const char *needle); + +/** + * Replaces all upper-case characters within the supplied string with their + * lower-case counterparts, modifying the original string's contents. + * + * Returns the supplied / modified string. + */ +char *str_tolowercase(char *s); + +/** + * Replaces all lower-case characters within the supplied string with their + * upper-case counterparts, modifying the original string's contents. + * + * Returns the supplied / modified string. + */ +char *str_touppercase(char *s); + +/** + * Replaces all occurrences of 'needle' in the string 'haystack', substituting + * for them the value of 'replacement', and returns the result as a newly-allocated + * string. The original strings remain unchanged. + * + * It is the caller's responsibility to free the returned string. + * + * Examples: + * str_replace("string", "ri", "u") = "stung" + * str_replace("singing", "ing", "") = "s" + * str_replace("string", "foo", "bar") = "string" + * + * Note: An empty needle will match only an empty haystack + */ +char *str_replace(const char *haystack, const char *needle, const char *replacement); + + char *str_replace_many(const char *_haystack, ...); +////////////////////////////////////////////////////// +// String Buffer + +/** + * Creates and initializes a string buffer object which can be used with any of + * the string_buffer_*() functions. + * + * It is the caller's responsibility to free the string buffer resources with + * a call to string_buffer_destroy() when it is no longer needed. + */ +string_buffer_t *string_buffer_create(void); + +/** + * Frees the resources associated with a string buffer object, including space + * allocated for any appended characters / strings. + */ +void string_buffer_destroy(string_buffer_t *sb); + +/** + * Appends a single character to the end of the supplied string buffer. + */ +void string_buffer_append(string_buffer_t *sb, char c); + +/** + * Removes a single character from the end of the string and + * returns it. Does nothing if string is empty and returns NULL + */ +char string_buffer_pop_back(string_buffer_t *sb); + +/** + * Appends the supplied string to the end of the supplied string buffer. + */ +void string_buffer_append_string(string_buffer_t *sb, const char *str); + +/** + * Formats the supplied string and arguments in a manner akin to printf(), and + * appends the resulting string to the end of the supplied string buffer. + */ +void string_buffer_appendf(string_buffer_t *sb, const char *fmt, ...) +#ifndef _MSC_VER +__attribute__ ((format (printf, 2, 3))) +#endif +; + +/** + * Determines whether the character contents held by the supplied string buffer + * ends with the supplied string. + * + * Returns true if the string buffer's contents ends with 'str', else false. + */ +bool string_buffer_ends_with(string_buffer_t *sb, const char *str); + +/** + * Returns the string-length of the contents of the string buffer (not counting \0). + * Equivalent to calling strlen() on the string returned by string_buffer_to_string(sb). + */ +size_t string_buffer_size(string_buffer_t *sb); + +/** + * Returns the contents of the string buffer in a newly-allocated string, which + * it is the caller's responsibility to free once it is no longer needed. + */ +char *string_buffer_to_string(string_buffer_t *sb); + +/** + * Clears the contents of the string buffer, setting its length to zero. + */ +void string_buffer_reset(string_buffer_t *sb); + +////////////////////////////////////////////////////// +// String Feeder + +/** + * Creates a string feeder object which can be used to traverse the supplied + * string using the string_feeder_*() functions. A local copy of the string's + * contents will be stored so that future changes to 'str' will not be + * reflected by the string feeder object. + * + * It is the caller's responsibility to call string_feeder_destroy() on the + * returned object when it is no longer needed. + */ +string_feeder_t *string_feeder_create(const char *str); + +/** + * Frees resources associated with the supplied string feeder object, after + * which it will no longer be valid for use. + */ +void string_feeder_destroy(string_feeder_t *sf); + +/** + * Determines whether any characters remain to be retrieved from the string + * feeder's string (not including the terminating '\0'). + * + * Returns true if at least one more character can be retrieved with calls to + * string_feeder_next(), string_feeder_peek(), string_feeder_peek(), or + * string_feeder_consume(), else false. + */ +bool string_feeder_has_next(string_feeder_t *sf); + +/** + * Retrieves the next available character from the supplied string feeder + * (which may be the terminating '\0' character) and advances the feeder's + * position to the next character in the string. + * + * Note: Attempts to read past the end of the string will throw an assertion. + */ +char string_feeder_next(string_feeder_t *sf); + +/** + * Retrieves a series of characters from the supplied string feeder. The number + * of characters returned will be 'length' or the number of characters + * remaining in the string, whichever is shorter. The string feeder's position + * will be advanced by the number of characters returned. + * + * It is the caller's responsibility to free the returned string when it is no + * longer needed. + * + * Note: Calling once the end of the string has already been read will throw an assertion. + */ +char *string_feeder_next_length(string_feeder_t *sf, size_t length); + +/** + * Retrieves the next available character from the supplied string feeder + * (which may be the terminating '\0' character), but does not advance + * the feeder's position so that subsequent calls to _next() or _peek() will + * retrieve the same character. + * + * Note: Attempts to peek past the end of the string will throw an assertion. + */ +char string_feeder_peek(string_feeder_t *sf); + +/** + * Retrieves a series of characters from the supplied string feeder. The number + * of characters returned will be 'length' or the number of characters + * remaining in the string, whichever is shorter. The string feeder's position + * will not be advanced. + * + * It is the caller's responsibility to free the returned string when it is no + * longer needed. + * + * Note: Calling once the end of the string has already been read will throw an assertion. + */ +char *string_feeder_peek_length(string_feeder_t *sf, size_t length); + +/** + * Retrieves the line number of the current position in the supplied + * string feeder, which will be incremented whenever a newline is consumed. + * + * Examples: + * prior to reading 1st character: line = 1, column = 0 + * after reading 1st non-newline character: line = 1, column = 1 + * after reading 2nd non-newline character: line = 1, column = 2 + * after reading 1st newline character: line = 2, column = 0 + * after reading 1st character after 1st newline: line = 2, column = 1 + * after reading 2nd newline character: line = 3, column = 0 + */ +int string_feeder_get_line(string_feeder_t *sf); + +/** + * Retrieves the column index in the current line for the current position + * in the supplied string feeder, which will be incremented with each + * non-newline character consumed, and reset to 0 whenever a newline (\n) is + * consumed. + * + * Examples: + * prior to reading 1st character: line = 1, column = 0 + * after reading 1st non-newline character: line = 1, column = 1 + * after reading 2nd non-newline character: line = 1, column = 2 + * after reading 1st newline character: line = 2, column = 0 + * after reading 1st character after 1st newline: line = 2, column = 1 + * after reading 2nd newline character: line = 3, column = 0 + */ +int string_feeder_get_column(string_feeder_t *sf); + +/** + * Determines whether the supplied string feeder's remaining contents starts + * with the given string. + * + * Returns true if the beginning of the string feeder's remaining contents matches + * the supplied string exactly, else false. + */ +bool string_feeder_starts_with(string_feeder_t *sf, const char *str); + +/** + * Consumes from the string feeder the number of characters contained in the + * given string (not including the terminating '\0'). + * + * Throws an assertion if the consumed characters do not exactly match the + * contents of the supplied string. + */ +void string_feeder_require(string_feeder_t *sf, const char *str); + +/*#ifndef strdup + static inline char *strdup(const char *s) { + int len = strlen(s); + char *out = malloc(len+1); + memcpy(out, s, len + 1); + return out; + } +#endif +*/ + + +// find everything that looks like an env variable and expand it +// using getenv. Caller should free the result. +// e.g. "$HOME/abc" ==> "/home/ebolson/abc" +char *str_expand_envs(const char *in); + +#ifdef __cplusplus +} +#endif diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/svd22.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/svd22.h new file mode 100644 index 0000000000..5277f8b788 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/svd22.h @@ -0,0 +1,34 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +void svd22(const double A[4], double U[4], double S[2], double V[4]); + +// for the matrix [a b; b d] +void svd_sym_singular_values(double A00, double A01, double A11, + double *Lmin, double *Lmax); diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/time_util.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/time_util.h new file mode 100644 index 0000000000..c42466b3a0 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/time_util.h @@ -0,0 +1,83 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#include +#include +#include + +#ifdef _WIN32 +#include +typedef long long suseconds_t; +#endif + +#ifdef _MSC_VER + +inline int gettimeofday(struct timeval* tp, void* tzp) +{ + unsigned long t; + t = time(NULL); + tp->tv_sec = t / 1000; + tp->tv_usec = t % 1000; + return 0; +} +#else +#include +#include +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct timeutil_rest timeutil_rest_t; +timeutil_rest_t *timeutil_rest_create(void); +void timeutil_rest_destroy(timeutil_rest_t * rest); + +int64_t utime_now(void); // blacklist-ignore +int64_t utime_get_seconds(int64_t v); +int64_t utime_get_useconds(int64_t v); +void utime_to_timeval(int64_t v, struct timeval *tv); +void utime_to_timespec(int64_t v, struct timespec *ts); + +int32_t timeutil_usleep(int64_t useconds); +uint32_t timeutil_sleep(unsigned int seconds); +int32_t timeutil_sleep_hz(timeutil_rest_t *rest, double hz); + +void timeutil_timer_reset(timeutil_rest_t *rest); +void timeutil_timer_start(timeutil_rest_t *rest); +void timeutil_timer_stop(timeutil_rest_t *rest); +bool timeutil_timer_timeout(timeutil_rest_t *rest, double timeout_s); + +int64_t time_util_hhmmss_ss_to_utime(double time); + +int64_t timeutil_ms_to_us(int32_t ms); + +#ifdef __cplusplus +} +#endif diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/timeprofile.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/timeprofile.h new file mode 100644 index 0000000000..197d6ceb33 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/timeprofile.h @@ -0,0 +1,120 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include + +#include "time_util.h" +#include "zarray.h" + +struct timeprofile_entry +{ + char name[32]; + int64_t utime; +}; + +typedef struct timeprofile timeprofile_t; +struct timeprofile +{ + int64_t utime; + zarray_t *stamps; +}; + +static inline timeprofile_t *timeprofile_create(void) +{ + timeprofile_t *tp = (timeprofile_t*) calloc(1, sizeof(timeprofile_t)); + tp->stamps = zarray_create(sizeof(struct timeprofile_entry)); + + tp->utime = utime_now(); + + return tp; +} + +static inline void timeprofile_destroy(timeprofile_t *tp) +{ + zarray_destroy(tp->stamps); + free(tp); +} + +static inline void timeprofile_clear(timeprofile_t *tp) +{ + zarray_clear(tp->stamps); + tp->utime = utime_now(); +} + +static inline void timeprofile_stamp(timeprofile_t *tp, const char *name) +{ + struct timeprofile_entry tpe; + + strncpy(tpe.name, name, sizeof(tpe.name)); + tpe.name[sizeof(tpe.name)-1] = 0; + tpe.utime = utime_now(); + + zarray_add(tp->stamps, &tpe); +} + +static inline void timeprofile_display(timeprofile_t *tp) +{ + int64_t lastutime = tp->utime; + + for (int i = 0; i < zarray_size(tp->stamps); i++) { + struct timeprofile_entry *stamp; + + zarray_get_volatile(tp->stamps, i, &stamp); + + double cumtime = (stamp->utime - tp->utime)/1000000.0; + + double parttime = (stamp->utime - lastutime)/1000000.0; + + printf("%2d %32s %15f ms %15f ms\n", i, stamp->name, parttime*1000, cumtime*1000); + + lastutime = stamp->utime; + } +} + +static inline uint64_t timeprofile_total_utime(timeprofile_t *tp) +{ + if (zarray_size(tp->stamps) == 0) + return 0; + + struct timeprofile_entry *first, *last; + zarray_get_volatile(tp->stamps, 0, &first); + zarray_get_volatile(tp->stamps, zarray_size(tp->stamps) - 1, &last); + + return last->utime - first->utime; +} + +#ifdef __cplusplus +} +#endif diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/unionfind.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/unionfind.h new file mode 100644 index 0000000000..fdfef9dc30 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/unionfind.h @@ -0,0 +1,145 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#include +#include +#include + +typedef struct unionfind unionfind_t; + +struct unionfind +{ + uint32_t maxid; + + // Parent node for each. Initialized to 0xffffffff + uint32_t *parent; + + // The size of the tree excluding the root + uint32_t *size; +}; + +static inline unionfind_t *unionfind_create(uint32_t maxid) +{ + unionfind_t *uf = (unionfind_t*) calloc(1, sizeof(unionfind_t)); + uf->maxid = maxid; + uf->parent = (uint32_t *) malloc((maxid+1) * sizeof(uint32_t) * 2); + memset(uf->parent, 0xff, (maxid+1) * sizeof(uint32_t)); + uf->size = uf->parent + (maxid+1); + memset(uf->size, 0, (maxid+1) * sizeof(uint32_t)); + return uf; +} + +static inline void unionfind_destroy(unionfind_t *uf) +{ + free(uf->parent); + free(uf); +} + +/* +static inline uint32_t unionfind_get_representative(unionfind_t *uf, uint32_t id) +{ + // base case: a node is its own parent + if (uf->parent[id] == id) + return id; + + // otherwise, recurse + uint32_t root = unionfind_get_representative(uf, uf->parent[id]); + + // short circuit the path. [XXX This write prevents tail recursion] + uf->parent[id] = root; + + return root; +} +*/ + +// this one seems to be every-so-slightly faster than the recursive +// version above. +static inline uint32_t unionfind_get_representative(unionfind_t *uf, uint32_t id) +{ + uint32_t root = uf->parent[id]; + // unititialized node, so set to self + if (root == 0xffffffff) { + uf->parent[id] = id; + return id; + } + + // chase down the root + while (uf->parent[root] != root) { + root = uf->parent[root]; + } + + // go back and collapse the tree. + while (uf->parent[id] != root) { + uint32_t tmp = uf->parent[id]; + uf->parent[id] = root; + id = tmp; + } + + return root; +} + +static inline uint32_t unionfind_get_set_size(unionfind_t *uf, uint32_t id) +{ + uint32_t repid = unionfind_get_representative(uf, id); + return uf->size[repid] + 1; +} + +static inline uint32_t unionfind_connect(unionfind_t *uf, uint32_t aid, uint32_t bid) +{ + uint32_t aroot = unionfind_get_representative(uf, aid); + uint32_t broot = unionfind_get_representative(uf, bid); + + if (aroot == broot) + return aroot; + + // we don't perform "union by rank", but we perform a similar + // operation (but probably without the same asymptotic guarantee): + // We join trees based on the number of *elements* (as opposed to + // rank) contained within each tree. I.e., we use size as a proxy + // for rank. In my testing, it's often *faster* to use size than + // rank, perhaps because the rank of the tree isn't that critical + // if there are very few nodes in it. + uint32_t asize = uf->size[aroot] + 1; + uint32_t bsize = uf->size[broot] + 1; + + // optimization idea: We could shortcut some or all of the tree + // that is grafted onto the other tree. Pro: those nodes were just + // read and so are probably in cache. Con: it might end up being + // wasted effort -- the tree might be grafted onto another tree in + // a moment! + if (asize > bsize) { + uf->parent[broot] = aroot; + uf->size[aroot] += bsize; + return aroot; + } else { + uf->parent[aroot] = broot; + uf->size[broot] += asize; + return broot; + } +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/workerpool.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/workerpool.h new file mode 100644 index 0000000000..070a983cbb --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/workerpool.h @@ -0,0 +1,49 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#include "zarray.h" + +typedef struct workerpool workerpool_t; + +// as a special case, if nthreads==1, no additional threads are +// created, and workerpool_run will run synchronously. +workerpool_t *workerpool_create(int nthreads); +void workerpool_destroy(workerpool_t *wp); + +void workerpool_add_task(workerpool_t *wp, void (*f)(void *p), void *p); + +// runs all added tasks, waits for them to complete. +void workerpool_run(workerpool_t *wp); + +// same as workerpool_run, except always single threaded. (mostly for debugging). +void workerpool_run_single(workerpool_t *wp); + +int workerpool_get_nthreads(workerpool_t *wp); + +int workerpool_get_nprocs(void); diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/zarray.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/zarray.h new file mode 100644 index 0000000000..411ee7b649 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/zarray.h @@ -0,0 +1,465 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#include +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Defines a structure which acts as a resize-able array ala Java's ArrayList. + */ +typedef struct zarray zarray_t; +struct zarray +{ + size_t el_sz; // size of each element + + int size; // how many elements? + int alloc; // we've allocated storage for how many elements? + char *data; +}; + +/** + * Creates and returns a variable array structure capable of holding elements of + * the specified size. It is the caller's responsibility to call zarray_destroy() + * on the returned array when it is no longer needed. + */ +static inline zarray_t *zarray_create(size_t el_sz) +{ + assert(el_sz > 0); + + zarray_t *za = (zarray_t*) calloc(1, sizeof(zarray_t)); + za->el_sz = el_sz; + return za; +} + +/** + * Frees all resources associated with the variable array structure which was + * created by zarray_create(). After calling, 'za' will no longer be valid for storage. + */ +static inline void zarray_destroy(zarray_t *za) +{ + if (za == NULL) + return; + + if (za->data != NULL) + free(za->data); + memset(za, 0, sizeof(zarray_t)); + free(za); +} + +/** Allocate a new zarray that contains a copy of the data in the argument. **/ +static inline zarray_t *zarray_copy(const zarray_t *za) +{ + assert(za != NULL); + + zarray_t *zb = (zarray_t*) calloc(1, sizeof(zarray_t)); + zb->el_sz = za->el_sz; + zb->size = za->size; + zb->alloc = za->alloc; + zb->data = (char*) malloc(zb->alloc * zb->el_sz); + memcpy(zb->data, za->data, za->size * za->el_sz); + return zb; +} + +static int iceillog2(int v) +{ + v--; + v |= v >> 1; + v |= v >> 2; + v |= v >> 4; + v |= v >> 8; + v |= v >> 16; + v++; + return v; +} + +/** + * Allocate a new zarray that contains a subset of the original + * elements. NOTE: end index is EXCLUSIVE, that is one past the last + * element you want. + */ +static inline zarray_t *zarray_copy_subset(const zarray_t *za, + int start_idx, + int end_idx_exclusive) +{ + zarray_t *out = (zarray_t*) calloc(1, sizeof(zarray_t)); + out->el_sz = za->el_sz; + out->size = end_idx_exclusive - start_idx; + out->alloc = iceillog2(out->size); // round up pow 2 + out->data = (char*) malloc(out->alloc * out->el_sz); + memcpy(out->data, za->data +(start_idx*out->el_sz), out->size*out->el_sz); + return out; +} + +/** + * Retrieves the number of elements currently being contained by the passed + * array, which may be different from its capacity. The index of the last element + * in the array will be one less than the returned value. + */ +static inline int zarray_size(const zarray_t *za) +{ + assert(za != NULL); + + return za->size; +} + +/** + * Returns 1 if zarray_size(za) == 0, + * returns 0 otherwise. + */ +/* +JUST CALL zarray_size +int zarray_isempty(const zarray_t *za) +{ + assert(za != NULL); + if (za->size <= 0) + return 1; + else + return 0; +} +*/ + + +/** + * Allocates enough internal storage in the supplied variable array structure to + * guarantee that the supplied number of elements (capacity) can be safely stored. + */ +static inline void zarray_ensure_capacity(zarray_t *za, int capacity) +{ + assert(za != NULL); + + if (capacity <= za->alloc) + return; + + while (za->alloc < capacity) { + za->alloc *= 2; + if (za->alloc < 8) + za->alloc = 8; + } + + za->data = (char*) realloc(za->data, za->alloc * za->el_sz); +} + +/** + * Adds a new element to the end of the supplied array, and sets its value + * (by copying) from the data pointed to by the supplied pointer 'p'. + * Automatically ensures that enough storage space is available for the new element. + */ +static inline void zarray_add(zarray_t *za, const void *p) +{ + assert(za != NULL); + assert(p != NULL); + + zarray_ensure_capacity(za, za->size + 1); + + memcpy(&za->data[za->size*za->el_sz], p, za->el_sz); + za->size++; +} + +/** + * Retrieves the element from the supplied array located at the zero-based + * index of 'idx' and copies its value into the variable pointed to by the pointer + * 'p'. + */ +static inline void zarray_get(const zarray_t *za, int idx, void *p) +{ + assert(za != NULL); + assert(p != NULL); + assert(idx >= 0); + assert(idx < za->size); + + memcpy(p, &za->data[idx*za->el_sz], za->el_sz); +} + +/** + * Similar to zarray_get(), but returns a "live" pointer to the internal + * storage, avoiding a memcpy. This pointer is not valid across + * operations which might move memory around (i.e. zarray_remove_value(), + * zarray_remove_index(), zarray_insert(), zarray_sort(), zarray_clear()). + * 'p' should be a pointer to the pointer which will be set to the internal address. + */ +inline static void zarray_get_volatile(const zarray_t *za, int idx, void *p) +{ + assert(za != NULL); + assert(p != NULL); + assert(idx >= 0); + assert(idx < za->size); + + *((void**) p) = &za->data[idx*za->el_sz]; +} + +inline static void zarray_truncate(zarray_t *za, int sz) +{ + assert(za != NULL); + assert(sz <= za->size); + za->size = sz; +} + +/** + * Removes the entry at index 'idx'. + * If shuffle is true, the last element in the array will be placed in + * the newly-open space; if false, the zarray is compacted. + */ +static inline void zarray_remove_index(zarray_t *za, int idx, int shuffle) +{ + assert(za != NULL); + assert(idx >= 0); + assert(idx < za->size); + + if (shuffle) { + if (idx < za->size-1) + memcpy(&za->data[idx*za->el_sz], &za->data[(za->size-1)*za->el_sz], za->el_sz); + za->size--; + return; + } else { + // size = 10, idx = 7. Should copy 2 entries (at idx=8 and idx=9). + // size = 10, idx = 9. Should copy 0 entries. + int ncopy = za->size - idx - 1; + if (ncopy > 0) + memmove(&za->data[idx*za->el_sz], &za->data[(idx+1)*za->el_sz], ncopy*za->el_sz); + za->size--; + return; + } +} + +/** + * Remove the entry whose value is equal to the value pointed to by 'p'. + * If shuffle is true, the last element in the array will be placed in + * the newly-open space; if false, the zarray is compacted. At most + * one element will be removed. + * + * Note that objects will be compared using memcmp over the full size + * of the value. If the value is a struct that contains padding, + * differences in the padding bytes can cause comparisons to + * fail. Thus, it remains best practice to bzero all structs so that + * the padding is set to zero. + * + * Returns the number of elements removed (0 or 1). + */ +// remove the entry whose value is equal to the value pointed to by p. +// if shuffle is true, the last element in the array will be placed in +// the newly-open space; if false, the zarray is compacted. +static inline int zarray_remove_value(zarray_t *za, const void *p, int shuffle) +{ + assert(za != NULL); + assert(p != NULL); + + for (int idx = 0; idx < za->size; idx++) { + if (!memcmp(p, &za->data[idx*za->el_sz], za->el_sz)) { + zarray_remove_index(za, idx, shuffle); + return 1; + } + } + + return 0; +} + + +/** + * Creates a new entry and inserts it into the array so that it will have the + * index 'idx' (i.e. before the item which currently has that index). The value + * of the new entry is set to (copied from) the data pointed to by 'p'. 'idx' + * can be one larger than the current max index to place the new item at the end + * of the array, or zero to add it to an empty array. + */ +static inline void zarray_insert(zarray_t *za, int idx, const void *p) +{ + assert(za != NULL); + assert(p != NULL); + assert(idx >= 0); + assert(idx <= za->size); + + zarray_ensure_capacity(za, za->size + 1); + // size = 10, idx = 7. Should copy three entries (idx=7, idx=8, idx=9) + int ncopy = za->size - idx; + + memmove(&za->data[(idx+1)*za->el_sz], &za->data[idx*za->el_sz], ncopy*za->el_sz); + memcpy(&za->data[idx*za->el_sz], p, za->el_sz); + + za->size++; +} + + +/** + * Sets the value of the current element at index 'idx' by copying its value from + * the data pointed to by 'p'. The previous value of the changed element will be + * copied into the data pointed to by 'outp' if it is not null. + */ +static inline void zarray_set(zarray_t *za, int idx, const void *p, void *outp) +{ + assert(za != NULL); + assert(p != NULL); + assert(idx >= 0); + assert(idx < za->size); + + if (outp != NULL) + memcpy(outp, &za->data[idx*za->el_sz], za->el_sz); + + memcpy(&za->data[idx*za->el_sz], p, za->el_sz); +} + +/** + * Calls the supplied function for every element in the array in index order. + * The map function will be passed a pointer to each element in turn and must + * have the following format: + * + * void map_function(element_type *element) + */ +static inline void zarray_map(zarray_t *za, void (*f)(void*)) +{ + assert(za != NULL); + assert(f != NULL); + + for (int idx = 0; idx < za->size; idx++) + f(&za->data[idx*za->el_sz]); +} + +/** + * Calls the supplied function for every element in the array in index order. + * HOWEVER values are passed to the function, not pointers to values. In the + * case where the zarray stores object pointers, zarray_vmap allows you to + * pass in the object's destroy function (or free) directly. Can only be used + * with zarray's which contain pointer data. The map function should have the + * following format: + * + * void map_function(element_type *element) + */ + void zarray_vmap(zarray_t *za, void (*f)(void*)); + +/** + * Removes all elements from the array and sets its size to zero. Pointers to + * any data elements obtained i.e. by zarray_get_volatile() will no longer be + * valid. + */ +static inline void zarray_clear(zarray_t *za) +{ + assert(za != NULL); + za->size = 0; +} + +/** + * Determines whether any element in the array has a value which matches the + * data pointed to by 'p'. + * + * Returns 1 if a match was found anywhere in the array, else 0. + */ +static inline int zarray_contains(const zarray_t *za, const void *p) +{ + assert(za != NULL); + assert(p != NULL); + + for (int idx = 0; idx < za->size; idx++) { + if (!memcmp(p, &za->data[idx*za->el_sz], za->el_sz)) { + return 1; + } + } + + return 0; +} + +/** + * Uses qsort() to sort the elements contained by the array in ascending order. + * Uses the supplied comparison function to determine the appropriate order. + * + * The comparison function will be passed a pointer to two elements to be compared + * and should return a measure of the difference between them (see strcmp()). + * I.e. it should return a negative number if the first element is 'less than' + * the second, zero if they are equivalent, and a positive number if the first + * element is 'greater than' the second. The function should have the following format: + * + * int comparison_function(const element_type *first, const element_type *second) + * + * zstrcmp() can be used as the comparison function for string elements, which + * will call strcmp() internally. + */ +static inline void zarray_sort(zarray_t *za, int (*compar)(const void*, const void*)) +{ + assert(za != NULL); + assert(compar != NULL); + if (za->size == 0) + return; + + qsort(za->data, za->size, za->el_sz, compar); +} + +/** + * A comparison function for comparing strings which can be used by zarray_sort() + * to sort arrays with char* elements. + */ + int zstrcmp(const void * a_pp, const void * b_pp); + +/** + * Find the index of an element, or return -1 if not found. Remember that p is + * a pointer to the element. + **/ +// returns -1 if not in array. Remember p is a pointer to the item. +static inline int zarray_index_of(const zarray_t *za, const void *p) +{ + assert(za != NULL); + assert(p != NULL); + + for (int i = 0; i < za->size; i++) { + if (!memcmp(p, &za->data[i*za->el_sz], za->el_sz)) + return i; + } + + return -1; +} + +/** + * Add elements from start up to and excluding end from 'source' into 'dest'. + * el_sz must be the same for both lists + **/ +static inline void zarray_add_range(zarray_t *dest, const zarray_t *source, int start, int end) +{ + assert(dest->el_sz == source->el_sz); + assert(dest != NULL); + assert(source != NULL); + assert(start >= 0); + assert(end <= source->size); + if (start == end) { + return; + } + assert(start < end); + + int count = end - start; + zarray_ensure_capacity(dest, dest->size + count); + + memcpy(&dest->data[dest->size*dest->el_sz], &source->data[source->el_sz*start], dest->el_sz*count); + dest->size += count; +} + +#ifdef __cplusplus +} +#endif diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/zhash.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/zhash.h new file mode 100644 index 0000000000..9993a66e4a --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/zhash.h @@ -0,0 +1,432 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#include "zarray.h" + + +/** + * A hash table for structs and primitive types that stores entries by value. + * - The size of the key/values must be known at instantiation time, and remain fixed. + * e.g. for pointers: zhash_create(sizeof(void*), sizeof(void*)....) + * for structs: zhash_create(sizeof(struct key_struct), sizeof(struct value_struct)...) + * for bytes: zhash_create(sizeof(uint8_t), sizeof(uint8_t)...) + * - Entries are copied by value. This means you must always pass a reference to the start + * of 'key_size' and 'value_size' bytes, which you have already malloc'd or stack allocated + * - This data structure can be used to store types of any size, from bytes & doubles to + * user defined structs + * Note: if zhash stores pointers, user must be careful to manually manage the lifetime + * of the memory they point to. + * + */ + +typedef struct zhash zhash_t; + +// The contents of the iterator should be considered private. However, +// since our usage model prefers stack-based allocation of iterators, +// we must publicly declare them. +struct zhash_iterator +{ + zhash_t *zh; + const zhash_t *czh; + int last_entry; // points to the last entry returned by _next +}; + +typedef struct zhash_iterator zhash_iterator_t; + +/** + * Create, initializes, and returns an empty hash table structure. It is the + * caller's responsibility to call zhash_destroy() on the returned array when it + * is no longer needed. + * + * The size of values used in the hash and equals function must match 'keysz'. + * I.e. if keysz = sizeof(uint64_t), then hash() and equals() should accept + * parameters as *uint64_t. + */ +zhash_t *zhash_create(size_t keysz, size_t valuesz, + uint32_t(*hash)(const void *a), + int(*equals)(const void *a, const void *b)); + +/** + * Frees all resources associated with the hash table structure which was + * created by zhash_create(). After calling, 'zh' will no longer be valid for storage. + * + * If 'zh' contains pointer data, it is the caller's responsibility to manage + * the resources pointed to by those pointers. + */ +void zhash_destroy(zhash_t *zh); + +/** + * Creates and returns a new identical copy of the zhash (i.e. a "shallow" copy). + * If you're storing pointers, be sure not to double free their pointees! + * It is the caller's responsibility to call zhash_destroy() on the returned array + * when it is no longer needed (in addition to the zhash_destroy() call for the + * original zhash). + */ +zhash_t * zhash_copy(const zhash_t* other); + +/** + * Determines whether the supplied key value exists as an entry in the zhash + * table. If zhash stores pointer types as keys, this function can differentiate + * between a non-existent key and a key mapped to NULL. + * Returns 1 if the supplied key exists in the zhash table, else 0. + */ +int zhash_contains(const zhash_t *zh, const void *key); + +/** + * Retrieves the value for the given key, if it exists, by copying its contents + * into the space pointed to by 'out_value', which must already be allocated. + * Returns 1 if the supplied key exists in the table, else 0, in which case + * the contents of 'out_value' will be unchanged. + */ +int zhash_get(const zhash_t *zh, const void *key, void *out_value); + +/** + * Similar to zhash_get(), but more dangerous. Provides a pointer to the zhash's + * internal storage. This can be used to make simple modifications to + * the underlying data while avoiding the memcpys associated with + * zhash_get and zhash_put. However, some zhash operations (that + * resize the underlying storage, in particular) render this pointer + * invalid. For maximum safety, call no other zhash functions for the + * period during which you intend to use the pointer. + * 'out_p' should be a pointer to the pointer which will be set to the internal + * data address. + */ +int zhash_get_volatile(const zhash_t *zh, const void *key, void *out_p); + +/** + * Adds a key/value pair to the hash table, if the supplied key does not already + * exist in the table, or overwrites the value for the supplied key if it does + * already exist. In the latter case, the previous contents of the key and value + * will be copied into the spaces pointed to by 'oldkey' and 'oldvalue', respectively, + * if they are not NULL. + * + * The key/value is added to / updated in the hash table by copying 'keysz' bytes + * from the data pointed to by 'key' and 'valuesz' bytes from the data pointed + * to by 'value'. It is up to the caller to manage the memory allocation of the + * passed-in values, zhash will store and manage a copy. + * + * NOTE: If the key is a pointer type (such as a string), the contents of the + * data that it points to must not be modified after the call to zhash_put(), + * or future zhash calls will not successfully locate the key (using either its + * previous or new value). + * + * NOTE: When using array data as a key (such as a string), the array should not + * be passed directly or it will cause a segmentation fault when it is dereferenced. + * Instead, pass a pointer which points to the array location, i.e.: + * char key[strlen]; + * char *keyptr = key; + * zhash_put(zh, &keyptr, ...) + * + * Example: + * char * key = ...; + * zarray_t * val = ...; + * char * old_key = NULL; + * zarray_t * old_val = NULL; + * if (zhash_put(zh, &key, &val, &old_key, &old_value)) + * // manage resources for old_key and old_value + * + * Returns 1 if the supplied key previously existed in the table, else 0, in + * which case the data pointed to by 'oldkey' and 'oldvalue' will be set to zero + * if they are not NULL. + */ +int zhash_put(zhash_t *zh, const void *key, const void *value, void *oldkey, void *oldvalue); + +/** + * Removes from the zhash table the key/value pair for the supplied key, if + * it exists. If it does, the contents of the key and value will be copied into + * the spaces pointed to by 'oldkey' and 'oldvalue', respectively, if they are + * not NULL. If the key does not exist, the data pointed to by 'oldkey' and + * 'oldvalue' will be set to zero if they are not NULL. + * + * Returns 1 if the key existed and was removed, else 0, indicating that the + * table contents were not changed. + */ +int zhash_remove(zhash_t *zh, const void *key, void *oldkey, void *oldvalue); + +/** + * Removes all entries in the has table to create the equivalent of starting from + * a zhash_create(), using the same size parameters. If any elements need to be + * freed manually, this will need to occur before calling clear. + */ +void zhash_clear(zhash_t *zh); + +/** + * Retrieves the current number of key/value pairs currently contained in the + * zhash table, or 0 if the table is empty. + */ +int zhash_size(const zhash_t *zh); + +/** + * Initializes an iterator which can be used to traverse the key/value pairs of + * the supplied zhash table via successive calls to zhash_iterator_next() or + * zhash_iterator_next_volatile(). The iterator can also be used to remove elements + * from the zhash with zhash_iterator_remove(). + * + * Any modifications to the zhash table structure will invalidate the + * iterator, with the exception of zhash_iterator_remove(). + */ +void zhash_iterator_init(zhash_t *zh, zhash_iterator_t *zit); + +/** + * Initializes an iterator which can be used to traverse the key/value pairs of + * the supplied zhash table via successive calls to zhash_iterator_next() or + * zhash_iterator_next_volatile(). + * + * An iterator initialized with this function cannot be used with + * zhash_iterator_remove(). For that you must use zhash_iterator_init(). + * + * Any modifications to the zhash table structure will invalidate the + * iterator. + */ +void zhash_iterator_init_const(const zhash_t *zh, zhash_iterator_t *zit); + +/** + * Retrieves the next key/value pair from a zhash table via the (previously- + * initialized) iterator. Copies the key and value data into the space + * pointed to by outkey and outvalue, respectively, if they are not NULL. + * + * Returns 1 if the call retrieved the next available key/value pair, else 0 + * indicating that no entries remain, in which case the contents of outkey and + * outvalue will remain unchanged. + */ +int zhash_iterator_next(zhash_iterator_t *zit, void *outkey, void *outvalue); + +/** + * Similar to zhash_iterator_next() except that it retrieves a pointer to zhash's + * internal storage. This can be used to avoid the memcpys associated with + * zhash_iterator_next(). Call no other zhash functions for the + * period during which you intend to use the pointer. + * 'outkey' and 'outvalue' should be pointers to the pointers which will be set + * to the internal data addresses. + * + * Example: + * key_t *outkey; + * value_t *outvalue; + * if (zhash_iterator_next_volatile(&zit, &outkey, &outvalue)) + * // access internal key and value storage via outkey and outvalue + * + * Returns 1 if the call retrieved the next available key/value pair, else 0 + * indicating that no entries remain, in which case the pointers outkey and + * outvalue will remain unchanged. + */ +int zhash_iterator_next_volatile(zhash_iterator_t *zit, void *outkey, void *outvalue); + +/** + * Removes from the zhash table the key/value pair most recently returned via + * a call to zhash_iterator_next() or zhash_iterator_next_volatile() for the + * supplied iterator. + * + * Requires that the iterator was initialized with zhash_iterator_init(), + * not zhash_iterator_init_const(). + */ +void zhash_iterator_remove(zhash_iterator_t *zit); + +/** + * Calls the supplied function with a pointer to every key in the hash table in + * turn. The function will be passed a pointer to the table's internal storage + * for the key, which the caller should not modify, as the hash table will not be + * re-indexed. The function may be NULL, in which case no action is taken. + */ +void zhash_map_keys(zhash_t *zh, void (*f)(void *)); + +/** + * Calls the supplied function with a pointer to every value in the hash table in + * turn. The function will be passed a pointer to the table's internal storage + * for the value, which the caller may safely modify. The function may be NULL, + * in which case no action is taken. + */ +void zhash_map_values(zhash_t *zh, void (*f)(void *)); + +/** + * Calls the supplied function with a copy of every key in the hash table in + * turn. While zhash_map_keys() passes a pointer to internal storage, this function + * passes a copy of the actual storage. If the zhash stores pointers to data, + * functions like free() can be used directly with zhash_vmap_keys(). + * The function may be NULL, in which case no action is taken. + * + * NOTE: zhash_vmap_keys() can only be used with pointer-data keys. + * Use with non-pointer keys (i.e. integer, double, etc.) will likely cause a + * segmentation fault. + */ +void zhash_vmap_keys(zhash_t *vh, void (*f)(void *)); + +/** + * Calls the supplied function with a copy of every value in the hash table in + * turn. While zhash_map_values() passes a pointer to internal storage, this function + * passes a copy of the actual storage. If the zhash stores pointers to data, + * functions like free() can be used directly with zhash_vmap_values(). + * The function may be NULL, in which case no action is taken. + * + * NOTE: zhash_vmap_values() can only be used with pointer-data values. + * Use with non-pointer values (i.e. integer, double, etc.) will likely cause a + * segmentation fault. + */ +void zhash_vmap_values(zhash_t *vh, void (*f)(void *)); + +/** + * Returns an array which contains copies of all of the hash table's keys, in no + * particular order. It is the caller's responsibility to call zarray_destroy() + * on the returned structure when it is no longer needed. + */ +zarray_t *zhash_keys(const zhash_t *zh); + +/** + * Returns an array which contains copies of all of the hash table's values, in no + * particular order. It is the caller's responsibility to call zarray_destroy() + * on the returned structure when it is no longer needed. + */ +zarray_t *zhash_values(const zhash_t *zh); + +/** + * Defines a hash function which will calculate a zhash value for uint32_t input + * data. Can be used with zhash_create() for a key size of sizeof(uint32_t). + */ +uint32_t zhash_uint32_hash(const void *a); + +/** + * Defines a function to compare zhash values for uint32_t input data. + * Can be used with zhash_create() for a key size of sizeof(uint32_t). + */ +int zhash_uint32_equals(const void *a, const void *b); + +/** + * Defines a hash function which will calculate a zhash value for uint64_t input + * data. Can be used with zhash_create() for a key size of sizeof(uint64_t). + */ +uint32_t zhash_uint64_hash(const void *a); + +/** + * Defines a function to compare zhash values for uint64_t input data. + * Can be used with zhash_create() for a key size of sizeof(uint64_t). + */ +int zhash_uint64_equals(const void *a, const void *b); + +///////////////////////////////////////////////////// +// functions for keys that can be compared via their pointers. +/** + * Defines a hash function which will calculate a zhash value for pointer input + * data. Can be used with zhash_create() for a key size of sizeof(void*). Will + * use only the pointer value itself for computing the hash value. + */ +uint32_t zhash_ptr_hash(const void *a); + +/** + * Defines a function to compare zhash values for pointer input data. + * Can be used with zhash_create() for a key size of sizeof(void*). + */ +int zhash_ptr_equals(const void *a, const void *b); + +///////////////////////////////////////////////////// +// Functions for string-typed keys +/** + * Defines a hash function which will calculate a zhash value for string input + * data. Can be used with zhash_create() for a key size of sizeof(char*). Will + * use the contents of the string in computing the hash value. + */ +uint32_t zhash_str_hash(const void *a); + +/** + * Defines a function to compare zhash values for string input data. + * Can be used with zhash_create() for a key size of sizeof(char*). + */ +int zhash_str_equals(const void *a, const void *b); + +void zhash_debug(zhash_t *zh); + + static inline zhash_t *zhash_str_str_create(void) + { + return zhash_create(sizeof(char*), sizeof(char*), + zhash_str_hash, zhash_str_equals); + } + + + +// for zhashes that map strings to strings, this is a convenience +// function that allows easier retrieval of values. NULL is returned +// if the key is not found. +static inline char *zhash_str_str_get(zhash_t *zh, const char *key) +{ + char *value; + if (zhash_get(zh, &key, &value)) + return value; + return NULL; +} + + static inline void zhash_str_str_put(zhash_t *zh, char *key, char *value) + { + char *oldkey, *oldval; + if (zhash_put(zh, &key, &value, &oldkey, &oldval)) { + free(oldkey); + free(oldval); + } + } + + static inline void zhash_str_str_destroy(zhash_t *zh) + { + zhash_iterator_t zit; + zhash_iterator_init(zh, &zit); + + char *key, *value; + while (zhash_iterator_next(&zit, &key, &value)) { + free(key); + free(value); + } + + zhash_destroy(zh); + } + + +static inline uint32_t zhash_int_hash(const void *_a) +{ + assert(_a != NULL); + + uint32_t a = *((int*) _a); + return a; +} + +static inline int zhash_int_equals(const void *_a, const void *_b) +{ + assert(_a != NULL); + assert(_b != NULL); + + int a = *((int*) _a); + int b = *((int*) _b); + + return a==b; +} + +#ifdef __cplusplus +} +#endif diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/common/zmaxheap.h b/apriltag/src/main/native/thirdparty/apriltag/include/common/zmaxheap.h new file mode 100644 index 0000000000..af9d1458c5 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/common/zmaxheap.h @@ -0,0 +1,76 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#pragma once + +#include + +typedef struct zmaxheap zmaxheap_t; + +typedef struct zmaxheap_iterator zmaxheap_iterator_t; +struct zmaxheap_iterator { + zmaxheap_t *heap; + int in, out; +}; + +zmaxheap_t *zmaxheap_create(size_t el_sz); + +void zmaxheap_vmap(zmaxheap_t *heap, void (*f)(void*)); + +void zmaxheap_destroy(zmaxheap_t *heap); + +void zmaxheap_add(zmaxheap_t *heap, void *p, float v); + +int zmaxheap_size(zmaxheap_t *heap); + +// returns 0 if the heap is empty, so you can do +// while (zmaxheap_remove_max(...)) { } +int zmaxheap_remove_max(zmaxheap_t *heap, void *p, float *v); + +//////////////////////////////////////////// +// This is a peculiar iterator intended to support very specific (and +// unusual) applications, and the heap is not necessarily in a valid +// state until zmaxheap_iterator_finish is called. Consequently, do +// not call any other methods on the heap while iterating through. + +// you must provide your own storage for the iterator, and pass in a +// pointer. +void zmaxheap_iterator_init(zmaxheap_t *heap, zmaxheap_iterator_t *it); + +// Traverses the heap in top-down/left-right order. makes a copy of +// the content into memory (p) that you provide. +int zmaxheap_iterator_next(zmaxheap_iterator_t *it, void *p, float *v); + +// will set p to be a pointer to the heap's internal copy of the dfata. +int zmaxheap_iterator_next_volatile(zmaxheap_iterator_t *it, void *p, float *v); + +// remove the current element. +void zmaxheap_iterator_remove(zmaxheap_iterator_t *it); + +// call after all iterator operations are done. After calling this, +// the iterator should no longer be used, but the heap methods can be. +void zmaxheap_iterator_finish(zmaxheap_iterator_t *it); diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/tag16h5.h b/apriltag/src/main/native/thirdparty/apriltag/include/tag16h5.h new file mode 100644 index 0000000000..6661080b66 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/tag16h5.h @@ -0,0 +1,44 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#ifndef _TAG16H5 +#define _TAG16H5 + +#include "apriltag.h" + +#ifdef __cplusplus +extern "C" { +#endif + +apriltag_family_t *tag16h5_create(void); +void tag16h5_destroy(apriltag_family_t *tf); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/apriltag/src/main/native/thirdparty/apriltag/include/tag36h11.h b/apriltag/src/main/native/thirdparty/apriltag/include/tag36h11.h new file mode 100644 index 0000000000..45f9e5698e --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/include/tag36h11.h @@ -0,0 +1,44 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#ifndef _TAG36H11 +#define _TAG36H11 + +#include "apriltag.h" + +#ifdef __cplusplus +extern "C" { +#endif + +apriltag_family_t *tag36h11_create(void); +void tag36h11_destroy(apriltag_family_t *tf); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/apriltag.c b/apriltag/src/main/native/thirdparty/apriltag/src/apriltag.c new file mode 100644 index 0000000000..bfff1ea9d5 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/apriltag.c @@ -0,0 +1,1448 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. + +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#include "apriltag.h" + +#include +#include +#include +#include +#include +#include + +#include "common/image_u8.h" +#include "common/image_u8x3.h" +#include "common/zarray.h" +#include "common/matd.h" +#include "common/homography.h" +#include "common/timeprofile.h" +#include "common/math_util.h" +#include "common/g2d.h" +#include "common/debug_print.h" + +#include "apriltag_math.h" + +#ifndef M_PI +# define M_PI 3.141592653589793238462643383279502884196 +#endif + +#ifdef _WIN32 +static inline void srandom(unsigned int seed) +{ + srand(seed); +} + +static inline long int random(void) +{ + return rand(); +} +#endif + +#define APRILTAG_U64_ONE ((uint64_t) 1) + +extern zarray_t *apriltag_quad_thresh(apriltag_detector_t *td, image_u8_t *im); + +// Regresses a model of the form: +// intensity(x,y) = C0*x + C1*y + CC2 +// The J matrix is the: +// J = [ x1 y1 1 ] +// [ x2 y2 1 ] +// [ ... ] +// The A matrix is J'J + +struct graymodel +{ + double A[3][3]; + double B[3]; + double C[3]; +}; + +static void graymodel_init(struct graymodel *gm) +{ + memset(gm, 0, sizeof(struct graymodel)); +} + +static void graymodel_add(struct graymodel *gm, double x, double y, double gray) +{ + // update upper right entries of A = J'J + gm->A[0][0] += x*x; + gm->A[0][1] += x*y; + gm->A[0][2] += x; + gm->A[1][1] += y*y; + gm->A[1][2] += y; + gm->A[2][2] += 1; + + // update B = J'gray + gm->B[0] += x * gray; + gm->B[1] += y * gray; + gm->B[2] += gray; +} + +static void graymodel_solve(struct graymodel *gm) +{ + mat33_sym_solve((double*) gm->A, gm->B, gm->C); +} + +static double graymodel_interpolate(struct graymodel *gm, double x, double y) +{ + return gm->C[0]*x + gm->C[1]*y + gm->C[2]; +} + +struct quick_decode_entry +{ + uint64_t rcode; // the queried code + uint16_t id; // the tag ID (a small integer) + uint8_t hamming; // how many errors corrected? + uint8_t rotation; // number of rotations [0, 3] +}; + +struct quick_decode +{ + int nentries; + struct quick_decode_entry *entries; +}; + +/** + * Assuming we are drawing the image one quadrant at a time, what would the rotated image look like? + * Special care is taken to handle the case where there is a middle pixel of the image. + */ +static uint64_t rotate90(uint64_t w, int numBits) +{ + int p = numBits; + uint64_t l = 0; + if (numBits % 4 == 1) { + p = numBits - 1; + l = 1; + } + w = ((w >> l) << (p/4 + l)) | (w >> (3 * p/ 4 + l) << l) | (w & l); + w &= ((APRILTAG_U64_ONE << numBits) - 1); + return w; +} + +static void quad_destroy(struct quad *quad) +{ + if (!quad) + return; + + matd_destroy(quad->H); + matd_destroy(quad->Hinv); + free(quad); +} + +static struct quad *quad_copy(struct quad *quad) +{ + struct quad *q = calloc(1, sizeof(struct quad)); + memcpy(q, quad, sizeof(struct quad)); + if (quad->H) + q->H = matd_copy(quad->H); + if (quad->Hinv) + q->Hinv = matd_copy(quad->Hinv); + return q; +} + +static void quick_decode_add(struct quick_decode *qd, uint64_t code, int id, int hamming) +{ + uint32_t bucket = code % qd->nentries; + + while (qd->entries[bucket].rcode != UINT64_MAX) { + bucket = (bucket + 1) % qd->nentries; + } + + qd->entries[bucket].rcode = code; + qd->entries[bucket].id = id; + qd->entries[bucket].hamming = hamming; +} + +static void quick_decode_uninit(apriltag_family_t *fam) +{ + if (!fam->impl) + return; + + struct quick_decode *qd = (struct quick_decode*) fam->impl; + free(qd->entries); + free(qd); + fam->impl = NULL; +} + +static void quick_decode_init(apriltag_family_t *family, int maxhamming) +{ + assert(family->impl == NULL); + assert(family->ncodes < 65536); + + struct quick_decode *qd = calloc(1, sizeof(struct quick_decode)); + int capacity = family->ncodes; + + int nbits = family->nbits; + + if (maxhamming >= 1) + capacity += family->ncodes * nbits; + + if (maxhamming >= 2) + capacity += family->ncodes * nbits * (nbits-1); + + if (maxhamming >= 3) + capacity += family->ncodes * nbits * (nbits-1) * (nbits-2); + + qd->nentries = capacity * 3; + +// debug_print("capacity %d, size: %.0f kB\n", +// capacity, qd->nentries * sizeof(struct quick_decode_entry) / 1024.0); + + qd->entries = calloc(qd->nentries, sizeof(struct quick_decode_entry)); + if (qd->entries == NULL) { + debug_print("Failed to allocate hamming decode table\n"); + // errno already set to ENOMEM (Error No MEMory) by calloc() failure + return; + } + + for (int i = 0; i < qd->nentries; i++) + qd->entries[i].rcode = UINT64_MAX; + + errno = 0; + + for (int i = 0; i < family->ncodes; i++) { + uint64_t code = family->codes[i]; + + // add exact code (hamming = 0) + quick_decode_add(qd, code, i, 0); + + if (maxhamming >= 1) { + // add hamming 1 + for (int j = 0; j < nbits; j++) + quick_decode_add(qd, code ^ (APRILTAG_U64_ONE << j), i, 1); + } + + if (maxhamming >= 2) { + // add hamming 2 + for (int j = 0; j < nbits; j++) + for (int k = 0; k < j; k++) + quick_decode_add(qd, code ^ (APRILTAG_U64_ONE << j) ^ (APRILTAG_U64_ONE << k), i, 2); + } + + if (maxhamming >= 3) { + // add hamming 3 + for (int j = 0; j < nbits; j++) + for (int k = 0; k < j; k++) + for (int m = 0; m < k; m++) + quick_decode_add(qd, code ^ (APRILTAG_U64_ONE << j) ^ (APRILTAG_U64_ONE << k) ^ (APRILTAG_U64_ONE << m), i, 3); + } + + if (maxhamming > 3) { + debug_print("\"maxhamming\" beyond 3 not supported\n"); + // set errno to Error INvalid VALue + errno = EINVAL; + return; + } + } + + family->impl = qd; + + #if 0 + int longest_run = 0; + int run = 0; + int run_sum = 0; + int run_count = 0; + + // This accounting code doesn't check the last possible run that + // occurs at the wrap-around. That's pretty insignificant. + for (int i = 0; i < qd->nentries; i++) { + if (qd->entries[i].rcode == UINT64_MAX) { + if (run > 0) { + run_sum += run; + run_count ++; + } + run = 0; + } else { + run ++; + longest_run = imax(longest_run, run); + } + } + + printf("quick decode: longest run: %d, average run %.3f\n", longest_run, 1.0 * run_sum / run_count); + #endif +} + +// returns an entry with hamming set to 255 if no decode was found. +static void quick_decode_codeword(apriltag_family_t *tf, uint64_t rcode, + struct quick_decode_entry *entry) +{ + struct quick_decode *qd = (struct quick_decode*) tf->impl; + + // qd might be null if detector_add_family_bits() failed + for (int ridx = 0; qd != NULL && ridx < 4; ridx++) { + + for (int bucket = rcode % qd->nentries; + qd->entries[bucket].rcode != UINT64_MAX; + bucket = (bucket + 1) % qd->nentries) { + + if (qd->entries[bucket].rcode == rcode) { + *entry = qd->entries[bucket]; + entry->rotation = ridx; + return; + } + } + + rcode = rotate90(rcode, tf->nbits); + } + + entry->rcode = 0; + entry->id = 65535; + entry->hamming = 255; + entry->rotation = 0; +} + +static inline int detection_compare_function(const void *_a, const void *_b) +{ + apriltag_detection_t *a = *(apriltag_detection_t**) _a; + apriltag_detection_t *b = *(apriltag_detection_t**) _b; + + return a->id - b->id; +} + +void apriltag_detector_remove_family(apriltag_detector_t *td, apriltag_family_t *fam) +{ + quick_decode_uninit(fam); + zarray_remove_value(td->tag_families, &fam, 0); +} + +void apriltag_detector_add_family_bits(apriltag_detector_t *td, apriltag_family_t *fam, int bits_corrected) +{ + zarray_add(td->tag_families, &fam); + + if (!fam->impl) + quick_decode_init(fam, bits_corrected); +} + +void apriltag_detector_clear_families(apriltag_detector_t *td) +{ + for (int i = 0; i < zarray_size(td->tag_families); i++) { + apriltag_family_t *fam; + zarray_get(td->tag_families, i, &fam); + quick_decode_uninit(fam); + } + zarray_clear(td->tag_families); +} + +apriltag_detector_t *apriltag_detector_create(void) +{ + apriltag_detector_t *td = (apriltag_detector_t*) calloc(1, sizeof(apriltag_detector_t)); + + td->nthreads = 1; + td->quad_decimate = 2.0; + td->quad_sigma = 0.0; + + td->qtp.max_nmaxima = 10; + td->qtp.min_cluster_pixels = 5; + + td->qtp.max_line_fit_mse = 10.0; + td->qtp.cos_critical_rad = cos(10 * M_PI / 180); + td->qtp.deglitch = false; + td->qtp.min_white_black_diff = 5; + + td->tag_families = zarray_create(sizeof(apriltag_family_t*)); + + pthread_mutex_init(&td->mutex, NULL); + + td->tp = timeprofile_create(); + + td->refine_edges = true; + td->decode_sharpening = 0.25; + + + td->debug = false; + + // NB: defer initialization of td->wp so that the user can + // override td->nthreads. + + return td; +} + +void apriltag_detector_destroy(apriltag_detector_t *td) +{ + timeprofile_destroy(td->tp); + workerpool_destroy(td->wp); + + apriltag_detector_clear_families(td); + + zarray_destroy(td->tag_families); + free(td); +} + +struct quad_decode_task +{ + int i0, i1; + zarray_t *quads; + apriltag_detector_t *td; + + image_u8_t *im; + zarray_t *detections; + + image_u8_t *im_samples; +}; + +struct evaluate_quad_ret +{ + int64_t rcode; + double score; + matd_t *H, *Hinv; + + int decode_status; + struct quick_decode_entry e; +}; + +static matd_t* homography_compute2(double c[4][4]) { + double A[] = { + c[0][0], c[0][1], 1, 0, 0, 0, -c[0][0]*c[0][2], -c[0][1]*c[0][2], c[0][2], + 0, 0, 0, c[0][0], c[0][1], 1, -c[0][0]*c[0][3], -c[0][1]*c[0][3], c[0][3], + c[1][0], c[1][1], 1, 0, 0, 0, -c[1][0]*c[1][2], -c[1][1]*c[1][2], c[1][2], + 0, 0, 0, c[1][0], c[1][1], 1, -c[1][0]*c[1][3], -c[1][1]*c[1][3], c[1][3], + c[2][0], c[2][1], 1, 0, 0, 0, -c[2][0]*c[2][2], -c[2][1]*c[2][2], c[2][2], + 0, 0, 0, c[2][0], c[2][1], 1, -c[2][0]*c[2][3], -c[2][1]*c[2][3], c[2][3], + c[3][0], c[3][1], 1, 0, 0, 0, -c[3][0]*c[3][2], -c[3][1]*c[3][2], c[3][2], + 0, 0, 0, c[3][0], c[3][1], 1, -c[3][0]*c[3][3], -c[3][1]*c[3][3], c[3][3], + }; + + double epsilon = 1e-10; + + // Eliminate. + for (int col = 0; col < 8; col++) { + // Find best row to swap with. + double max_val = 0; + int max_val_idx = -1; + for (int row = col; row < 8; row++) { + double val = fabs(A[row*9 + col]); + if (val > max_val) { + max_val = val; + max_val_idx = row; + } + } + + if (max_val < epsilon) { + debug_print("WRN: Matrix is singular.\n"); + return NULL; + } + + // Swap to get best row. + if (max_val_idx != col) { + for (int i = col; i < 9; i++) { + double tmp = A[col*9 + i]; + A[col*9 + i] = A[max_val_idx*9 + i]; + A[max_val_idx*9 + i] = tmp; + } + } + + // Do eliminate. + for (int i = col + 1; i < 8; i++) { + double f = A[i*9 + col]/A[col*9 + col]; + A[i*9 + col] = 0; + for (int j = col + 1; j < 9; j++) { + A[i*9 + j] -= f*A[col*9 + j]; + } + } + } + + // Back solve. + for (int col = 7; col >=0; col--) { + double sum = 0; + for (int i = col + 1; i < 8; i++) { + sum += A[col*9 + i]*A[i*9 + 8]; + } + A[col*9 + 8] = (A[col*9 + 8] - sum)/A[col*9 + col]; + } + return matd_create_data(3, 3, (double[]) { A[8], A[17], A[26], A[35], A[44], A[53], A[62], A[71], 1 }); +} + +// returns non-zero if an error occurs (i.e., H has no inverse) +static int quad_update_homographies(struct quad *quad) +{ + //zarray_t *correspondences = zarray_create(sizeof(float[4])); + + double corr_arr[4][4]; + + for (int i = 0; i < 4; i++) { + corr_arr[i][0] = (i==0 || i==3) ? -1 : 1; + corr_arr[i][1] = (i==0 || i==1) ? -1 : 1; + corr_arr[i][2] = quad->p[i][0]; + corr_arr[i][3] = quad->p[i][1]; + } + + if (quad->H) + matd_destroy(quad->H); + if (quad->Hinv) + matd_destroy(quad->Hinv); + + // XXX Tunable + quad->H = homography_compute2(corr_arr); + if (quad->H != NULL) { + quad->Hinv = matd_inverse(quad->H); + if (quad->Hinv != NULL) { + // Success! + return 0; + } + matd_destroy(quad->H); + quad->H = NULL; + } + return -1; +} + +static double value_for_pixel(image_u8_t *im, double px, double py) { + int x1 = floor(px - 0.5); + int x2 = ceil(px - 0.5); + double x = px - 0.5 - x1; + int y1 = floor(py - 0.5); + int y2 = ceil(py - 0.5); + double y = py - 0.5 - y1; + if (x1 < 0 || x2 >= im->width || y1 < 0 || y2 >= im->height) { + return -1; + } + return im->buf[y1*im->stride + x1]*(1-x)*(1-y) + + im->buf[y1*im->stride + x2]*x*(1-y) + + im->buf[y2*im->stride + x1]*(1-x)*y + + im->buf[y2*im->stride + x2]*x*y; +} + +static void sharpen(apriltag_detector_t* td, double* values, int size) { + double *sharpened = malloc(sizeof(double)*size*size); + double kernel[9] = { + 0, -1, 0, + -1, 4, -1, + 0, -1, 0 + }; + + for (int y = 0; y < size; y++) { + for (int x = 0; x < size; x++) { + sharpened[y*size + x] = 0; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + if ((y + i - 1) < 0 || (y + i - 1) > size - 1 || (x + j - 1) < 0 || (x + j - 1) > size - 1) { + continue; + } + sharpened[y*size + x] += values[(y + i - 1)*size + (x + j - 1)]*kernel[i*3 + j]; + } + } + } + } + + + for (int y = 0; y < size; y++) { + for (int x = 0; x < size; x++) { + values[y*size + x] = values[y*size + x] + td->decode_sharpening*sharpened[y*size + x]; + } + } + + free(sharpened); +} + +// returns the decision margin. Return < 0 if the detection should be rejected. +static float quad_decode(apriltag_detector_t* td, apriltag_family_t *family, image_u8_t *im, struct quad *quad, struct quick_decode_entry *entry, image_u8_t *im_samples) +{ + // decode the tag binary contents by sampling the pixel + // closest to the center of each bit cell. + + // We will compute a threshold by sampling known white/black cells around this tag. + // This sampling is achieved by considering a set of samples along lines. + // + // coordinates are given in bit coordinates. ([0, fam->border_width]). + // + // { initial x, initial y, delta x, delta y, WHITE=1 } + float patterns[] = { + // left white column + -0.5, 0.5, + 0, 1, + 1, + + // left black column + 0.5, 0.5, + 0, 1, + 0, + + // right white column + family->width_at_border + 0.5, .5, + 0, 1, + 1, + + // right black column + family->width_at_border - 0.5, .5, + 0, 1, + 0, + + // top white row + 0.5, -0.5, + 1, 0, + 1, + + // top black row + 0.5, 0.5, + 1, 0, + 0, + + // bottom white row + 0.5, family->width_at_border + 0.5, + 1, 0, + 1, + + // bottom black row + 0.5, family->width_at_border - 0.5, + 1, 0, + 0 + + // XXX double-counts the corners. + }; + + struct graymodel whitemodel, blackmodel; + graymodel_init(&whitemodel); + graymodel_init(&blackmodel); + + for (int pattern_idx = 0; pattern_idx < sizeof(patterns)/(5*sizeof(float)); pattern_idx ++) { + float *pattern = &patterns[pattern_idx * 5]; + + int is_white = pattern[4]; + + for (int i = 0; i < family->width_at_border; i++) { + double tagx01 = (pattern[0] + i*pattern[2]) / (family->width_at_border); + double tagy01 = (pattern[1] + i*pattern[3]) / (family->width_at_border); + + double tagx = 2*(tagx01-0.5); + double tagy = 2*(tagy01-0.5); + + double px, py; + homography_project(quad->H, tagx, tagy, &px, &py); + + // don't round + int ix = px; + int iy = py; + if (ix < 0 || iy < 0 || ix >= im->width || iy >= im->height) + continue; + + int v = im->buf[iy*im->stride + ix]; + + if (im_samples) { + im_samples->buf[iy*im_samples->stride + ix] = (1-is_white)*255; + } + + if (is_white) + graymodel_add(&whitemodel, tagx, tagy, v); + else + graymodel_add(&blackmodel, tagx, tagy, v); + } + } + + if (family->width_at_border > 1) { + graymodel_solve(&whitemodel); + graymodel_solve(&blackmodel); + } else { + graymodel_solve(&whitemodel); + blackmodel.C[0] = 0; + blackmodel.C[1] = 0; + blackmodel.C[2] = blackmodel.B[2]/4; + } + + // XXX Tunable + if ((graymodel_interpolate(&whitemodel, 0, 0) - graymodel_interpolate(&blackmodel, 0, 0) < 0) != family->reversed_border) { + return -1; + } + + // compute the average decision margin (how far was each bit from + // the decision boundary? + // + // we score this separately for white and black pixels and return + // the minimum average threshold for black/white pixels. This is + // to penalize thresholds that are too close to an extreme. + float black_score = 0, white_score = 0; + float black_score_count = 1, white_score_count = 1; + + double *values = calloc(family->total_width*family->total_width, sizeof(double)); + + int min_coord = (family->width_at_border - family->total_width)/2; + for (int i = 0; i < family->nbits; i++) { + int bity = family->bit_y[i]; + int bitx = family->bit_x[i]; + + double tagx01 = (bitx + 0.5) / (family->width_at_border); + double tagy01 = (bity + 0.5) / (family->width_at_border); + + // scale to [-1, 1] + double tagx = 2*(tagx01-0.5); + double tagy = 2*(tagy01-0.5); + + double px, py; + homography_project(quad->H, tagx, tagy, &px, &py); + + double v = value_for_pixel(im, px, py); + + if (v == -1) { + continue; + } + + double thresh = (graymodel_interpolate(&blackmodel, tagx, tagy) + graymodel_interpolate(&whitemodel, tagx, tagy)) / 2.0; + values[family->total_width*(bity - min_coord) + bitx - min_coord] = v - thresh; + + if (im_samples) { + int ix = px; + int iy = py; + im_samples->buf[iy*im_samples->stride + ix] = (v < thresh) * 255; + } + } + + sharpen(td, values, family->total_width); + + uint64_t rcode = 0; + for (int i = 0; i < family->nbits; i++) { + int bity = family->bit_y[i]; + int bitx = family->bit_x[i]; + rcode = (rcode << 1); + double v = values[(bity - min_coord)*family->total_width + bitx - min_coord]; + + if (v > 0) { + white_score += v; + white_score_count++; + rcode |= 1; + } else { + black_score -= v; + black_score_count++; + } + } + + quick_decode_codeword(family, rcode, entry); + free(values); + return fmin(white_score / white_score_count, black_score / black_score_count); +} + +static void refine_edges(apriltag_detector_t *td, image_u8_t *im_orig, struct quad *quad) +{ + double lines[4][4]; // for each line, [Ex Ey nx ny] + + for (int edge = 0; edge < 4; edge++) { + int a = edge, b = (edge + 1) & 3; // indices of the end points. + + // compute the normal to the current line estimate + double nx = quad->p[b][1] - quad->p[a][1]; + double ny = -quad->p[b][0] + quad->p[a][0]; + double mag = sqrt(nx*nx + ny*ny); + nx /= mag; + ny /= mag; + + if (quad->reversed_border) { + nx = -nx; + ny = -ny; + } + + // we will now fit a NEW line by sampling points near + // our original line that have large gradients. On really big tags, + // we're willing to sample more to get an even better estimate. + int nsamples = imax(16, mag / 8); // XXX tunable + + // stats for fitting a line... + double Mx = 0, My = 0, Mxx = 0, Mxy = 0, Myy = 0, N = 0; + + for (int s = 0; s < nsamples; s++) { + // compute a point along the line... Note, we're avoiding + // sampling *right* at the corners, since those points are + // the least reliable. + double alpha = (1.0 + s) / (nsamples + 1); + double x0 = alpha*quad->p[a][0] + (1-alpha)*quad->p[b][0]; + double y0 = alpha*quad->p[a][1] + (1-alpha)*quad->p[b][1]; + + // search along the normal to this line, looking at the + // gradients along the way. We're looking for a strong + // response. + double Mn = 0; + double Mcount = 0; + + // XXX tunable: how far to search? We want to search far + // enough that we find the best edge, but not so far that + // we hit other edges that aren't part of the tag. We + // shouldn't ever have to search more than quad_decimate, + // since otherwise we would (ideally) have started our + // search on another pixel in the first place. Likewise, + // for very small tags, we don't want the range to be too + // big. + double range = td->quad_decimate + 1; + + // XXX tunable step size. + for (double n = -range; n <= range; n += 0.25) { + // Because of the guaranteed winding order of the + // points in the quad, we will start inside the white + // portion of the quad and work our way outward. + // + // sample to points (x1,y1) and (x2,y2) XXX tunable: + // how far +/- to look? Small values compute the + // gradient more precisely, but are more sensitive to + // noise. + double grange = 1; + int x1 = x0 + (n + grange)*nx; + int y1 = y0 + (n + grange)*ny; + if (x1 < 0 || x1 >= im_orig->width || y1 < 0 || y1 >= im_orig->height) + continue; + + int x2 = x0 + (n - grange)*nx; + int y2 = y0 + (n - grange)*ny; + if (x2 < 0 || x2 >= im_orig->width || y2 < 0 || y2 >= im_orig->height) + continue; + + int g1 = im_orig->buf[y1*im_orig->stride + x1]; + int g2 = im_orig->buf[y2*im_orig->stride + x2]; + + if (g1 < g2) // reject points whose gradient is "backwards". They can only hurt us. + continue; + + double weight = (g2 - g1)*(g2 - g1); // XXX tunable. What shape for weight=f(g2-g1)? + + // compute weighted average of the gradient at this point. + Mn += weight*n; + Mcount += weight; + } + + // what was the average point along the line? + if (Mcount == 0) + continue; + + double n0 = Mn / Mcount; + + // where is the point along the line? + double bestx = x0 + n0*nx; + double besty = y0 + n0*ny; + + // update our line fit statistics + Mx += bestx; + My += besty; + Mxx += bestx*bestx; + Mxy += bestx*besty; + Myy += besty*besty; + N++; + } + + // fit a line + double Ex = Mx / N, Ey = My / N; + double Cxx = Mxx / N - Ex*Ex; + double Cxy = Mxy / N - Ex*Ey; + double Cyy = Myy / N - Ey*Ey; + + // TODO: Can replace this with same code as in fit_line. + double normal_theta = .5 * atan2f(-2*Cxy, (Cyy - Cxx)); + nx = cosf(normal_theta); + ny = sinf(normal_theta); + lines[edge][0] = Ex; + lines[edge][1] = Ey; + lines[edge][2] = nx; + lines[edge][3] = ny; + } + + // now refit the corners of the quad + for (int i = 0; i < 4; i++) { + + // solve for the intersection of lines (i) and (i+1)&3. + double A00 = lines[i][3], A01 = -lines[(i+1)&3][3]; + double A10 = -lines[i][2], A11 = lines[(i+1)&3][2]; + double B0 = -lines[i][0] + lines[(i+1)&3][0]; + double B1 = -lines[i][1] + lines[(i+1)&3][1]; + + double det = A00 * A11 - A10 * A01; + + // inverse. + if (fabs(det) > 0.001) { + // solve + double W00 = A11 / det, W01 = -A01 / det; + + double L0 = W00*B0 + W01*B1; + + // Compute intersection. Note that line i represents the line from corner i to (i+1)&3, so + // the intersection of line i with line (i+1)&3 represents corner (i+1)&3. + quad->p[(i+1)&3][0] = lines[i][0] + L0*A00; + quad->p[(i+1)&3][1] = lines[i][1] + L0*A10; + } else { + // this is a bad sign. We'll just keep the corner we had. +// debug_print("bad det: %15f %15f %15f %15f %15f\n", A00, A11, A10, A01, det); + } + } +} + +static void quad_decode_task(void *_u) +{ + struct quad_decode_task *task = (struct quad_decode_task*) _u; + apriltag_detector_t *td = task->td; + image_u8_t *im = task->im; + + for (int quadidx = task->i0; quadidx < task->i1; quadidx++) { + struct quad *quad_original; + zarray_get_volatile(task->quads, quadidx, &quad_original); + + // refine edges is not dependent upon the tag family, thus + // apply this optimization BEFORE the other work. + //if (td->quad_decimate > 1 && td->refine_edges) { + if (td->refine_edges) { + refine_edges(td, im, quad_original); + } + + // make sure the homographies are computed... + if (quad_update_homographies(quad_original) != 0) + continue; + + for (int famidx = 0; famidx < zarray_size(td->tag_families); famidx++) { + apriltag_family_t *family; + zarray_get(td->tag_families, famidx, &family); + + if (family->reversed_border != quad_original->reversed_border) { + continue; + } + + // since the geometry of tag families can vary, start any + // optimization process over with the original quad. + struct quad *quad = quad_copy(quad_original); + + struct quick_decode_entry entry; + + float decision_margin = quad_decode(td, family, im, quad, &entry, task->im_samples); + + if (decision_margin >= 0 && entry.hamming < 255) { + apriltag_detection_t *det = calloc(1, sizeof(apriltag_detection_t)); + + det->family = family; + det->id = entry.id; + det->hamming = entry.hamming; + det->decision_margin = decision_margin; + + double theta = entry.rotation * M_PI / 2.0; + double c = cos(theta), s = sin(theta); + + // Fix the rotation of our homography to properly orient the tag + matd_t *R = matd_create(3,3); + MATD_EL(R, 0, 0) = c; + MATD_EL(R, 0, 1) = -s; + MATD_EL(R, 1, 0) = s; + MATD_EL(R, 1, 1) = c; + MATD_EL(R, 2, 2) = 1; + + det->H = matd_op("M*M", quad->H, R); + + matd_destroy(R); + + homography_project(det->H, 0, 0, &det->c[0], &det->c[1]); + + // [-1, -1], [1, -1], [1, 1], [-1, 1], Desired points + // [-1, 1], [1, 1], [1, -1], [-1, -1], FLIP Y + // adjust the points in det->p so that they correspond to + // counter-clockwise around the quad, starting at -1,-1. + for (int i = 0; i < 4; i++) { + int tcx = (i == 1 || i == 2) ? 1 : -1; + int tcy = (i < 2) ? 1 : -1; + + double p[2]; + + homography_project(det->H, tcx, tcy, &p[0], &p[1]); + + det->p[i][0] = p[0]; + det->p[i][1] = p[1]; + } + + pthread_mutex_lock(&td->mutex); + zarray_add(task->detections, &det); + pthread_mutex_unlock(&td->mutex); + } + + quad_destroy(quad); + } + } +} + +void apriltag_detection_destroy(apriltag_detection_t *det) +{ + if (det == NULL) + return; + + matd_destroy(det->H); + free(det); +} + +static int prefer_smaller(int pref, double q0, double q1) +{ + if (pref) // already prefer something? exit. + return pref; + + if (q0 < q1) + return -1; // we now prefer q0 + if (q1 < q0) + return 1; // we now prefer q1 + + // no preference + return 0; +} + +zarray_t *apriltag_detector_detect(apriltag_detector_t *td, image_u8_t *im_orig) +{ + if (zarray_size(td->tag_families) == 0) { + zarray_t *s = zarray_create(sizeof(apriltag_detection_t*)); + debug_print("No tag families enabled\n"); + return s; + } + + if (td->wp == NULL || td->nthreads != workerpool_get_nthreads(td->wp)) { + workerpool_destroy(td->wp); + td->wp = workerpool_create(td->nthreads); + if (td->wp == NULL) { + // creating workerpool failed - return empty zarray + return zarray_create(sizeof(apriltag_detection_t*)); + } + } + + timeprofile_clear(td->tp); + timeprofile_stamp(td->tp, "init"); + + /////////////////////////////////////////////////////////// + // Step 1. Detect quads according to requested image decimation + // and blurring parameters. + image_u8_t *quad_im = im_orig; + if (td->quad_decimate > 1) { + quad_im = image_u8_decimate(im_orig, td->quad_decimate); + + timeprofile_stamp(td->tp, "decimate"); + } + + if (td->quad_sigma != 0) { + // compute a reasonable kernel width by figuring that the + // kernel should go out 2 std devs. + // + // max sigma ksz + // 0.499 1 (disabled) + // 0.999 3 + // 1.499 5 + // 1.999 7 + + float sigma = fabsf((float) td->quad_sigma); + + int ksz = 4 * sigma; // 2 std devs in each direction + if ((ksz & 1) == 0) + ksz++; + + if (ksz > 1) { + + if (td->quad_sigma > 0) { + // Apply a blur + image_u8_gaussian_blur(quad_im, sigma, ksz); + } else { + // SHARPEN the image by subtracting the low frequency components. + image_u8_t *orig = image_u8_copy(quad_im); + image_u8_gaussian_blur(quad_im, sigma, ksz); + + for (int y = 0; y < orig->height; y++) { + for (int x = 0; x < orig->width; x++) { + int vorig = orig->buf[y*orig->stride + x]; + int vblur = quad_im->buf[y*quad_im->stride + x]; + + int v = 2*vorig - vblur; + if (v < 0) + v = 0; + if (v > 255) + v = 255; + + quad_im->buf[y*quad_im->stride + x] = (uint8_t) v; + } + } + image_u8_destroy(orig); + } + } + } + + timeprofile_stamp(td->tp, "blur/sharp"); + + if (td->debug) + image_u8_write_pnm(quad_im, "debug_preprocess.pnm"); + + zarray_t *quads = apriltag_quad_thresh(td, quad_im); + + // adjust centers of pixels so that they correspond to the + // original full-resolution image. + if (td->quad_decimate > 1) { + for (int i = 0; i < zarray_size(quads); i++) { + struct quad *q; + zarray_get_volatile(quads, i, &q); + + for (int j = 0; j < 4; j++) { + if (td->quad_decimate == 1.5) { + q->p[j][0] *= td->quad_decimate; + q->p[j][1] *= td->quad_decimate; + } else { + q->p[j][0] = (q->p[j][0] - 0.5)*td->quad_decimate + 0.5; + q->p[j][1] = (q->p[j][1] - 0.5)*td->quad_decimate + 0.5; + } + } + } + } + + if (quad_im != im_orig) + image_u8_destroy(quad_im); + + zarray_t *detections = zarray_create(sizeof(apriltag_detection_t*)); + + td->nquads = zarray_size(quads); + + timeprofile_stamp(td->tp, "quads"); + + if (td->debug) { + image_u8_t *im_quads = image_u8_copy(im_orig); + image_u8_darken(im_quads); + image_u8_darken(im_quads); + + srandom(0); + + for (int i = 0; i < zarray_size(quads); i++) { + struct quad *quad; + zarray_get_volatile(quads, i, &quad); + + const int bias = 100; + int color = bias + (random() % (255-bias)); + + image_u8_draw_line(im_quads, quad->p[0][0], quad->p[0][1], quad->p[1][0], quad->p[1][1], color, 1); + image_u8_draw_line(im_quads, quad->p[1][0], quad->p[1][1], quad->p[2][0], quad->p[2][1], color, 1); + image_u8_draw_line(im_quads, quad->p[2][0], quad->p[2][1], quad->p[3][0], quad->p[3][1], color, 1); + image_u8_draw_line(im_quads, quad->p[3][0], quad->p[3][1], quad->p[0][0], quad->p[0][1], color, 1); + } + + image_u8_write_pnm(im_quads, "debug_quads_raw.pnm"); + image_u8_destroy(im_quads); + } + + //////////////////////////////////////////////////////////////// + // Step 2. Decode tags from each quad. + if (1) { + image_u8_t *im_samples = td->debug ? image_u8_copy(im_orig) : NULL; + + int chunksize = 1 + zarray_size(quads) / (APRILTAG_TASKS_PER_THREAD_TARGET * td->nthreads); + + struct quad_decode_task *tasks = malloc(sizeof(struct quad_decode_task)*(zarray_size(quads) / chunksize + 1)); + + int ntasks = 0; + for (int i = 0; i < zarray_size(quads); i+= chunksize) { + tasks[ntasks].i0 = i; + tasks[ntasks].i1 = imin(zarray_size(quads), i + chunksize); + tasks[ntasks].quads = quads; + tasks[ntasks].td = td; + tasks[ntasks].im = im_orig; + tasks[ntasks].detections = detections; + + tasks[ntasks].im_samples = im_samples; + + workerpool_add_task(td->wp, quad_decode_task, &tasks[ntasks]); + ntasks++; + } + + workerpool_run(td->wp); + + free(tasks); + + if (im_samples != NULL) { + image_u8_write_pnm(im_samples, "debug_samples.pnm"); + image_u8_destroy(im_samples); + } + } + + if (td->debug) { + image_u8_t *im_quads = image_u8_copy(im_orig); + image_u8_darken(im_quads); + image_u8_darken(im_quads); + + srandom(0); + + for (int i = 0; i < zarray_size(quads); i++) { + struct quad *quad; + zarray_get_volatile(quads, i, &quad); + + const int bias = 100; + int color = bias + (random() % (255-bias)); + + image_u8_draw_line(im_quads, quad->p[0][0], quad->p[0][1], quad->p[1][0], quad->p[1][1], color, 1); + image_u8_draw_line(im_quads, quad->p[1][0], quad->p[1][1], quad->p[2][0], quad->p[2][1], color, 1); + image_u8_draw_line(im_quads, quad->p[2][0], quad->p[2][1], quad->p[3][0], quad->p[3][1], color, 1); + image_u8_draw_line(im_quads, quad->p[3][0], quad->p[3][1], quad->p[0][0], quad->p[0][1], color, 1); + + } + + image_u8_write_pnm(im_quads, "debug_quads_fixed.pnm"); + image_u8_destroy(im_quads); + } + + timeprofile_stamp(td->tp, "decode+refinement"); + + //////////////////////////////////////////////////////////////// + // Step 3. Reconcile detections--- don't report the same tag more + // than once. (Allow non-overlapping duplicate detections.) + if (1) { + zarray_t *poly0 = g2d_polygon_create_zeros(4); + zarray_t *poly1 = g2d_polygon_create_zeros(4); + + for (int i0 = 0; i0 < zarray_size(detections); i0++) { + + apriltag_detection_t *det0; + zarray_get(detections, i0, &det0); + + for (int k = 0; k < 4; k++) + zarray_set(poly0, k, det0->p[k], NULL); + + for (int i1 = i0+1; i1 < zarray_size(detections); i1++) { + + apriltag_detection_t *det1; + zarray_get(detections, i1, &det1); + + if (det0->id != det1->id || det0->family != det1->family) + continue; + + for (int k = 0; k < 4; k++) + zarray_set(poly1, k, det1->p[k], NULL); + + if (g2d_polygon_overlaps_polygon(poly0, poly1)) { + // the tags overlap. Delete one, keep the other. + + int pref = 0; // 0 means undecided which one we'll keep. + pref = prefer_smaller(pref, det0->hamming, det1->hamming); // want small hamming + pref = prefer_smaller(pref, -det0->decision_margin, -det1->decision_margin); // want bigger margins + + // if we STILL don't prefer one detection over the other, then pick + // any deterministic criterion. + for (int i = 0; i < 4; i++) { + pref = prefer_smaller(pref, det0->p[i][0], det1->p[i][0]); + pref = prefer_smaller(pref, det0->p[i][1], det1->p[i][1]); + } + + if (pref == 0) { + // at this point, we should only be undecided if the tag detections + // are *exactly* the same. How would that happen? + debug_print("uh oh, no preference for overlappingdetection\n"); + } + + if (pref < 0) { + // keep det0, destroy det1 + apriltag_detection_destroy(det1); + zarray_remove_index(detections, i1, 1); + i1--; // retry the same index + goto retry1; + } else { + // keep det1, destroy det0 + apriltag_detection_destroy(det0); + zarray_remove_index(detections, i0, 1); + i0--; // retry the same index. + goto retry0; + } + } + + retry1: ; + } + + retry0: ; + } + + zarray_destroy(poly0); + zarray_destroy(poly1); + } + + timeprofile_stamp(td->tp, "reconcile"); + + //////////////////////////////////////////////////////////////// + // Produce final debug output + if (td->debug) { + + image_u8_t *darker = image_u8_copy(im_orig); + image_u8_darken(darker); + image_u8_darken(darker); + + // assume letter, which is 612x792 points. + FILE *f = fopen("debug_output.ps", "w"); + fprintf(f, "%%!PS\n\n"); + double scale = fmin(612.0/darker->width, 792.0/darker->height); + fprintf(f, "%f %f scale\n", scale, scale); + fprintf(f, "0 %d translate\n", darker->height); + fprintf(f, "1 -1 scale\n"); + + image_u8_destroy(darker); + + for (int i = 0; i < zarray_size(detections); i++) { + apriltag_detection_t *det; + zarray_get(detections, i, &det); + + float rgb[3]; + int bias = 100; + + for (int j = 0; j < 3; j++) { + rgb[j] = bias + (random() % (255-bias)); + } + + fprintf(f, "%f %f %f setrgbcolor\n", rgb[0]/255.0f, rgb[1]/255.0f, rgb[2]/255.0f); + fprintf(f, "%f %f moveto %f %f lineto %f %f lineto %f %f lineto %f %f lineto stroke\n", + det->p[0][0], det->p[0][1], + det->p[1][0], det->p[1][1], + det->p[2][0], det->p[2][1], + det->p[3][0], det->p[3][1], + det->p[0][0], det->p[0][1]); + } + + fprintf(f, "showpage\n"); + fclose(f); + } + + if (td->debug) { + image_u8_t *darker = image_u8_copy(im_orig); + image_u8_darken(darker); + image_u8_darken(darker); + + image_u8x3_t *out = image_u8x3_create(darker->width, darker->height); + for (int y = 0; y < im_orig->height; y++) { + for (int x = 0; x < im_orig->width; x++) { + out->buf[y*out->stride + 3*x + 0] = darker->buf[y*darker->stride + x]; + out->buf[y*out->stride + 3*x + 1] = darker->buf[y*darker->stride + x]; + out->buf[y*out->stride + 3*x + 2] = darker->buf[y*darker->stride + x]; + } + } + + image_u8_destroy(darker); + + for (int i = 0; i < zarray_size(detections); i++) { + apriltag_detection_t *det; + zarray_get(detections, i, &det); + + float rgb[3]; + int bias = 100; + + for (int j = 0; j < 3; j++) { + rgb[j] = bias + (random() % (255-bias)); + } + + for (int j = 0; j < 4; j++) { + int k = (j + 1) & 3; + image_u8x3_draw_line(out, + det->p[j][0], det->p[j][1], det->p[k][0], det->p[k][1], + (uint8_t[]) { rgb[0], rgb[1], rgb[2] }, + 1); + } + } + + image_u8x3_write_pnm(out, "debug_output.pnm"); + image_u8x3_destroy(out); + } + + // deallocate + if (td->debug) { + FILE *f = fopen("debug_quads.ps", "w"); + fprintf(f, "%%!PS\n\n"); + + image_u8_t *darker = image_u8_copy(im_orig); + image_u8_darken(darker); + image_u8_darken(darker); + + // assume letter, which is 612x792 points. + double scale = fmin(612.0/darker->width, 792.0/darker->height); + fprintf(f, "%f %f scale\n", scale, scale); + fprintf(f, "0 %d translate\n", darker->height); + fprintf(f, "1 -1 scale\n"); + + image_u8_destroy(darker); + + for (int i = 0; i < zarray_size(quads); i++) { + struct quad *q; + zarray_get_volatile(quads, i, &q); + + float rgb[3]; + int bias = 100; + + for (int j = 0; j < 3; j++) { + rgb[j] = bias + (random() % (255-bias)); + } + + fprintf(f, "%f %f %f setrgbcolor\n", rgb[0]/255.0f, rgb[1]/255.0f, rgb[2]/255.0f); + fprintf(f, "%f %f moveto %f %f lineto %f %f lineto %f %f lineto %f %f lineto stroke\n", + q->p[0][0], q->p[0][1], + q->p[1][0], q->p[1][1], + q->p[2][0], q->p[2][1], + q->p[3][0], q->p[3][1], + q->p[0][0], q->p[0][1]); + } + + fprintf(f, "showpage\n"); + fclose(f); + } + + timeprofile_stamp(td->tp, "debug output"); + + for (int i = 0; i < zarray_size(quads); i++) { + struct quad *quad; + zarray_get_volatile(quads, i, &quad); + matd_destroy(quad->H); + matd_destroy(quad->Hinv); + } + + zarray_destroy(quads); + + zarray_sort(detections, detection_compare_function); + timeprofile_stamp(td->tp, "cleanup"); + + return detections; +} + + +// Call this method on each of the tags returned by apriltag_detector_detect +void apriltag_detections_destroy(zarray_t *detections) +{ + for (int i = 0; i < zarray_size(detections); i++) { + apriltag_detection_t *det; + zarray_get(detections, i, &det); + + apriltag_detection_destroy(det); + } + + zarray_destroy(detections); +} + +image_u8_t *apriltag_to_image(apriltag_family_t *fam, int idx) +{ + assert(fam != NULL); + assert(idx >= 0 && idx < fam->ncodes); + + uint64_t code = fam->codes[idx]; + + image_u8_t *im = image_u8_create(fam->total_width, fam->total_width); + + int white_border_width = fam->width_at_border + (fam->reversed_border ? 0 : 2); + int white_border_start = (fam->total_width - white_border_width)/2; + // Make 1px white border + for (int i = 0; i < white_border_width - 1; i += 1) { + im->buf[white_border_start*im->stride + white_border_start + i] = 255; + im->buf[(white_border_start + i)*im->stride + fam->total_width - 1 - white_border_start] = 255; + im->buf[(fam->total_width - 1 - white_border_start)*im->stride + white_border_start + i + 1] = 255; + im->buf[(white_border_start + 1 + i)*im->stride + white_border_start] = 255; + } + + int border_start = (fam->total_width - fam->width_at_border)/2; + for (int i = 0; i < fam->nbits; i++) { + if (code & (APRILTAG_U64_ONE << (fam->nbits - i - 1))) { + im->buf[(fam->bit_y[i] + border_start)*im->stride + fam->bit_x[i] + border_start] = 255; + } + } + return im; +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/apriltag_pose.c b/apriltag/src/main/native/thirdparty/apriltag/src/apriltag_pose.c new file mode 100644 index 0000000000..782729225c --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/apriltag_pose.c @@ -0,0 +1,559 @@ +#include +#include + +#include "common/debug_print.h" +#include "apriltag_pose.h" +#include "common/homography.h" + + +/** + * Calculate projection operator from image points. + */ +matd_t* calculate_F(matd_t* v) { + matd_t* outer_product = matd_op("MM'", v, v, v, v); + matd_t* inner_product = matd_op("M'M", v, v); + matd_scale_inplace(outer_product, 1.0/inner_product->data[0]); + matd_destroy(inner_product); + return outer_product; +} + +/** + * Returns the value of the supplied scalar matrix 'a' and destroys the matrix. + */ +double matd_to_double(matd_t *a) +{ + assert(matd_is_scalar(a)); + double d = a->data[0]; + matd_destroy(a); + return d; +} + +/** + * @param v Image points on the image plane. + * @param p Object points in object space. + * @outparam t Optimal translation. + * @param R In/Outparam. Should be set to initial guess at R. Will be modified to be the optimal translation. + * @param n_points Number of points. + * @param n_steps Number of iterations. + * @param min_improvement_per_iteration Min object-space error improvement; if less than this, solver will exit early + * + * @return Object-space error after iteration. + * + * Implementation of Orthogonal Iteration from Lu, 2000. + */ +double orthogonal_iteration(matd_t** v, matd_t** p, matd_t** t, matd_t** R, int n_points, int n_steps, double min_improvement_per_iteration) { + matd_t* p_mean = matd_create(3, 1); + for (int i = 0; i < n_points; i++) { + matd_add_inplace(p_mean, p[i]); + } + matd_scale_inplace(p_mean, 1.0/n_points); + + matd_t** p_res = malloc(sizeof(matd_t *)*n_points); + for (int i = 0; i < n_points; i++) { + p_res[i] = matd_op("M-M", p[i], p_mean); + } + + // Compute M1_inv. + matd_t** F = malloc(sizeof(matd_t *)*n_points); + matd_t *avg_F = matd_create(3, 3); + for (int i = 0; i < n_points; i++) { + F[i] = calculate_F(v[i]); + matd_add_inplace(avg_F, F[i]); + } + matd_scale_inplace(avg_F, 1.0/n_points); + matd_t *I3 = matd_identity(3); + matd_t *M1 = matd_subtract(I3, avg_F); + matd_t *M1_inv = matd_inverse(M1); + matd_destroy(avg_F); + matd_destroy(M1); + + double prev_error = HUGE_VAL; + // Iterate. + for (int i = 0; i < n_steps; i++) { + // Calculate translation. + matd_t *M2 = matd_create(3, 1); + for (int j = 0; j < n_points; j++) { + matd_t* M2_update = matd_op("(M - M)*M*M", F[j], I3, *R, p[j]); + matd_add_inplace(M2, M2_update); + matd_destroy(M2_update); + } + matd_scale_inplace(M2, 1.0/n_points); + matd_destroy(*t); + *t = matd_multiply(M1_inv, M2); + matd_destroy(M2); + + // Calculate rotation. + matd_t** q = malloc(sizeof(matd_t *)*n_points); + matd_t* q_mean = matd_create(3, 1); + for (int j = 0; j < n_points; j++) { + q[j] = matd_op("M*(M*M+M)", F[j], *R, p[j], *t); + matd_add_inplace(q_mean, q[j]); + } + matd_scale_inplace(q_mean, 1.0/n_points); + + matd_t* M3 = matd_create(3, 3); + for (int j = 0; j < n_points; j++) { + matd_t *M3_update = matd_op("(M-M)*M'", q[j], q_mean, p_res[j]); + matd_add_inplace(M3, M3_update); + matd_destroy(M3_update); + } + matd_svd_t M3_svd = matd_svd(M3); + matd_destroy(M3); + matd_destroy(*R); + *R = matd_op("M*M'", M3_svd.U, M3_svd.V); + double R_det = matd_det(*R); + if (R_det < 0) { + matd_put(*R, 0, 2, - matd_get(*R, 0, 2)); + matd_put(*R, 1, 2, - matd_get(*R, 1, 2)); + matd_put(*R, 2, 2, - matd_get(*R, 2, 2)); + } + matd_destroy(M3_svd.U); + matd_destroy(M3_svd.S); + matd_destroy(M3_svd.V); + matd_destroy(q_mean); + for (int j = 0; j < n_points; j++) { + matd_destroy(q[j]); + } + + double error = 0; + for (int j = 0; j < 4; j++) { + matd_t* err_vec = matd_op("(M-M)(MM+M)", I3, F[j], *R, p[j], *t); + error += matd_to_double(matd_op("M'M", err_vec, err_vec)); + matd_destroy(err_vec); + } + + free(q); + + // Return early if the iterations converged + if (fabs(error - prev_error) < min_improvement_per_iteration) { + prev_error = error; + break; + } + + prev_error = error; + } + + matd_destroy(I3); + matd_destroy(M1_inv); + for (int i = 0; i < n_points; i++) { + matd_destroy(p_res[i]); + matd_destroy(F[i]); + } + free(p_res); + free(F); + matd_destroy(p_mean); + return prev_error; +} + +/** + * Evaluates polynomial p at x. + */ +double polyval(double* p, int degree, double x) { + double ret = 0; + for (int i = 0; i <= degree; i++) { + ret += p[i]*pow(x, i); + } + return ret; +} + +/** + * Numerically solve small degree polynomials. This is a customized method. It + * ignores roots larger than 1000 and only gives small roots approximately. + * + * @param p Array of parameters s.t. p(x) = p[0] + p[1]*x + ... + * @param degree The degree of p(x). + * @outparam roots + * @outparam n_roots + */ +void solve_poly_approx(double* p, int degree, double* roots, int* n_roots) { + static const int MAX_ROOT = 1000; + if (degree == 1) { + if (fabs(p[0]) > MAX_ROOT*fabs(p[1])) { + *n_roots = 0; + } else { + roots[0] = -p[0]/p[1]; + *n_roots = 1; + } + return; + } + + // Calculate roots of derivative. + double *p_der = malloc(sizeof(double)*degree); + for (int i = 0; i < degree; i++) { + p_der[i] = (i + 1) * p[i+1]; + } + + double *der_roots = malloc(sizeof(double)*(degree - 1)); + int n_der_roots; + solve_poly_approx(p_der, degree - 1, der_roots, &n_der_roots); + + + // Go through all possibilities for roots of the polynomial. + *n_roots = 0; + for (int i = 0; i <= n_der_roots; i++) { + double min; + if (i == 0) { + min = -MAX_ROOT; + } else { + min = der_roots[i - 1]; + } + + double max; + if (i == n_der_roots) { + max = MAX_ROOT; + } else { + max = der_roots[i]; + } + + if (polyval(p, degree, min)*polyval(p, degree, max) < 0) { + // We have a zero-crossing in this interval, use a combination of Newton' and bisection. + // Some thanks to Numerical Recipes in C. + + double lower; + double upper; + if (polyval(p, degree, min) < polyval(p, degree, max)) { + lower = min; + upper = max; + } else { + lower = max; + upper = min; + } + double root = 0.5*(lower + upper); + double dx_old = upper - lower; + double dx = dx_old; + double f = polyval(p, degree, root); + double df = polyval(p_der, degree - 1, root); + + for (int j = 0; j < 100; j++) { + if (((f + df*(upper - root))*(f + df*(lower - root)) > 0) + || (fabs(2*f) > fabs(dx_old*df))) { + dx_old = dx; + dx = 0.5*(upper - lower); + root = lower + dx; + } else { + dx_old = dx; + dx = -f/df; + root += dx; + } + + if (root == upper || root == lower) { + break; + } + + f = polyval(p, degree, root); + df = polyval(p_der, degree - 1, root); + + if (f > 0) { + upper = root; + } else { + lower = root; + } + } + + roots[(*n_roots)++] = root; + } else if(polyval(p, degree, max) == 0) { + // Double/triple root. + roots[(*n_roots)++] = max; + } + } + + free(der_roots); + free(p_der); +} + +/** + * Given a local minima of the pose error tries to find the other minima. + */ +matd_t* fix_pose_ambiguities(matd_t** v, matd_t** p, matd_t* t, matd_t* R, int n_points) { + matd_t* I3 = matd_identity(3); + + // 1. Find R_t + matd_t* R_t_3 = matd_vec_normalize(t); + + matd_t* e_x = matd_create(3, 1); + MATD_EL(e_x, 0, 0) = 1; + matd_t* R_t_1_tmp = matd_op("M-(M'*M)*M", e_x, e_x, R_t_3, R_t_3); + matd_t* R_t_1 = matd_vec_normalize(R_t_1_tmp); + matd_destroy(e_x); + matd_destroy(R_t_1_tmp); + + matd_t* R_t_2 = matd_crossproduct(R_t_3, R_t_1); + + matd_t* R_t = matd_create_data(3, 3, (double[]) { + MATD_EL(R_t_1, 0, 0), MATD_EL(R_t_1, 0, 1), MATD_EL(R_t_1, 0, 2), + MATD_EL(R_t_2, 0, 0), MATD_EL(R_t_2, 0, 1), MATD_EL(R_t_2, 0, 2), + MATD_EL(R_t_3, 0, 0), MATD_EL(R_t_3, 0, 1), MATD_EL(R_t_3, 0, 2)}); + matd_destroy(R_t_1); + matd_destroy(R_t_2); + matd_destroy(R_t_3); + + // 2. Find R_z + matd_t* R_1_prime = matd_multiply(R_t, R); + double r31 = MATD_EL(R_1_prime, 2, 0); + double r32 = MATD_EL(R_1_prime, 2, 1); + double hypotenuse = sqrt(r31*r31 + r32*r32); + if (hypotenuse < 1e-100) { + r31 = 1; + r32 = 0; + hypotenuse = 1; + } + matd_t* R_z = matd_create_data(3, 3, (double[]) { + r31/hypotenuse, -r32/hypotenuse, 0, + r32/hypotenuse, r31/hypotenuse, 0, + 0, 0, 1}); + + // 3. Calculate parameters of Eos + matd_t* R_trans = matd_multiply(R_1_prime, R_z); + double sin_gamma = -MATD_EL(R_trans, 0, 1); + double cos_gamma = MATD_EL(R_trans, 1, 1); + matd_t* R_gamma = matd_create_data(3, 3, (double[]) { + cos_gamma, -sin_gamma, 0, + sin_gamma, cos_gamma, 0, + 0, 0, 1}); + + double sin_beta = -MATD_EL(R_trans, 2, 0); + double cos_beta = MATD_EL(R_trans, 2, 2); + double t_initial = atan2(sin_beta, cos_beta); + matd_destroy(R_trans); + + matd_t** v_trans = malloc(sizeof(matd_t *)*n_points); + matd_t** p_trans = malloc(sizeof(matd_t *)*n_points); + matd_t** F_trans = malloc(sizeof(matd_t *)*n_points); + matd_t* avg_F_trans = matd_create(3, 3); + for (int i = 0; i < n_points; i++) { + p_trans[i] = matd_op("M'*M", R_z, p[i]); + v_trans[i] = matd_op("M*M", R_t, v[i]); + F_trans[i] = calculate_F(v_trans[i]); + matd_add_inplace(avg_F_trans, F_trans[i]); + } + matd_scale_inplace(avg_F_trans, 1.0/n_points); + + matd_t* G = matd_op("(M-M)^-1", I3, avg_F_trans); + matd_scale_inplace(G, 1.0/n_points); + + matd_t* M1 = matd_create_data(3, 3, (double[]) { + 0, 0, 2, + 0, 0, 0, + -2, 0, 0}); + matd_t* M2 = matd_create_data(3, 3, (double[]) { + -1, 0, 0, + 0, 1, 0, + 0, 0, -1}); + + matd_t* b0 = matd_create(3, 1); + matd_t* b1 = matd_create(3, 1); + matd_t* b2 = matd_create(3, 1); + for (int i = 0; i < n_points; i++) { + matd_t* op_tmp1 = matd_op("(M-M)MM", F_trans[i], I3, R_gamma, p_trans[i]); + matd_t* op_tmp2 = matd_op("(M-M)MMM", F_trans[i], I3, R_gamma, M1, p_trans[i]); + matd_t* op_tmp3 = matd_op("(M-M)MMM", F_trans[i], I3, R_gamma, M2, p_trans[i]); + + matd_add_inplace(b0, op_tmp1); + matd_add_inplace(b1, op_tmp2); + matd_add_inplace(b2, op_tmp3); + + matd_destroy(op_tmp1); + matd_destroy(op_tmp2); + matd_destroy(op_tmp3); + } + matd_t* b0_ = matd_multiply(G, b0); + matd_t* b1_ = matd_multiply(G, b1); + matd_t* b2_ = matd_multiply(G, b2); + + double a0 = 0; + double a1 = 0; + double a2 = 0; + double a3 = 0; + double a4 = 0; + for (int i = 0; i < n_points; i++) { + matd_t* c0 = matd_op("(M-M)(MM+M)", I3, F_trans[i], R_gamma, p_trans[i], b0_); + matd_t* c1 = matd_op("(M-M)(MMM+M)", I3, F_trans[i], R_gamma, M1, p_trans[i], b1_); + matd_t* c2 = matd_op("(M-M)(MMM+M)", I3, F_trans[i], R_gamma, M2, p_trans[i], b2_); + + a0 += matd_to_double(matd_op("M'M", c0, c0)); + a1 += matd_to_double(matd_op("2M'M", c0, c1)); + a2 += matd_to_double(matd_op("M'M+2M'M", c1, c1, c0, c2)); + a3 += matd_to_double(matd_op("2M'M", c1, c2)); + a4 += matd_to_double(matd_op("M'M", c2, c2)); + + matd_destroy(c0); + matd_destroy(c1); + matd_destroy(c2); + } + + matd_destroy(b0); + matd_destroy(b1); + matd_destroy(b2); + matd_destroy(b0_); + matd_destroy(b1_); + matd_destroy(b2_); + + for (int i = 0; i < n_points; i++) { + matd_destroy(p_trans[i]); + matd_destroy(v_trans[i]); + matd_destroy(F_trans[i]); + } + free(p_trans); + free(v_trans); + free(F_trans); + matd_destroy(avg_F_trans); + matd_destroy(G); + + + // 4. Solve for minima of Eos. + double p0 = a1; + double p1 = 2*a2 - 4*a0; + double p2 = 3*a3 - 3*a1; + double p3 = 4*a4 - 2*a2; + double p4 = -a3; + + double roots[4]; + int n_roots; + solve_poly_approx((double []) {p0, p1, p2, p3, p4}, 4, roots, &n_roots); + + double minima[4]; + int n_minima = 0; + for (int i = 0; i < n_roots; i++) { + double t1 = roots[i]; + double t2 = t1*t1; + double t3 = t1*t2; + double t4 = t1*t3; + double t5 = t1*t4; + // Check extrema is a minima. + if (a2 - 2*a0 + (3*a3 - 6*a1)*t1 + (6*a4 - 8*a2 + 10*a0)*t2 + (-8*a3 + 6*a1)*t3 + (-6*a4 + 3*a2)*t4 + a3*t5 >= 0) { + // And that it corresponds to an angle different than the known minimum. + double t_cur = 2*atan(roots[i]); + // We only care about finding a second local minima which is qualitatively + // different than the first. + if (fabs(t_cur - t_initial) > 0.1) { + minima[n_minima++] = roots[i]; + } + } + } + + // 5. Get poses for minima. + matd_t* ret = NULL; + if (n_minima == 1) { + double t_cur = minima[0]; + matd_t* R_beta = matd_copy(M2); + matd_scale_inplace(R_beta, t_cur); + matd_add_inplace(R_beta, M1); + matd_scale_inplace(R_beta, t_cur); + matd_add_inplace(R_beta, I3); + matd_scale_inplace(R_beta, 1/(1 + t_cur*t_cur)); + ret = matd_op("M'MMM'", R_t, R_gamma, R_beta, R_z); + matd_destroy(R_beta); + } else if (n_minima > 1) { + // This can happen if our prior pose estimate was not very good. + debug_print("Error, more than one new minimum found.\n"); + } + matd_destroy(I3); + matd_destroy(M1); + matd_destroy(M2); + matd_destroy(R_t); + matd_destroy(R_gamma); + matd_destroy(R_z); + matd_destroy(R_1_prime); + return ret; +} + +/** + * Estimate pose of the tag using the homography method. + */ +void estimate_pose_for_tag_homography(apriltag_detection_info_t* info, apriltag_pose_t* solution) { + double scale = info->tagsize/2.0; + + matd_t *M_H = homography_to_pose(info->det->H, -info->fx, info->fy, info->cx, info->cy); + MATD_EL(M_H, 0, 3) *= scale; + MATD_EL(M_H, 1, 3) *= scale; + MATD_EL(M_H, 2, 3) *= scale; + + matd_t* fix = matd_create(4, 4); + MATD_EL(fix, 0, 0) = 1; + MATD_EL(fix, 1, 1) = -1; + MATD_EL(fix, 2, 2) = -1; + MATD_EL(fix, 3, 3) = 1; + + matd_t* initial_pose = matd_multiply(fix, M_H); + matd_destroy(M_H); + matd_destroy(fix); + + solution->R = matd_create(3, 3); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + MATD_EL(solution->R, i, j) = MATD_EL(initial_pose, i, j); + } + } + + solution->t = matd_create(3, 1); + for (int i = 0; i < 3; i++) { + MATD_EL(solution->t, i, 0) = MATD_EL(initial_pose, i, 3); + } + matd_destroy(initial_pose); +} + +/** + * Estimate tag pose using orthogonal iteration. + */ +void estimate_tag_pose_orthogonal_iteration( + apriltag_detection_info_t* info, + double* err1, + apriltag_pose_t* solution1, + double* err2, + apriltag_pose_t* solution2, + int nIters, + double min_improvement_per_iteration) { + double scale = info->tagsize/2.0; + matd_t* p[4] = { + matd_create_data(3, 1, (double[]) {-scale, scale, 0}), + matd_create_data(3, 1, (double[]) {scale, scale, 0}), + matd_create_data(3, 1, (double[]) {scale, -scale, 0}), + matd_create_data(3, 1, (double[]) {-scale, -scale, 0})}; + matd_t* v[4]; + for (int i = 0; i < 4; i++) { + v[i] = matd_create_data(3, 1, (double[]) { + (info->det->p[i][0] - info->cx)/info->fx, (info->det->p[i][1] - info->cy)/info->fy, 1}); + } + + estimate_pose_for_tag_homography(info, solution1); + *err1 = orthogonal_iteration(v, p, &solution1->t, &solution1->R, 4, nIters, min_improvement_per_iteration); + solution2->R = fix_pose_ambiguities(v, p, solution1->t, solution1->R, 4); + if (solution2->R) { + solution2->t = matd_create(3, 1); + *err2 = orthogonal_iteration(v, p, &solution2->t, &solution2->R, 4, nIters, min_improvement_per_iteration); + } else { + solution2->t = NULL; + *err2 = HUGE_VAL; + } + + for (int i = 0; i < 4; i++) { + matd_destroy(p[i]); + matd_destroy(v[i]); + } +} + +/** + * Estimate tag pose. + */ +double estimate_tag_pose(apriltag_detection_info_t* info, apriltag_pose_t* pose) { + double err1, err2; + apriltag_pose_t pose1, pose2; + // 50 iterations is a good sensible default + // 1e-7 improvement per iteration is also pretty sane + estimate_tag_pose_orthogonal_iteration(info, &err1, &pose1, &err2, &pose2, 50, 1e-7); + if (err1 <= err2) { + pose->R = pose1.R; + pose->t = pose1.t; + if (pose2.R) { + matd_destroy(pose2.t); + } + matd_destroy(pose2.R); + return err1; + } else { + pose->R = pose2.R; + pose->t = pose2.t; + matd_destroy(pose1.R); + matd_destroy(pose1.t); + return err2; + } +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/apriltag_quad_thresh.c b/apriltag/src/main/native/thirdparty/apriltag/src/apriltag_quad_thresh.c new file mode 100644 index 0000000000..677f365765 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/apriltag_quad_thresh.c @@ -0,0 +1,2017 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +// limitation: image size must be <32768 in width and height. This is +// because we use a fixed-point 16 bit integer representation with one +// fractional bit. +#include +#include +#include +#include +#include + +#include "apriltag.h" +#include "common/image_u8x3.h" +#include "common/zarray.h" +#include "common/unionfind.h" +#include "common/timeprofile.h" +#include "common/zmaxheap.h" +#include "common/math_util.h" + +#ifdef _WIN32 +static inline long int random(void) +{ + return rand(); +} +#endif + +static inline uint32_t u64hash_2(uint64_t x) { + return (2654435761 * x) >> 32; +} + +struct uint64_zarray_entry +{ + uint64_t id; + zarray_t *cluster; + + struct uint64_zarray_entry *next; +}; + +#ifndef M_PI +# define M_PI 3.141592653589793238462643383279502884196 +#endif + +struct pt +{ + // Note: these represent 2*actual value. + uint16_t x, y; + int16_t gx, gy; + + float slope; +}; + +struct unionfind_task +{ + int y0, y1; + int w, h, s; + unionfind_t *uf; + image_u8_t *im; +}; + +struct quad_task +{ + zarray_t *clusters; + int cidx0, cidx1; // [cidx0, cidx1) + zarray_t *quads; + apriltag_detector_t *td; + int w, h; + + image_u8_t *im; + int tag_width; + bool normal_border; + bool reversed_border; +}; + + +struct cluster_task +{ + int y0; + int y1; + int w; + int s; + int nclustermap; + unionfind_t* uf; + image_u8_t* im; + zarray_t* clusters; +}; + +struct minmax_task { + int ty; + + image_u8_t *im; + uint8_t *im_max; + uint8_t *im_min; +}; + +struct blur_task { + int ty; + + image_u8_t *im; + uint8_t *im_max; + uint8_t *im_min; + uint8_t *im_max_tmp; + uint8_t *im_min_tmp; +}; + +struct threshold_task { + int ty; + + apriltag_detector_t *td; + image_u8_t *im; + image_u8_t *threshim; + uint8_t *im_max; + uint8_t *im_min; +}; + +struct remove_vertex +{ + int i; // which vertex to remove? + int left, right; // left vertex, right vertex + + double err; +}; + +struct segment +{ + int is_vertex; + + // always greater than zero, but right can be > size, which denotes + // a wrap around back to the beginning of the points. and left < right. + int left, right; +}; + +struct line_fit_pt +{ + double Mx, My; + double Mxx, Myy, Mxy; + double W; // total weight +}; + +struct cluster_hash +{ + uint32_t hash; + uint64_t id; + zarray_t* data; +}; + + +// lfps contains *cumulative* moments for N points, with +// index j reflecting points [0,j] (inclusive). +// +// fit a line to the points [i0, i1] (inclusive). i0, i1 are both [0, +// sz) if i1 < i0, we treat this as a wrap around. +void fit_line(struct line_fit_pt *lfps, int sz, int i0, int i1, double *lineparm, double *err, double *mse) +{ + assert(i0 != i1); + assert(i0 >= 0 && i1 >= 0 && i0 < sz && i1 < sz); + + double Mx, My, Mxx, Myy, Mxy, W; + int N; // how many points are included in the set? + + if (i0 < i1) { + N = i1 - i0 + 1; + + Mx = lfps[i1].Mx; + My = lfps[i1].My; + Mxx = lfps[i1].Mxx; + Mxy = lfps[i1].Mxy; + Myy = lfps[i1].Myy; + W = lfps[i1].W; + + if (i0 > 0) { + Mx -= lfps[i0-1].Mx; + My -= lfps[i0-1].My; + Mxx -= lfps[i0-1].Mxx; + Mxy -= lfps[i0-1].Mxy; + Myy -= lfps[i0-1].Myy; + W -= lfps[i0-1].W; + } + + } else { + // i0 > i1, e.g. [15, 2]. Wrap around. + assert(i0 > 0); + + Mx = lfps[sz-1].Mx - lfps[i0-1].Mx; + My = lfps[sz-1].My - lfps[i0-1].My; + Mxx = lfps[sz-1].Mxx - lfps[i0-1].Mxx; + Mxy = lfps[sz-1].Mxy - lfps[i0-1].Mxy; + Myy = lfps[sz-1].Myy - lfps[i0-1].Myy; + W = lfps[sz-1].W - lfps[i0-1].W; + + Mx += lfps[i1].Mx; + My += lfps[i1].My; + Mxx += lfps[i1].Mxx; + Mxy += lfps[i1].Mxy; + Myy += lfps[i1].Myy; + W += lfps[i1].W; + + N = sz - i0 + i1 + 1; + } + + assert(N >= 2); + + double Ex = Mx / W; + double Ey = My / W; + double Cxx = Mxx / W - Ex*Ex; + double Cxy = Mxy / W - Ex*Ey; + double Cyy = Myy / W - Ey*Ey; + + //if (1) { + // // on iOS about 5% of total CPU spent in these trig functions. + // // 85 ms per frame on 5S, example.pnm + // // + // // XXX this was using the double-precision atan2. Was there a case where + // // we needed that precision? Seems doubtful. + // double normal_theta = .5 * atan2f(-2*Cxy, (Cyy - Cxx)); + // nx_old = cosf(normal_theta); + // ny_old = sinf(normal_theta); + //} + + // Instead of using the above cos/sin method, pose it as an eigenvalue problem. + double eig_small = 0.5*(Cxx + Cyy - sqrtf((Cxx - Cyy)*(Cxx - Cyy) + 4*Cxy*Cxy)); + + if (lineparm) { + lineparm[0] = Ex; + lineparm[1] = Ey; + + double eig = 0.5*(Cxx + Cyy + sqrtf((Cxx - Cyy)*(Cxx - Cyy) + 4*Cxy*Cxy)); + double nx1 = Cxx - eig; + double ny1 = Cxy; + double M1 = nx1*nx1 + ny1*ny1; + double nx2 = Cxy; + double ny2 = Cyy - eig; + double M2 = nx2*nx2 + ny2*ny2; + + double nx, ny, M; + if (M1 > M2) { + nx = nx1; + ny = ny1; + M = M1; + } else { + nx = nx2; + ny = ny2; + M = M2; + } + + double length = sqrtf(M); + lineparm[2] = nx/length; + lineparm[3] = ny/length; + } + + // sum of squared errors = + // + // SUM_i ((p_x - ux)*nx + (p_y - uy)*ny)^2 + // SUM_i nx*nx*(p_x - ux)^2 + 2nx*ny(p_x -ux)(p_y-uy) + ny*ny*(p_y-uy)*(p_y-uy) + // nx*nx*SUM_i((p_x -ux)^2) + 2nx*ny*SUM_i((p_x-ux)(p_y-uy)) + ny*ny*SUM_i((p_y-uy)^2) + // + // nx*nx*N*Cxx + 2nx*ny*N*Cxy + ny*ny*N*Cyy + + // sum of squared errors + if (err) + *err = N*eig_small; + + // mean squared error + if (mse) + *mse = eig_small; +} + +float pt_compare_angle(struct pt *a, struct pt *b) { + return a->slope - b->slope; +} + +int err_compare_descending(const void *_a, const void *_b) +{ + const double *a = _a; + const double *b = _b; + + return ((*a) < (*b)) ? 1 : -1; +} + +/* + + 1. Identify A) white points near a black point and B) black points near a white point. + + 2. Find the connected components within each of the classes above, + yielding clusters of "white-near-black" and + "black-near-white". (These two classes are kept separate). Each + segment has a unique id. + + 3. For every pair of "white-near-black" and "black-near-white" + clusters, find the set of points that are in one and adjacent to the + other. In other words, a "boundary" layer between the two + clusters. (This is actually performed by iterating over the pixels, + rather than pairs of clusters.) Critically, this helps keep nearby + edges from becoming connected. +*/ +int quad_segment_maxima(apriltag_detector_t *td, zarray_t *cluster, struct line_fit_pt *lfps, int indices[4]) +{ + int sz = zarray_size(cluster); + + // ksz: when fitting points, how many points on either side do we consider? + // (actual "kernel" width is 2ksz). + // + // This value should be about: 0.5 * (points along shortest edge). + // + // If all edges were equally-sized, that would give a value of + // sz/8. We make it somewhat smaller to account for tags at high + // aspects. + + // XXX Tunable. Maybe make a multiple of JPEG block size to increase robustness + // to JPEG compression artifacts? + int ksz = imin(20, sz / 12); + + // can't fit a quad if there are too few points. + if (ksz < 2) + return 0; + + double *errs = malloc(sizeof(double)*sz); + + for (int i = 0; i < sz; i++) { + fit_line(lfps, sz, (i + sz - ksz) % sz, (i + ksz) % sz, NULL, &errs[i], NULL); + } + + // apply a low-pass filter to errs + if (1) { + double *y = malloc(sizeof(double)*sz); + + // how much filter to apply? + + // XXX Tunable + double sigma = 1; // was 3 + + // cutoff = exp(-j*j/(2*sigma*sigma)); + // log(cutoff) = -j*j / (2*sigma*sigma) + // log(cutoff)*2*sigma*sigma = -j*j; + + // how big a filter should we use? We make our kernel big + // enough such that we represent any values larger than + // 'cutoff'. + + // XXX Tunable (though not super useful to change) + double cutoff = 0.05; + int fsz = sqrt(-log(cutoff)*2*sigma*sigma) + 1; + fsz = 2*fsz + 1; + + // For default values of cutoff = 0.05, sigma = 3, + // we have fsz = 17. + float *f = malloc(sizeof(float)*fsz); + + for (int i = 0; i < fsz; i++) { + int j = i - fsz / 2; + f[i] = exp(-j*j/(2*sigma*sigma)); + } + + for (int iy = 0; iy < sz; iy++) { + double acc = 0; + + for (int i = 0; i < fsz; i++) { + acc += errs[(iy + i - fsz / 2 + sz) % sz] * f[i]; + } + y[iy] = acc; + } + + memcpy(errs, y, sizeof(double)*sz); + free(y); + free(f); + } + + int *maxima = malloc(sizeof(int)*sz); + double *maxima_errs = malloc(sizeof(double)*sz); + int nmaxima = 0; + + for (int i = 0; i < sz; i++) { + if (errs[i] > errs[(i+1)%sz] && errs[i] > errs[(i+sz-1)%sz]) { + maxima[nmaxima] = i; + maxima_errs[nmaxima] = errs[i]; + nmaxima++; + } + } + free(errs); + + // if we didn't get at least 4 maxima, we can't fit a quad. + if (nmaxima < 4){ + free(maxima); + free(maxima_errs); + return 0; + } + + // select only the best maxima if we have too many + int max_nmaxima = td->qtp.max_nmaxima; + + if (nmaxima > max_nmaxima) { + double *maxima_errs_copy = malloc(sizeof(double)*nmaxima); + memcpy(maxima_errs_copy, maxima_errs, sizeof(double)*nmaxima); + + // throw out all but the best handful of maxima. Sorts descending. + qsort(maxima_errs_copy, nmaxima, sizeof(double), err_compare_descending); + + double maxima_thresh = maxima_errs_copy[max_nmaxima]; + int out = 0; + for (int in = 0; in < nmaxima; in++) { + if (maxima_errs[in] <= maxima_thresh) + continue; + maxima[out++] = maxima[in]; + } + nmaxima = out; + free(maxima_errs_copy); + } + free(maxima_errs); + + int best_indices[4]; + double best_error = HUGE_VALF; + + double err01, err12, err23, err30; + double mse01, mse12, mse23, mse30; + double params01[4], params12[4], params23[4], params30[4]; + + // disallow quads where the angle is less than a critical value. + double max_dot = td->qtp.cos_critical_rad; //25*M_PI/180); + + for (int m0 = 0; m0 < nmaxima - 3; m0++) { + int i0 = maxima[m0]; + + for (int m1 = m0+1; m1 < nmaxima - 2; m1++) { + int i1 = maxima[m1]; + + fit_line(lfps, sz, i0, i1, params01, &err01, &mse01); + + if (mse01 > td->qtp.max_line_fit_mse) + continue; + + for (int m2 = m1+1; m2 < nmaxima - 1; m2++) { + int i2 = maxima[m2]; + + fit_line(lfps, sz, i1, i2, params12, &err12, &mse12); + if (mse12 > td->qtp.max_line_fit_mse) + continue; + + double dot = params01[2]*params12[2] + params01[3]*params12[3]; + if (fabs(dot) > max_dot) + continue; + + for (int m3 = m2+1; m3 < nmaxima; m3++) { + int i3 = maxima[m3]; + + fit_line(lfps, sz, i2, i3, params23, &err23, &mse23); + if (mse23 > td->qtp.max_line_fit_mse) + continue; + + fit_line(lfps, sz, i3, i0, params30, &err30, &mse30); + if (mse30 > td->qtp.max_line_fit_mse) + continue; + + double err = err01 + err12 + err23 + err30; + if (err < best_error) { + best_error = err; + best_indices[0] = i0; + best_indices[1] = i1; + best_indices[2] = i2; + best_indices[3] = i3; + } + } + } + } + } + + free(maxima); + + if (best_error == HUGE_VALF) + return 0; + + for (int i = 0; i < 4; i++) + indices[i] = best_indices[i]; + + if (best_error / sz < td->qtp.max_line_fit_mse) + return 1; + return 0; +} + +// returns 0 if the cluster looks bad. +int quad_segment_agg(apriltag_detector_t *td, zarray_t *cluster, struct line_fit_pt *lfps, int indices[4]) +{ + int sz = zarray_size(cluster); + + zmaxheap_t *heap = zmaxheap_create(sizeof(struct remove_vertex*)); + + // We will initially allocate sz rvs. We then have two types of + // iterations: some iterations that are no-ops in terms of + // allocations, and those that remove a vertex and allocate two + // more children. This will happen at most (sz-4) times. Thus we + // need: sz + 2*(sz-4) entries. + + int rvalloc_pos = 0; + int rvalloc_size = 3*sz; + struct remove_vertex *rvalloc = calloc(rvalloc_size, sizeof(struct remove_vertex)); + + struct segment *segs = calloc(sz, sizeof(struct segment)); + + // populate with initial entries + for (int i = 0; i < sz; i++) { + struct remove_vertex *rv = &rvalloc[rvalloc_pos++]; + rv->i = i; + if (i == 0) { + rv->left = sz-1; + rv->right = 1; + } else { + rv->left = i-1; + rv->right = (i+1) % sz; + } + + fit_line(lfps, sz, rv->left, rv->right, NULL, NULL, &rv->err); + + zmaxheap_add(heap, &rv, -rv->err); + + segs[i].left = rv->left; + segs[i].right = rv->right; + segs[i].is_vertex = 1; + } + + int nvertices = sz; + + while (nvertices > 4) { + assert(rvalloc_pos < rvalloc_size); + + struct remove_vertex *rv; + float err; + + int res = zmaxheap_remove_max(heap, &rv, &err); + if (!res) + return 0; + assert(res); + + // is this remove_vertex valid? (Or has one of the left/right + // vertices changes since we last looked?) + if (!segs[rv->i].is_vertex || + !segs[rv->left].is_vertex || + !segs[rv->right].is_vertex) { + continue; + } + + // we now merge. + assert(segs[rv->i].is_vertex); + + segs[rv->i].is_vertex = 0; + segs[rv->left].right = rv->right; + segs[rv->right].left = rv->left; + + // create the join to the left + if (1) { + struct remove_vertex *child = &rvalloc[rvalloc_pos++]; + child->i = rv->left; + child->left = segs[rv->left].left; + child->right = rv->right; + + fit_line(lfps, sz, child->left, child->right, NULL, NULL, &child->err); + + zmaxheap_add(heap, &child, -child->err); + } + + // create the join to the right + if (1) { + struct remove_vertex *child = &rvalloc[rvalloc_pos++]; + child->i = rv->right; + child->left = rv->left; + child->right = segs[rv->right].right; + + fit_line(lfps, sz, child->left, child->right, NULL, NULL, &child->err); + + zmaxheap_add(heap, &child, -child->err); + } + + // we now have one less vertex + nvertices--; + } + + free(rvalloc); + zmaxheap_destroy(heap); + + int idx = 0; + for (int i = 0; i < sz; i++) { + if (segs[i].is_vertex) { + indices[idx++] = i; + } + } + + free(segs); + + return 1; +} + +/** + * Compute statistics that allow line fit queries to be + * efficiently computed for any contiguous range of indices. + */ +struct line_fit_pt* compute_lfps(int sz, zarray_t* cluster, image_u8_t* im) { + struct line_fit_pt *lfps = calloc(sz, sizeof(struct line_fit_pt)); + + for (int i = 0; i < sz; i++) { + struct pt *p; + zarray_get_volatile(cluster, i, &p); + + if (i > 0) { + memcpy(&lfps[i], &lfps[i-1], sizeof(struct line_fit_pt)); + } + + { + // we now undo our fixed-point arithmetic. + double delta = 0.5; // adjust for pixel center bias + double x = p->x * .5 + delta; + double y = p->y * .5 + delta; + int ix = x, iy = y; + double W = 1; + + if (ix > 0 && ix+1 < im->width && iy > 0 && iy+1 < im->height) { + int grad_x = im->buf[iy * im->stride + ix + 1] - + im->buf[iy * im->stride + ix - 1]; + + int grad_y = im->buf[(iy+1) * im->stride + ix] - + im->buf[(iy-1) * im->stride + ix]; + + // XXX Tunable. How to shape the gradient magnitude? + W = sqrt(grad_x*grad_x + grad_y*grad_y) + 1; + } + + double fx = x, fy = y; + lfps[i].Mx += W * fx; + lfps[i].My += W * fy; + lfps[i].Mxx += W * fx * fx; + lfps[i].Mxy += W * fx * fy; + lfps[i].Myy += W * fy * fy; + lfps[i].W += W; + } + } + return lfps; +} + +static inline void ptsort(struct pt *pts, int sz) +{ +#define MAYBE_SWAP(arr,apos,bpos) \ + if (pt_compare_angle(&(arr[apos]), &(arr[bpos])) > 0) { \ + tmp = arr[apos]; arr[apos] = arr[bpos]; arr[bpos] = tmp; \ + }; + + if (sz <= 1) + return; + + if (sz == 2) { + struct pt tmp; + MAYBE_SWAP(pts, 0, 1); + return; + } + + // NB: Using less-branch-intensive sorting networks here on the + // hunch that it's better for performance. + if (sz == 3) { // 3 element bubble sort is optimal + struct pt tmp; + MAYBE_SWAP(pts, 0, 1); + MAYBE_SWAP(pts, 1, 2); + MAYBE_SWAP(pts, 0, 1); + return; + } + + if (sz == 4) { // 4 element optimal sorting network. + struct pt tmp; + MAYBE_SWAP(pts, 0, 1); // sort each half, like a merge sort + MAYBE_SWAP(pts, 2, 3); + MAYBE_SWAP(pts, 0, 2); // minimum value is now at 0. + MAYBE_SWAP(pts, 1, 3); // maximum value is now at end. + MAYBE_SWAP(pts, 1, 2); // that only leaves the middle two. + return; + } + if (sz == 5) { + // this 9-step swap is optimal for a sorting network, but two + // steps slower than a generic sort. + struct pt tmp; + MAYBE_SWAP(pts, 0, 1); // sort each half (3+2), like a merge sort + MAYBE_SWAP(pts, 3, 4); + MAYBE_SWAP(pts, 1, 2); + MAYBE_SWAP(pts, 0, 1); + MAYBE_SWAP(pts, 0, 3); // minimum element now at 0 + MAYBE_SWAP(pts, 2, 4); // maximum element now at end + MAYBE_SWAP(pts, 1, 2); // now resort the three elements 1-3. + MAYBE_SWAP(pts, 2, 3); + MAYBE_SWAP(pts, 1, 2); + return; + } + +#undef MAYBE_SWAP + + // a merge sort with temp storage. + + struct pt *tmp = malloc(sizeof(struct pt) * sz); + + memcpy(tmp, pts, sizeof(struct pt) * sz); + + int asz = sz/2; + int bsz = sz - asz; + + struct pt *as = &tmp[0]; + struct pt *bs = &tmp[asz]; + + ptsort(as, asz); + ptsort(bs, bsz); + + #define MERGE(apos,bpos) \ + if (pt_compare_angle(&(as[apos]), &(bs[bpos])) < 0) \ + pts[outpos++] = as[apos++]; \ + else \ + pts[outpos++] = bs[bpos++]; + + int apos = 0, bpos = 0, outpos = 0; + while (apos + 8 < asz && bpos + 8 < bsz) { + MERGE(apos,bpos); MERGE(apos,bpos); MERGE(apos,bpos); MERGE(apos,bpos); + MERGE(apos,bpos); MERGE(apos,bpos); MERGE(apos,bpos); MERGE(apos,bpos); + } + + while (apos < asz && bpos < bsz) { + MERGE(apos,bpos); + } + + if (apos < asz) + memcpy(&pts[outpos], &as[apos], (asz-apos)*sizeof(struct pt)); + if (bpos < bsz) + memcpy(&pts[outpos], &bs[bpos], (bsz-bpos)*sizeof(struct pt)); + + free(tmp); + +#undef MERGE +} + +// return 1 if the quad looks okay, 0 if it should be discarded +int fit_quad( + apriltag_detector_t *td, + image_u8_t *im, + zarray_t *cluster, + struct quad *quad, + int tag_width, + bool normal_border, + bool reversed_border) { + int res = 0; + + int sz = zarray_size(cluster); + if (sz < 24) // Synchronize with later check. + return 0; + + ///////////////////////////////////////////////////////////// + // Step 1. Sort points so they wrap around the center of the + // quad. We will constrain our quad fit to simply partition this + // ordered set into 4 groups. + + // compute a bounding box so that we can order the points + // according to their angle WRT the center. + struct pt *p1; + zarray_get_volatile(cluster, 0, &p1); + uint16_t xmax = p1->x; + uint16_t xmin = p1->x; + uint16_t ymax = p1->y; + uint16_t ymin = p1->y; + for (int pidx = 1; pidx < zarray_size(cluster); pidx++) { + struct pt *p; + zarray_get_volatile(cluster, pidx, &p); + + if (p->x > xmax) { + xmax = p->x; + } else if (p->x < xmin) { + xmin = p->x; + } + + if (p->y > ymax) { + ymax = p->y; + } else if (p->y < ymin) { + ymin = p->y; + } + } + + if ((xmax - xmin)*(ymax - ymin) < tag_width) { + return 0; + } + + // add some noise to (cx,cy) so that pixels get a more diverse set + // of theta estimates. This will help us remove more points. + // (Only helps a small amount. The actual noise values here don't + // matter much at all, but we want them [-1, 1]. (XXX with + // fixed-point, should range be bigger?) + float cx = (xmin + xmax) * 0.5 + 0.05118; + float cy = (ymin + ymax) * 0.5 + -0.028581; + + float dot = 0; + + float quadrants[2][2] = {{-1*(2 << 15), 0}, {2*(2 << 15), 2 << 15}}; + + for (int pidx = 0; pidx < zarray_size(cluster); pidx++) { + struct pt *p; + zarray_get_volatile(cluster, pidx, &p); + + float dx = p->x - cx; + float dy = p->y - cy; + + dot += dx*p->gx + dy*p->gy; + + float quadrant = quadrants[dy > 0][dx > 0]; + if (dy < 0) { + dy = -dy; + dx = -dx; + } + + if (dx < 0) { + float tmp = dx; + dx = dy; + dy = -tmp; + } + p->slope = quadrant + dy/dx; + } + + // Ensure that the black border is inside the white border. + quad->reversed_border = dot < 0; + if (!reversed_border && quad->reversed_border) { + return 0; + } + if (!normal_border && !quad->reversed_border) { + return 0; + } + + // we now sort the points according to theta. This is a prepatory + // step for segmenting them into four lines. + if (1) { + ptsort((struct pt*) cluster->data, zarray_size(cluster)); + + // remove duplicate points. (A byproduct of our segmentation system.) + if (1) { + int outpos = 1; + + struct pt *last; + zarray_get_volatile(cluster, 0, &last); + + for (int i = 1; i < sz; i++) { + + struct pt *p; + zarray_get_volatile(cluster, i, &p); + + if (p->x != last->x || p->y != last->y) { + + if (i != outpos) { + struct pt *out; + zarray_get_volatile(cluster, outpos, &out); + memcpy(out, p, sizeof(struct pt)); + } + + outpos++; + } + + last = p; + } + + cluster->size = outpos; + sz = outpos; + } + + } + + if (sz < 24) + return 0; + + + struct line_fit_pt *lfps = compute_lfps(sz, cluster, im); + + int indices[4]; + if (1) { + if (!quad_segment_maxima(td, cluster, lfps, indices)) + goto finish; + } else { + if (!quad_segment_agg(td, cluster, lfps, indices)) + goto finish; + } + + + double lines[4][4]; + + for (int i = 0; i < 4; i++) { + int i0 = indices[i]; + int i1 = indices[(i+1)&3]; + + double err; + fit_line(lfps, sz, i0, i1, lines[i], NULL, &err); + + if (err > td->qtp.max_line_fit_mse) { + res = 0; + goto finish; + } + } + + for (int i = 0; i < 4; i++) { + // solve for the intersection of lines (i) and (i+1)&3. + // p0 + lambda0*u0 = p1 + lambda1*u1, where u0 and u1 + // are the line directions. + // + // lambda0*u0 - lambda1*u1 = (p1 - p0) + // + // rearrange (solve for lambdas) + // + // [u0_x -u1_x ] [lambda0] = [ p1_x - p0_x ] + // [u0_y -u1_y ] [lambda1] [ p1_y - p0_y ] + // + // remember that lines[i][0,1] = p, lines[i][2,3] = NORMAL vector. + // We want the unit vector, so we need the perpendiculars. Thus, below + // we have swapped the x and y components and flipped the y components. + + double A00 = lines[i][3], A01 = -lines[(i+1)&3][3]; + double A10 = -lines[i][2], A11 = lines[(i+1)&3][2]; + double B0 = -lines[i][0] + lines[(i+1)&3][0]; + double B1 = -lines[i][1] + lines[(i+1)&3][1]; + + double det = A00 * A11 - A10 * A01; + + // inverse. + double W00 = A11 / det, W01 = -A01 / det; + if (fabs(det) < 0.001) { + res = 0; + goto finish; + } + + // solve + double L0 = W00*B0 + W01*B1; + + // compute intersection + quad->p[i][0] = lines[i][0] + L0*A00; + quad->p[i][1] = lines[i][1] + L0*A10; + + res = 1; + } + + // reject quads that are too small + if (1) { + double area = 0; + + // get area of triangle formed by points 0, 1, 2, 0 + double length[3], p; + for (int i = 0; i < 3; i++) { + int idxa = i; // 0, 1, 2, + int idxb = (i+1) % 3; // 1, 2, 0 + length[i] = sqrt(sq(quad->p[idxb][0] - quad->p[idxa][0]) + + sq(quad->p[idxb][1] - quad->p[idxa][1])); + } + p = (length[0] + length[1] + length[2]) / 2; + + area += sqrt(p*(p-length[0])*(p-length[1])*(p-length[2])); + + // get area of triangle formed by points 2, 3, 0, 2 + for (int i = 0; i < 3; i++) { + int idxs[] = { 2, 3, 0, 2 }; + int idxa = idxs[i]; + int idxb = idxs[i+1]; + length[i] = sqrt(sq(quad->p[idxb][0] - quad->p[idxa][0]) + + sq(quad->p[idxb][1] - quad->p[idxa][1])); + } + p = (length[0] + length[1] + length[2]) / 2; + + area += sqrt(p*(p-length[0])*(p-length[1])*(p-length[2])); + + if (area < 0.95*tag_width*tag_width) { + res = 0; + goto finish; + } + } + + // reject quads whose cumulative angle change isn't equal to 2PI + if (1) { + for (int i = 0; i < 4; i++) { + int i0 = i, i1 = (i+1)&3, i2 = (i+2)&3; + + double dx1 = quad->p[i1][0] - quad->p[i0][0]; + double dy1 = quad->p[i1][1] - quad->p[i0][1]; + double dx2 = quad->p[i2][0] - quad->p[i1][0]; + double dy2 = quad->p[i2][1] - quad->p[i1][1]; + double cos_dtheta = (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2)); + + if ((cos_dtheta > td->qtp.cos_critical_rad || cos_dtheta < -td->qtp.cos_critical_rad) || dx1*dy2 < dy1*dx2) { + res = 0; + goto finish; + } + } + } + + finish: + + free(lfps); + + return res; +} + +#define DO_UNIONFIND2(dx, dy) if (im->buf[(y + dy)*s + x + dx] == v) unionfind_connect(uf, y*w + x, (y + dy)*w + x + dx); + +static void do_unionfind_first_line(unionfind_t *uf, image_u8_t *im, int h, int w, int s) +{ + int y = 0; + uint8_t v; + + for (int x = 1; x < w - 1; x++) { + v = im->buf[y*s + x]; + + if (v == 127) + continue; + + DO_UNIONFIND2(-1, 0); + } +} + +static void do_unionfind_line2(unionfind_t *uf, image_u8_t *im, int h, int w, int s, int y) +{ + assert(y > 0); + + uint8_t v_m1_m1; + uint8_t v_0_m1 = im->buf[(y - 1)*s]; + uint8_t v_1_m1 = im->buf[(y - 1)*s + 1]; + uint8_t v_m1_0; + uint8_t v = im->buf[y*s]; + + for (int x = 1; x < w - 1; x++) { + v_m1_m1 = v_0_m1; + v_0_m1 = v_1_m1; + v_1_m1 = im->buf[(y - 1)*s + x + 1]; + v_m1_0 = v; + v = im->buf[y*s + x]; + + if (v == 127) + continue; + + // (dx,dy) pairs for 8 connectivity: + // (-1, -1) (0, -1) (1, -1) + // (-1, 0) (REFERENCE) + DO_UNIONFIND2(-1, 0); + + if (x == 1 || !((v_m1_0 == v_m1_m1) && (v_m1_m1 == v_0_m1))) { + DO_UNIONFIND2(0, -1); + } + + if (v == 255) { + if (x == 1 || !(v_m1_0 == v_m1_m1 || v_0_m1 == v_m1_m1) ) { + DO_UNIONFIND2(-1, -1); + } + if (!(v_0_m1 == v_1_m1)) { + DO_UNIONFIND2(1, -1); + } + } + } +} +#undef DO_UNIONFIND2 + +static void do_unionfind_task2(void *p) +{ + struct unionfind_task *task = (struct unionfind_task*) p; + + for (int y = task->y0; y < task->y1; y++) { + do_unionfind_line2(task->uf, task->im, task->h, task->w, task->s, y); + } +} + +static void do_quad_task(void *p) +{ + struct quad_task *task = (struct quad_task*) p; + + zarray_t *clusters = task->clusters; + zarray_t *quads = task->quads; + apriltag_detector_t *td = task->td; + int w = task->w, h = task->h; + + for (int cidx = task->cidx0; cidx < task->cidx1; cidx++) { + + zarray_t **cluster; + zarray_get_volatile(clusters, cidx, &cluster); + + if (zarray_size(*cluster) < td->qtp.min_cluster_pixels) + continue; + + // a cluster should contain only boundary points around the + // tag. it cannot be bigger than the whole screen. (Reject + // large connected blobs that will be prohibitively slow to + // fit quads to.) A typical point along an edge is added three + // times (because it has 3 neighbors). The maximum perimeter + // is 2w+2h. + if (zarray_size(*cluster) > 3*(2*w+2*h)) { + continue; + } + + struct quad quad; + memset(&quad, 0, sizeof(struct quad)); + + if (fit_quad(td, task->im, *cluster, &quad, task->tag_width, task->normal_border, task->reversed_border)) { + pthread_mutex_lock(&td->mutex); + zarray_add(quads, &quad); + pthread_mutex_unlock(&td->mutex); + } + } +} + +void do_minmax_task(void *p) +{ + const int tilesz = 4; + struct minmax_task* task = (struct minmax_task*) p; + int s = task->im->stride; + int ty = task->ty; + int tw = task->im->width / tilesz; + image_u8_t *im = task->im; + + for (int tx = 0; tx < tw; tx++) { + uint8_t max = 0, min = 255; + + for (int dy = 0; dy < tilesz; dy++) { + + for (int dx = 0; dx < tilesz; dx++) { + + uint8_t v = im->buf[(ty*tilesz+dy)*s + tx*tilesz + dx]; + if (v < min) + min = v; + if (v > max) + max = v; + } + } + + task->im_max[ty*tw+tx] = max; + task->im_min[ty*tw+tx] = min; + } +} + +void do_blur_task(void *p) +{ + const int tilesz = 4; + struct blur_task* task = (struct blur_task*) p; + int ty = task->ty; + int tw = task->im->width / tilesz; + int th = task->im->height / tilesz; + uint8_t *im_max = task->im_max; + uint8_t *im_min = task->im_min; + + for (int tx = 0; tx < tw; tx++) { + uint8_t max = 0, min = 255; + + for (int dy = -1; dy <= 1; dy++) { + if (ty+dy < 0 || ty+dy >= th) + continue; + for (int dx = -1; dx <= 1; dx++) { + if (tx+dx < 0 || tx+dx >= tw) + continue; + + uint8_t m = im_max[(ty+dy)*tw+tx+dx]; + if (m > max) + max = m; + m = im_min[(ty+dy)*tw+tx+dx]; + if (m < min) + min = m; + } + } + + task->im_max_tmp[ty*tw + tx] = max; + task->im_min_tmp[ty*tw + tx] = min; + } +} + +void do_threshold_task(void *p) +{ + const int tilesz = 4; + struct threshold_task* task = (struct threshold_task*) p; + int ty = task->ty; + int tw = task->im->width / tilesz; + int s = task->im->stride; + uint8_t *im_max = task->im_max; + uint8_t *im_min = task->im_min; + image_u8_t *im = task->im; + image_u8_t *threshim = task->threshim; + int min_white_black_diff = task->td->qtp.min_white_black_diff; + + for (int tx = 0; tx < tw; tx++) { + int min = im_min[ty*tw + tx]; + int max = im_max[ty*tw + tx]; + + // low contrast region? (no edges) + if (max - min < min_white_black_diff) { + for (int dy = 0; dy < tilesz; dy++) { + int y = ty*tilesz + dy; + + for (int dx = 0; dx < tilesz; dx++) { + int x = tx*tilesz + dx; + + threshim->buf[y*s+x] = 127; + } + } + continue; + } + + // otherwise, actually threshold this tile. + + // argument for biasing towards dark; specular highlights + // can be substantially brighter than white tag parts + uint8_t thresh = min + (max - min) / 2; + + for (int dy = 0; dy < tilesz; dy++) { + int y = ty*tilesz + dy; + + for (int dx = 0; dx < tilesz; dx++) { + int x = tx*tilesz + dx; + + uint8_t v = im->buf[y*s+x]; + if (v > thresh) + threshim->buf[y*s+x] = 255; + else + threshim->buf[y*s+x] = 0; + } + } + } +} + +image_u8_t *threshold(apriltag_detector_t *td, image_u8_t *im) +{ + int w = im->width, h = im->height, s = im->stride; + assert(w < 32768); + assert(h < 32768); + + image_u8_t *threshim = image_u8_create_alignment(w, h, s); + assert(threshim->stride == s); + + // The idea is to find the maximum and minimum values in a + // window around each pixel. If it's a contrast-free region + // (max-min is small), don't try to binarize. Otherwise, + // threshold according to (max+min)/2. + // + // Mark low-contrast regions with value 127 so that we can skip + // future work on these areas too. + + // however, computing max/min around every pixel is needlessly + // expensive. We compute max/min for tiles. To avoid artifacts + // that arise when high-contrast features appear near a tile + // edge (and thus moving from one tile to another results in a + // large change in max/min value), the max/min values used for + // any pixel are computed from all 3x3 surrounding tiles. Thus, + // the max/min sampling area for nearby pixels overlap by at least + // one tile. + // + // The important thing is that the windows be large enough to + // capture edge transitions; the tag does not need to fit into + // a tile. + + // XXX Tunable. Generally, small tile sizes--- so long as they're + // large enough to span a single tag edge--- seem to be a winner. + const int tilesz = 4; + + // the last (possibly partial) tiles along each row and column will + // just use the min/max value from the last full tile. + int tw = w / tilesz; + int th = h / tilesz; + + uint8_t *im_max = calloc(tw*th, sizeof(uint8_t)); + uint8_t *im_min = calloc(tw*th, sizeof(uint8_t)); + + struct minmax_task *minmax_tasks = malloc(sizeof(struct minmax_task)*th); + // first, collect min/max statistics for each tile + for (int ty = 0; ty < th; ty++) { + minmax_tasks[ty].im = im; + minmax_tasks[ty].im_max = im_max; + minmax_tasks[ty].im_min = im_min; + minmax_tasks[ty].ty = ty; + + workerpool_add_task(td->wp, do_minmax_task, &minmax_tasks[ty]); + } + workerpool_run(td->wp); + free(minmax_tasks); + + // second, apply 3x3 max/min convolution to "blur" these values + // over larger areas. This reduces artifacts due to abrupt changes + // in the threshold value. + if (1) { + uint8_t *im_max_tmp = calloc(tw*th, sizeof(uint8_t)); + uint8_t *im_min_tmp = calloc(tw*th, sizeof(uint8_t)); + + struct blur_task *blur_tasks = malloc(sizeof(struct blur_task)*th); + for (int ty = 0; ty < th; ty++) { + blur_tasks[ty].im = im; + blur_tasks[ty].im_max = im_max; + blur_tasks[ty].im_min = im_min; + blur_tasks[ty].im_max_tmp = im_max_tmp; + blur_tasks[ty].im_min_tmp = im_min_tmp; + blur_tasks[ty].ty = ty; + + workerpool_add_task(td->wp, do_blur_task, &blur_tasks[ty]); + } + workerpool_run(td->wp); + free(blur_tasks); + free(im_max); + free(im_min); + im_max = im_max_tmp; + im_min = im_min_tmp; + } + + struct threshold_task *threshold_tasks = malloc(sizeof(struct threshold_task)*th); + for (int ty = 0; ty < th; ty++) { + threshold_tasks[ty].im = im; + threshold_tasks[ty].threshim = threshim; + threshold_tasks[ty].im_max = im_max; + threshold_tasks[ty].im_min = im_min; + threshold_tasks[ty].ty = ty; + threshold_tasks[ty].td = td; + + workerpool_add_task(td->wp, do_threshold_task, &threshold_tasks[ty]); + } + workerpool_run(td->wp); + free(threshold_tasks); + + // we skipped over the non-full-sized tiles above. Fix those now. + if (1) { + for (int y = 0; y < h; y++) { + + // what is the first x coordinate we need to process in this row? + + int x0; + + if (y >= th*tilesz) { + x0 = 0; // we're at the bottom; do the whole row. + } else { + x0 = tw*tilesz; // we only need to do the right most part. + } + + // compute tile coordinates and clamp. + int ty = y / tilesz; + if (ty >= th) + ty = th - 1; + + for (int x = x0; x < w; x++) { + int tx = x / tilesz; + if (tx >= tw) + tx = tw - 1; + + int max = im_max[ty*tw + tx]; + int min = im_min[ty*tw + tx]; + int thresh = min + (max - min) / 2; + + uint8_t v = im->buf[y*s+x]; + if (v > thresh) + threshim->buf[y*s+x] = 255; + else + threshim->buf[y*s+x] = 0; + } + } + } + + free(im_min); + free(im_max); + + // this is a dilate/erode deglitching scheme that does not improve + // anything as far as I can tell. + if (td->qtp.deglitch) { + image_u8_t *tmp = image_u8_create(w, h); + + for (int y = 1; y + 1 < h; y++) { + for (int x = 1; x + 1 < w; x++) { + uint8_t max = 0; + for (int dy = -1; dy <= 1; dy++) { + for (int dx = -1; dx <= 1; dx++) { + uint8_t v = threshim->buf[(y+dy)*s + x + dx]; + if (v > max) + max = v; + } + } + tmp->buf[y*s+x] = max; + } + } + + for (int y = 1; y + 1 < h; y++) { + for (int x = 1; x + 1 < w; x++) { + uint8_t min = 255; + for (int dy = -1; dy <= 1; dy++) { + for (int dx = -1; dx <= 1; dx++) { + uint8_t v = tmp->buf[(y+dy)*s + x + dx]; + if (v < min) + min = v; + } + } + threshim->buf[y*s+x] = min; + } + } + + image_u8_destroy(tmp); + } + + timeprofile_stamp(td->tp, "threshold"); + + return threshim; +} + +// basically the same as threshold(), but assumes the input image is a +// bayer image. It collects statistics separately for each 2x2 block +// of pixels. NOT WELL TESTED. +image_u8_t *threshold_bayer(apriltag_detector_t *td, image_u8_t *im) +{ + int w = im->width, h = im->height, s = im->stride; + + image_u8_t *threshim = image_u8_create_alignment(w, h, s); + assert(threshim->stride == s); + + int tilesz = 32; + assert((tilesz & 1) == 0); // must be multiple of 2 + + int tw = w/tilesz + 1; + int th = h/tilesz + 1; + + uint8_t *im_max[4], *im_min[4]; + for (int i = 0; i < 4; i++) { + im_max[i] = calloc(tw*th, sizeof(uint8_t)); + im_min[i] = calloc(tw*th, sizeof(uint8_t)); + } + + for (int ty = 0; ty < th; ty++) { + for (int tx = 0; tx < tw; tx++) { + + uint8_t max[4] = { 0, 0, 0, 0}; + uint8_t min[4] = { 255, 255, 255, 255 }; + + for (int dy = 0; dy < tilesz; dy++) { + if (ty*tilesz+dy >= h) + continue; + + for (int dx = 0; dx < tilesz; dx++) { + if (tx*tilesz+dx >= w) + continue; + + // which bayer element is this pixel? + int idx = (2*(dy&1) + (dx&1)); + + uint8_t v = im->buf[(ty*tilesz+dy)*s + tx*tilesz + dx]; + if (v < min[idx]) + min[idx] = v; + if (v > max[idx]) + max[idx] = v; + } + } + + for (int i = 0; i < 4; i++) { + im_max[i][ty*tw+tx] = max[i]; + im_min[i][ty*tw+tx] = min[i]; + } + } + } + + for (int ty = 0; ty < th; ty++) { + for (int tx = 0; tx < tw; tx++) { + + uint8_t max[4] = { 0, 0, 0, 0}; + uint8_t min[4] = { 255, 255, 255, 255 }; + + for (int dy = -1; dy <= 1; dy++) { + if (ty+dy < 0 || ty+dy >= th) + continue; + for (int dx = -1; dx <= 1; dx++) { + if (tx+dx < 0 || tx+dx >= tw) + continue; + + for (int i = 0; i < 4; i++) { + uint8_t m = im_max[i][(ty+dy)*tw+tx+dx]; + if (m > max[i]) + max[i] = m; + m = im_min[i][(ty+dy)*tw+tx+dx]; + if (m < min[i]) + min[i] = m; + } + } + } + + // XXX CONSTANT +// if (max - min < 30) +// continue; + + // argument for biasing towards dark: specular highlights + // can be substantially brighter than white tag parts + uint8_t thresh[4]; + for (int i = 0; i < 4; i++) { + thresh[i] = min[i] + (max[i] - min[i]) / 2; + } + + for (int dy = 0; dy < tilesz; dy++) { + int y = ty*tilesz + dy; + if (y >= h) + continue; + + for (int dx = 0; dx < tilesz; dx++) { + int x = tx*tilesz + dx; + if (x >= w) + continue; + + // which bayer element is this pixel? + int idx = (2*(y&1) + (x&1)); + + uint8_t v = im->buf[y*s+x]; + threshim->buf[y*s+x] = v > thresh[idx]; + } + } + } + } + + for (int i = 0; i < 4; i++) { + free(im_min[i]); + free(im_max[i]); + } + + timeprofile_stamp(td->tp, "threshold"); + + return threshim; +} + +unionfind_t* connected_components(apriltag_detector_t *td, image_u8_t* threshim, int w, int h, int ts) { + unionfind_t *uf = unionfind_create(w * h); + + if (td->nthreads <= 1) { + do_unionfind_first_line(uf, threshim, h, w, ts); + for (int y = 1; y < h; y++) { + do_unionfind_line2(uf, threshim, h, w, ts, y); + } + } else { + do_unionfind_first_line(uf, threshim, h, w, ts); + + int sz = h; + int chunksize = 1 + sz / (APRILTAG_TASKS_PER_THREAD_TARGET * td->nthreads); + struct unionfind_task *tasks = malloc(sizeof(struct unionfind_task)*(sz / chunksize + 1)); + + int ntasks = 0; + + for (int i = 1; i < sz; i += chunksize) { + // each task will process [y0, y1). Note that this attaches + // each cell to the right and down, so row y1 *is* potentially modified. + // + // for parallelization, make sure that each task doesn't touch rows + // used by another thread. + tasks[ntasks].y0 = i; + tasks[ntasks].y1 = imin(sz, i + chunksize - 1); + tasks[ntasks].h = h; + tasks[ntasks].w = w; + tasks[ntasks].s = ts; + tasks[ntasks].uf = uf; + tasks[ntasks].im = threshim; + + workerpool_add_task(td->wp, do_unionfind_task2, &tasks[ntasks]); + ntasks++; + } + + workerpool_run(td->wp); + + // XXX stitch together the different chunks. + for (int i = 1; i < ntasks; i++) { + do_unionfind_line2(uf, threshim, h, w, ts, tasks[i].y0 - 1); + } + + free(tasks); + } + return uf; +} + +zarray_t* do_gradient_clusters(image_u8_t* threshim, int ts, int y0, int y1, int w, int nclustermap, unionfind_t* uf, zarray_t* clusters) { + struct uint64_zarray_entry **clustermap = calloc(nclustermap, sizeof(struct uint64_zarray_entry*)); + + int mem_chunk_size = 2048; + struct uint64_zarray_entry** mem_pools = malloc(sizeof(struct uint64_zarray_entry *)*(1 + 2 * nclustermap / mem_chunk_size)); // SmodeTech: avoid memory corruption when nclustermap < mem_chunk_size + int mem_pool_idx = 0; + int mem_pool_loc = 0; + mem_pools[mem_pool_idx] = calloc(mem_chunk_size, sizeof(struct uint64_zarray_entry)); + + for (int y = y0; y < y1; y++) { + for (int x = 1; x < w-1; x++) { + + uint8_t v0 = threshim->buf[y*ts + x]; + if (v0 == 127) + continue; + + // XXX don't query this until we know we need it? + uint64_t rep0 = unionfind_get_representative(uf, y*w + x); + if (unionfind_get_set_size(uf, rep0) < 25) { + continue; + } + + // whenever we find two adjacent pixels such that one is + // white and the other black, we add the point half-way + // between them to a cluster associated with the unique + // ids of the white and black regions. + // + // We additionally compute the gradient direction (i.e., which + // direction was the white pixel?) Note: if (v1-v0) == 255, then + // (dx,dy) points towards the white pixel. if (v1-v0) == -255, then + // (dx,dy) points towards the black pixel. p.gx and p.gy will thus + // be -255, 0, or 255. + // + // Note that any given pixel might be added to multiple + // different clusters. But in the common case, a given + // pixel will be added multiple times to the same cluster, + // which increases the size of the cluster and thus the + // computational costs. + // + // A possible optimization would be to combine entries + // within the same cluster. + +#define DO_CONN(dx, dy) \ + if (1) { \ + uint8_t v1 = threshim->buf[(y + dy)*ts + x + dx]; \ + \ + if (v0 + v1 == 255) { \ + uint64_t rep1 = unionfind_get_representative(uf, (y + dy)*w + x + dx); \ + if (unionfind_get_set_size(uf, rep1) > 24) { \ + uint64_t clusterid; \ + if (rep0 < rep1) \ + clusterid = (rep1 << 32) + rep0; \ + else \ + clusterid = (rep0 << 32) + rep1; \ + \ + /* XXX lousy hash function */ \ + uint32_t clustermap_bucket = u64hash_2(clusterid) % nclustermap; \ + struct uint64_zarray_entry *entry = clustermap[clustermap_bucket]; \ + while (entry && entry->id != clusterid) { \ + entry = entry->next; \ + } \ + \ + if (!entry) { \ + if (mem_pool_loc == mem_chunk_size) { \ + mem_pool_loc = 0; \ + mem_pool_idx++; \ + mem_pools[mem_pool_idx] = calloc(mem_chunk_size, sizeof(struct uint64_zarray_entry)); \ + } \ + entry = mem_pools[mem_pool_idx] + mem_pool_loc; \ + mem_pool_loc++; \ + \ + entry->id = clusterid; \ + entry->cluster = zarray_create(sizeof(struct pt)); \ + entry->next = clustermap[clustermap_bucket]; \ + clustermap[clustermap_bucket] = entry; \ + } \ + \ + struct pt p = { .x = 2*x + dx, .y = 2*y + dy, .gx = dx*((int) v1-v0), .gy = dy*((int) v1-v0)}; \ + zarray_add(entry->cluster, &p); \ + } \ + } \ + } + + // do 4 connectivity. NB: Arguments must be [-1, 1] or we'll overflow .gx, .gy + DO_CONN(1, 0); + DO_CONN(0, 1); + + // do 8 connectivity + DO_CONN(-1, 1); + DO_CONN(1, 1); + } + } +#undef DO_CONN + + for (int i = 0; i < nclustermap; i++) { + int start = zarray_size(clusters); + for (struct uint64_zarray_entry *entry = clustermap[i]; entry; entry = entry->next) { + struct cluster_hash* cluster_hash = malloc(sizeof(struct cluster_hash)); + cluster_hash->hash = u64hash_2(entry->id) % nclustermap; + cluster_hash->id = entry->id; + cluster_hash->data = entry->cluster; + zarray_add(clusters, &cluster_hash); + } + int end = zarray_size(clusters); + + // Do a quick bubblesort on the secondary key. + int n = end - start; + for (int j = 0; j < n - 1; j++) { + for (int k = 0; k < n - j - 1; k++) { + struct cluster_hash** hash1; + struct cluster_hash** hash2; + zarray_get_volatile(clusters, start + k, &hash1); + zarray_get_volatile(clusters, start + k + 1, &hash2); + if ((*hash1)->id > (*hash2)->id) { + struct cluster_hash tmp = **hash2; + **hash2 = **hash1; + **hash1 = tmp; + } + } + } + } + for (int i = 0; i <= mem_pool_idx; i++) { + free(mem_pools[i]); + } + free(mem_pools); + free(clustermap); + + return clusters; +} + +static void do_cluster_task(void *p) +{ + struct cluster_task *task = (struct cluster_task*) p; + + do_gradient_clusters(task->im, task->s, task->y0, task->y1, task->w, task->nclustermap, task->uf, task->clusters); +} + +zarray_t* merge_clusters(zarray_t* c1, zarray_t* c2) { + zarray_t* ret = zarray_create(sizeof(struct cluster_hash*)); + zarray_ensure_capacity(ret, zarray_size(c1) + zarray_size(c2)); + + int i1 = 0; + int i2 = 0; + int l1 = zarray_size(c1); + int l2 = zarray_size(c2); + + while (i1 < l1 && i2 < l2) { + struct cluster_hash** h1; + struct cluster_hash** h2; + zarray_get_volatile(c1, i1, &h1); + zarray_get_volatile(c2, i2, &h2); + + if ((*h1)->hash == (*h2)->hash && (*h1)->id == (*h2)->id) { + zarray_add_range((*h1)->data, (*h2)->data, 0, zarray_size((*h2)->data)); + zarray_add(ret, h1); + i1++; + i2++; + zarray_destroy((*h2)->data); + free(*h2); + } else if ((*h2)->hash < (*h1)->hash || ((*h2)->hash == (*h1)->hash && (*h2)->id < (*h1)->id)) { + zarray_add(ret, h2); + i2++; + } else { + zarray_add(ret, h1); + i1++; + } + } + + zarray_add_range(ret, c1, i1, l1); + zarray_add_range(ret, c2, i2, l2); + + zarray_destroy(c1); + zarray_destroy(c2); + + return ret; +} + +zarray_t* gradient_clusters(apriltag_detector_t *td, image_u8_t* threshim, int w, int h, int ts, unionfind_t* uf) { + zarray_t* clusters; + int nclustermap = 0.2*w*h; + + int sz = h - 1; + int chunksize = 1 + sz / (APRILTAG_TASKS_PER_THREAD_TARGET * td->nthreads); + struct cluster_task *tasks = malloc(sizeof(struct cluster_task)*(sz / chunksize + 1)); + + int ntasks = 0; + + for (int i = 1; i < sz; i += chunksize) { + // each task will process [y0, y1). Note that this processes + // each cell to the right and down. + tasks[ntasks].y0 = i; + tasks[ntasks].y1 = imin(sz, i + chunksize); + tasks[ntasks].w = w; + tasks[ntasks].s = ts; + tasks[ntasks].uf = uf; + tasks[ntasks].im = threshim; + tasks[ntasks].nclustermap = nclustermap/(sz / chunksize + 1); + tasks[ntasks].clusters = zarray_create(sizeof(struct cluster_hash*)); + + workerpool_add_task(td->wp, do_cluster_task, &tasks[ntasks]); + ntasks++; + } + + workerpool_run(td->wp); + + zarray_t** clusters_list = malloc(sizeof(zarray_t *)*ntasks); + for (int i = 0; i < ntasks; i++) { + clusters_list[i] = tasks[i].clusters; + } + + int length = ntasks; + while (length > 1) { + int write = 0; + for (int i = 0; i < length - 1; i += 2) { + clusters_list[write] = merge_clusters(clusters_list[i], clusters_list[i + 1]); + write++; + } + + if (length % 2) { + clusters_list[write] = clusters_list[length - 1]; + } + + length = (length >> 1) + length % 2; + } + + clusters = zarray_create(sizeof(zarray_t*)); + zarray_ensure_capacity(clusters, zarray_size(clusters_list[0])); + for (int i = 0; i < zarray_size(clusters_list[0]); i++) { + struct cluster_hash** hash; + zarray_get_volatile(clusters_list[0], i, &hash); + zarray_add(clusters, &(*hash)->data); + free(*hash); + } + zarray_destroy(clusters_list[0]); + free(clusters_list); + free(tasks); + return clusters; +} + +zarray_t* fit_quads(apriltag_detector_t *td, int w, int h, zarray_t* clusters, image_u8_t* im) { + zarray_t *quads = zarray_create(sizeof(struct quad)); + + bool normal_border = false; + bool reversed_border = false; + int min_tag_width = 1000000; + for (int i = 0; i < zarray_size(td->tag_families); i++) { + apriltag_family_t* family; + zarray_get(td->tag_families, i, &family); + if (family->width_at_border < min_tag_width) { + min_tag_width = family->width_at_border; + } + normal_border |= !family->reversed_border; + reversed_border |= family->reversed_border; + } + min_tag_width /= td->quad_decimate; + if (min_tag_width < 3) { + min_tag_width = 3; + } + + int sz = zarray_size(clusters); + int chunksize = 1 + sz / (APRILTAG_TASKS_PER_THREAD_TARGET * td->nthreads); + struct quad_task *tasks = malloc(sizeof(struct quad_task)*(sz / chunksize + 1)); + + int ntasks = 0; + for (int i = 0; i < sz; i += chunksize) { + tasks[ntasks].td = td; + tasks[ntasks].cidx0 = i; + tasks[ntasks].cidx1 = imin(sz, i + chunksize); + tasks[ntasks].h = h; + tasks[ntasks].w = w; + tasks[ntasks].quads = quads; + tasks[ntasks].clusters = clusters; + tasks[ntasks].im = im; + tasks[ntasks].tag_width = min_tag_width; + tasks[ntasks].normal_border = normal_border; + tasks[ntasks].reversed_border = reversed_border; + + workerpool_add_task(td->wp, do_quad_task, &tasks[ntasks]); + ntasks++; + } + + workerpool_run(td->wp); + + free(tasks); + + return quads; +} + +zarray_t *apriltag_quad_thresh(apriltag_detector_t *td, image_u8_t *im) +{ + //////////////////////////////////////////////////////// + // step 1. threshold the image, creating the edge image. + + int w = im->width, h = im->height; + + image_u8_t *threshim = threshold(td, im); + int ts = threshim->stride; + + if (td->debug) + image_u8_write_pnm(threshim, "debug_threshold.pnm"); + + + //////////////////////////////////////////////////////// + // step 2. find connected components. + unionfind_t* uf = connected_components(td, threshim, w, h, ts); + + // make segmentation image. + if (td->debug) { + image_u8x3_t *d = image_u8x3_create(w, h); + + uint32_t *colors = (uint32_t*) calloc(w*h, sizeof(*colors)); + + for (int y = 0; y < h; y++) { + for (int x = 0; x < w; x++) { + uint32_t v = unionfind_get_representative(uf, y*w+x); + + if (unionfind_get_set_size(uf, v) < td->qtp.min_cluster_pixels) + continue; + + uint32_t color = colors[v]; + uint8_t r = color >> 16, + g = color >> 8, + b = color; + + if (color == 0) { + const int bias = 50; + r = bias + (random() % (200-bias)); + g = bias + (random() % (200-bias)); + b = bias + (random() % (200-bias)); + colors[v] = (r << 16) | (g << 8) | b; + } + + d->buf[y*d->stride + 3*x + 0] = r; + d->buf[y*d->stride + 3*x + 1] = g; + d->buf[y*d->stride + 3*x + 2] = b; + } + } + + free(colors); + + image_u8x3_write_pnm(d, "debug_segmentation.pnm"); + image_u8x3_destroy(d); + } + + + timeprofile_stamp(td->tp, "unionfind"); + + zarray_t* clusters = gradient_clusters(td, threshim, w, h, ts, uf); + + if (td->debug) { + image_u8x3_t *d = image_u8x3_create(w, h); + + for (int i = 0; i < zarray_size(clusters); i++) { + zarray_t *cluster; + zarray_get(clusters, i, &cluster); + + uint32_t r, g, b; + + if (1) { + const int bias = 50; + r = bias + (random() % (200-bias)); + g = bias + (random() % (200-bias)); + b = bias + (random() % (200-bias)); + } + + for (int j = 0; j < zarray_size(cluster); j++) { + struct pt *p; + zarray_get_volatile(cluster, j, &p); + + int x = p->x / 2; + int y = p->y / 2; + d->buf[y*d->stride + 3*x + 0] = r; + d->buf[y*d->stride + 3*x + 1] = g; + d->buf[y*d->stride + 3*x + 2] = b; + } + } + + image_u8x3_write_pnm(d, "debug_clusters.pnm"); + image_u8x3_destroy(d); + } + + + image_u8_destroy(threshim); + timeprofile_stamp(td->tp, "make clusters"); + + //////////////////////////////////////////////////////// + // step 3. process each connected component. + + zarray_t* quads = fit_quads(td, w, h, clusters, im); + + if (td->debug) { + FILE *f = fopen("debug_lines.ps", "w"); + fprintf(f, "%%!PS\n\n"); + + image_u8_t *im2 = image_u8_copy(im); + image_u8_darken(im2); + image_u8_darken(im2); + + // assume letter, which is 612x792 points. + double scale = fmin(612.0/im->width, 792.0/im2->height); + fprintf(f, "%.15f %.15f scale\n", scale, scale); + fprintf(f, "0 %d translate\n", im2->height); + fprintf(f, "1 -1 scale\n"); + + image_u8_destroy(im2); + + for (int i = 0; i < zarray_size(quads); i++) { + struct quad *q; + zarray_get_volatile(quads, i, &q); + + float rgb[3]; + int bias = 100; + + for (int j = 0; j < 3; j++) + rgb[j] = bias + (random() % (255-bias)); + + fprintf(f, "%f %f %f setrgbcolor\n", rgb[0]/255.0f, rgb[1]/255.0f, rgb[2]/255.0f); + fprintf(f, "%.15f %.15f moveto %.15f %.15f lineto %.15f %.15f lineto %.15f %.15f lineto %.15f %.15f lineto stroke\n", + q->p[0][0], q->p[0][1], + q->p[1][0], q->p[1][1], + q->p[2][0], q->p[2][1], + q->p[3][0], q->p[3][1], + q->p[0][0], q->p[0][1]); + } + + fclose(f); + } + + timeprofile_stamp(td->tp, "fit quads to clusters"); + + unionfind_destroy(uf); + + for (int i = 0; i < zarray_size(clusters); i++) { + zarray_t *cluster; + zarray_get(clusters, i, &cluster); + zarray_destroy(cluster); + } + zarray_destroy(clusters); + + return quads; +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/common/g2d.c b/apriltag/src/main/native/thirdparty/apriltag/src/common/g2d.c new file mode 100644 index 0000000000..e010643771 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/common/g2d.c @@ -0,0 +1,919 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#include +#include +#include +#include + +#include "g2d.h" +#include "common/math_util.h" + +#ifdef _WIN32 +static inline long int random(void) +{ + return rand(); +} +#endif + +double g2d_distance(const double a[2], const double b[2]) +{ + return sqrtf(sq(a[0]-b[0]) + sq(a[1]-b[1])); +} + +zarray_t *g2d_polygon_create_empty(void) +{ + return zarray_create(sizeof(double[2])); +} + +void g2d_polygon_add(zarray_t *poly, double v[2]) +{ + zarray_add(poly, v); +} + +zarray_t *g2d_polygon_create_data(double v[][2], int sz) +{ + zarray_t *points = g2d_polygon_create_empty(); + + for (int i = 0; i < sz; i++) + g2d_polygon_add(points, v[i]); + + return points; +} + +zarray_t *g2d_polygon_create_zeros(int sz) +{ + zarray_t *points = zarray_create(sizeof(double[2])); + + double z[2] = { 0, 0 }; + + for (int i = 0; i < sz; i++) + zarray_add(points, z); + + return points; +} + +void g2d_polygon_make_ccw(zarray_t *poly) +{ + // Step one: we want the points in counter-clockwise order. + // If the points are in clockwise order, we'll reverse them. + double total_theta = 0; + double last_theta = 0; + + // Count the angle accumulated going around the polygon. If + // the sum is +2pi, it's CCW. Otherwise, we'll get -2pi. + int sz = zarray_size(poly); + + for (int i = 0; i <= sz; i++) { + double p0[2], p1[2]; + zarray_get(poly, i % sz, &p0); + zarray_get(poly, (i+1) % sz, &p1); + + double this_theta = atan2(p1[1]-p0[1], p1[0]-p0[0]); + + if (i > 0) { + double dtheta = mod2pi(this_theta-last_theta); + total_theta += dtheta; + } + + last_theta = this_theta; + } + + int ccw = (total_theta > 0); + + // reverse order if necessary. + if (!ccw) { + for (int i = 0; i < sz / 2; i++) { + double a[2], b[2]; + + zarray_get(poly, i, a); + zarray_get(poly, sz-1-i, b); + zarray_set(poly, i, b, NULL); + zarray_set(poly, sz-1-i, a, NULL); + } + } +} + +int g2d_polygon_contains_point_ref(const zarray_t *poly, double q[2]) +{ + // use winding. If the point is inside the polygon, we'll wrap + // around it (accumulating 6.28 radians). If we're outside the + // polygon, we'll accumulate zero. + int psz = zarray_size(poly); + + double acc_theta = 0; + + double last_theta; + + for (int i = 0; i <= psz; i++) { + double p[2]; + + zarray_get(poly, i % psz, &p); + + double this_theta = atan2(q[1]-p[1], q[0]-p[0]); + + if (i != 0) + acc_theta += mod2pi(this_theta - last_theta); + + last_theta = this_theta; + } + + return acc_theta > M_PI; +} + +/* +// sort by x coordinate, ascending +static int g2d_convex_hull_sort(const void *_a, const void *_b) +{ + double *a = (double*) _a; + double *b = (double*) _b; + + if (a[0] < b[0]) + return -1; + if (a[0] == b[0]) + return 0; + return 1; +} +*/ + +/* +zarray_t *g2d_convex_hull2(const zarray_t *points) +{ + zarray_t *hull = zarray_copy(points); + + zarray_sort(hull, g2d_convex_hull_sort); + + int hsz = zarray_size(hull); + int hout = 0; + + for (int hin = 1; hin < hsz; hin++) { + double *p; + zarray_get_volatile(hull, i, &p); + + // Everything to the right of hin is already convex. We now + // add one point, p, which begins "connected" by two + // (coincident) edges from the last right-most point to p. + double *last; + zarray_get_volatile(hull, hout, &last); + + // We now remove points from the convex hull by moving + } + + return hull; +} +*/ + +// creates and returns a zarray(double[2]). The resulting polygon is +// CCW and implicitly closed. Unnecessary colinear points are omitted. +zarray_t *g2d_convex_hull(const zarray_t *points) +{ + zarray_t *hull = zarray_create(sizeof(double[2])); + + // gift-wrap algorithm. + + // step 1: find left most point. + int insz = zarray_size(points); + + // must have at least 2 points. (XXX need 3?) + assert(insz >= 2); + + double *pleft = NULL; + for (int i = 0; i < insz; i++) { + double *p; + zarray_get_volatile(points, i, &p); + + if (pleft == NULL || p[0] < pleft[0]) + pleft = p; + } + + // cannot be NULL since there must be at least one point. + assert(pleft != NULL); + + zarray_add(hull, pleft); + + // step 2. gift wrap. Keep searching for points that make the + // smallest-angle left-hand turn. This implementation is carefully + // written to use only addition/subtraction/multiply. No division + // or sqrts. This guarantees exact results for integer-coordinate + // polygons (no rounding/precision problems). + double *p = pleft; + + while (1) { + assert(p != NULL); + + double *q = NULL; + double n0 = 0, n1 = 0; // the normal to the line (p, q) (not + // necessarily unit length). + + // Search for the point q for which the line (p,q) is most "to + // the right of" the other points. (i.e., every time we find a + // point that is to the right of our current line, we change + // lines.) + for (int i = 0; i < insz; i++) { + double *thisq; + zarray_get_volatile(points, i, &thisq); + + if (thisq == p) + continue; + + // the first time we find another point, we initialize our + // value of q, forming the line (p,q) + if (q == NULL) { + q = thisq; + n0 = q[1] - p[1]; + n1 = -q[0] + p[0]; + } else { + // we already have a line (p,q). is point thisq RIGHT OF line (p, q)? + double e0 = thisq[0] - p[0], e1 = thisq[1] - p[1]; + double dot = e0*n0 + e1*n1; + + if (dot > 0) { + // it is. change our line. + q = thisq; + n0 = q[1] - p[1]; + n1 = -q[0] + p[0]; + } + } + } + + // we must have elected *some* line, so long as there are at + // least 2 points in the polygon. + assert(q != NULL); + + // loop completed? + if (q == pleft) + break; + + int colinear = 0; + + // is this new point colinear with the last two? + if (zarray_size(hull) > 1) { + double *o; + zarray_get_volatile(hull, zarray_size(hull) - 2, &o); + + double e0 = o[0] - p[0]; + double e1 = o[1] - p[1]; + + if (n0*e0 + n1*e1 == 0) + colinear = 1; + } + + // if it is colinear, overwrite the last one. + if (colinear) + zarray_set(hull, zarray_size(hull)-1, q, NULL); + else + zarray_add(hull, q); + + p = q; + } + + return hull; +} + +// Find point p on the boundary of poly that is closest to q. +void g2d_polygon_closest_boundary_point(const zarray_t *poly, const double q[2], double *p) +{ + int psz = zarray_size(poly); + double min_dist = HUGE_VALF; + + for (int i = 0; i < psz; i++) { + double *p0, *p1; + + zarray_get_volatile(poly, i, &p0); + zarray_get_volatile(poly, (i+1) % psz, &p1); + + g2d_line_segment_t seg; + g2d_line_segment_init_from_points(&seg, p0, p1); + + double thisp[2]; + g2d_line_segment_closest_point(&seg, q, thisp); + + double dist = g2d_distance(q, thisp); + if (dist < min_dist) { + memcpy(p, thisp, sizeof(double[2])); + min_dist = dist; + } + } +} + +int g2d_polygon_contains_point(const zarray_t *poly, double q[2]) +{ + // use winding. If the point is inside the polygon, we'll wrap + // around it (accumulating 6.28 radians). If we're outside the + // polygon, we'll accumulate zero. + int psz = zarray_size(poly); + assert(psz > 0); + + int last_quadrant; + int quad_acc = 0; + + for (int i = 0; i <= psz; i++) { + double *p; + + zarray_get_volatile(poly, i % psz, &p); + + // p[0] < q[0] p[1] < q[1] quadrant + // 0 0 0 + // 0 1 3 + // 1 0 1 + // 1 1 2 + + // p[1] < q[1] p[0] < q[0] quadrant + // 0 0 0 + // 0 1 1 + // 1 0 3 + // 1 1 2 + + int quadrant; + if (p[0] < q[0]) + quadrant = (p[1] < q[1]) ? 2 : 1; + else + quadrant = (p[1] < q[1]) ? 3 : 0; + + if (i > 0) { + int dquadrant = quadrant - last_quadrant; + + // encourage a jump table by mapping to small positive integers. + switch (dquadrant) { + case -3: + case 1: + quad_acc ++; + break; + case -1: + case 3: + quad_acc --; + break; + case 0: + break; + case -2: + case 2: + { + // get the previous point. + double *p0; + zarray_get_volatile(poly, i-1, &p0); + + // Consider the points p0 and p (the points around the + //polygon that we are tracing) and the query point q. + // + // If we've moved diagonally across quadrants, we want + // to measure whether we have rotated +PI radians or + // -PI radians. We can test this by computing the dot + // product of vector (p0-q) with the vector + // perpendicular to vector (p-q) + double nx = p[1] - q[1]; + double ny = -p[0] + q[0]; + + double dot = nx*(p0[0]-q[0]) + ny*(p0[1]-q[1]); + if (dot < 0) + quad_acc -= 2; + else + quad_acc += 2; + + break; + } + } + } + + last_quadrant = quadrant; + } + + int v = (quad_acc >= 2) || (quad_acc <= -2); + + #if 0 + if (v != g2d_polygon_contains_point_ref(poly, q)) { + printf("FAILURE %d %d\n", v, quad_acc); + exit(-1); + } + #endif + + return v; +} + +void g2d_line_init_from_points(g2d_line_t *line, const double p0[2], const double p1[2]) +{ + line->p[0] = p0[0]; + line->p[1] = p0[1]; + line->u[0] = p1[0]-p0[0]; + line->u[1] = p1[1]-p0[1]; + double mag = sqrtf(sq(line->u[0]) + sq(line->u[1])); + + line->u[0] /= mag; + line->u[1] /= mag; +} + +double g2d_line_get_coordinate(const g2d_line_t *line, const double q[2]) +{ + return (q[0]-line->p[0])*line->u[0] + (q[1]-line->p[1])*line->u[1]; +} + +// Compute intersection of two line segments. If they intersect, +// result is stored in p and 1 is returned. Otherwise, zero is +// returned. p may be NULL. +int g2d_line_intersect_line(const g2d_line_t *linea, const g2d_line_t *lineb, double *p) +{ + // this implementation is many times faster than the original, + // mostly due to avoiding a general-purpose LU decomposition in + // Matrix.inverse(). + double m00, m01, m10, m11; + double i00, i01; + double b00, b10; + + m00 = linea->u[0]; + m01= -lineb->u[0]; + m10 = linea->u[1]; + m11= -lineb->u[1]; + + // determinant of m + double det = m00*m11-m01*m10; + + // parallel lines? + if (fabs(det) < 0.00000001) + return 0; + + // inverse of m + i00 = m11/det; + i01 = -m01/det; + + b00 = lineb->p[0] - linea->p[0]; + b10 = lineb->p[1] - linea->p[1]; + + double x00; //, x10; + x00 = i00*b00+i01*b10; + + if (p != NULL) { + p[0] = linea->u[0]*x00 + linea->p[0]; + p[1] = linea->u[1]*x00 + linea->p[1]; + } + + return 1; +} + + +void g2d_line_segment_init_from_points(g2d_line_segment_t *seg, const double p0[2], const double p1[2]) +{ + g2d_line_init_from_points(&seg->line, p0, p1); + seg->p1[0] = p1[0]; + seg->p1[1] = p1[1]; +} + +// Find the point p on segment seg that is closest to point q. +void g2d_line_segment_closest_point(const g2d_line_segment_t *seg, const double *q, double *p) +{ + double a = g2d_line_get_coordinate(&seg->line, seg->line.p); + double b = g2d_line_get_coordinate(&seg->line, seg->p1); + double c = g2d_line_get_coordinate(&seg->line, q); + + if (a < b) + c = dclamp(c, a, b); + else + c = dclamp(c, b, a); + + p[0] = seg->line.p[0] + c * seg->line.u[0]; + p[1] = seg->line.p[1] + c * seg->line.u[1]; +} + +// Compute intersection of two line segments. If they intersect, +// result is stored in p and 1 is returned. Otherwise, zero is +// returned. p may be NULL. +int g2d_line_segment_intersect_segment(const g2d_line_segment_t *sega, const g2d_line_segment_t *segb, double *p) +{ + double tmp[2]; + + if (!g2d_line_intersect_line(&sega->line, &segb->line, tmp)) + return 0; + + double a = g2d_line_get_coordinate(&sega->line, sega->line.p); + double b = g2d_line_get_coordinate(&sega->line, sega->p1); + double c = g2d_line_get_coordinate(&sega->line, tmp); + + // does intersection lie on the first line? + if ((ca && c>b)) + return 0; + + a = g2d_line_get_coordinate(&segb->line, segb->line.p); + b = g2d_line_get_coordinate(&segb->line, segb->p1); + c = g2d_line_get_coordinate(&segb->line, tmp); + + // does intersection lie on second line? + if ((ca && c>b)) + return 0; + + if (p != NULL) { + p[0] = tmp[0]; + p[1] = tmp[1]; + } + + return 1; +} + +// Compute intersection of a line segment and a line. If they +// intersect, result is stored in p and 1 is returned. Otherwise, zero +// is returned. p may be NULL. +int g2d_line_segment_intersect_line(const g2d_line_segment_t *seg, const g2d_line_t *line, double *p) +{ + double tmp[2]; + + if (!g2d_line_intersect_line(&seg->line, line, tmp)) + return 0; + + double a = g2d_line_get_coordinate(&seg->line, seg->line.p); + double b = g2d_line_get_coordinate(&seg->line, seg->p1); + double c = g2d_line_get_coordinate(&seg->line, tmp); + + // does intersection lie on the first line? + if ((ca && c>b)) + return 0; + + if (p != NULL) { + p[0] = tmp[0]; + p[1] = tmp[1]; + } + + return 1; +} + +// do the edges of polya and polyb collide? (Does NOT test for containment). +int g2d_polygon_intersects_polygon(const zarray_t *polya, const zarray_t *polyb) +{ + // do any of the line segments collide? If so, the answer is no. + + // dumb N^2 method. + for (int ia = 0; ia < zarray_size(polya); ia++) { + double pa0[2], pa1[2]; + zarray_get(polya, ia, pa0); + zarray_get(polya, (ia+1)%zarray_size(polya), pa1); + + g2d_line_segment_t sega; + g2d_line_segment_init_from_points(&sega, pa0, pa1); + + for (int ib = 0; ib < zarray_size(polyb); ib++) { + double pb0[2], pb1[2]; + zarray_get(polyb, ib, pb0); + zarray_get(polyb, (ib+1)%zarray_size(polyb), pb1); + + g2d_line_segment_t segb; + g2d_line_segment_init_from_points(&segb, pb0, pb1); + + if (g2d_line_segment_intersect_segment(&sega, &segb, NULL)) + return 1; + } + } + + return 0; +} + +// does polya completely contain polyb? +int g2d_polygon_contains_polygon(const zarray_t *polya, const zarray_t *polyb) +{ + // do any of the line segments collide? If so, the answer is no. + if (g2d_polygon_intersects_polygon(polya, polyb)) + return 0; + + // if none of the edges cross, then the polygon is either fully + // contained or fully outside. + double p[2]; + zarray_get(polyb, 0, p); + + return g2d_polygon_contains_point(polya, p); +} + +// compute a point that is inside the polygon. (It may not be *far* inside though) +void g2d_polygon_get_interior_point(const zarray_t *poly, double *p) +{ + // take the first three points, which form a triangle. Find the middle point + double a[2], b[2], c[2]; + + zarray_get(poly, 0, a); + zarray_get(poly, 1, b); + zarray_get(poly, 2, c); + + p[0] = (a[0]+b[0]+c[0])/3; + p[1] = (a[1]+b[1]+c[1])/3; +} + +int g2d_polygon_overlaps_polygon(const zarray_t *polya, const zarray_t *polyb) +{ + // do any of the line segments collide? If so, the answer is yes. + if (g2d_polygon_intersects_polygon(polya, polyb)) + return 1; + + // if none of the edges cross, then the polygon is either fully + // contained or fully outside. + double p[2]; + g2d_polygon_get_interior_point(polyb, p); + + if (g2d_polygon_contains_point(polya, p)) + return 1; + + g2d_polygon_get_interior_point(polya, p); + + if (g2d_polygon_contains_point(polyb, p)) + return 1; + + return 0; +} + +static int double_sort_up(const void *_a, const void *_b) +{ + double a = *((double*) _a); + double b = *((double*) _b); + + if (a < b) + return -1; + + if (a == b) + return 0; + + return 1; +} + +// Compute the crossings of the polygon along line y, storing them in +// the array x. X must be allocated to be at least as long as +// zarray_size(poly). X will be sorted, ready for +// rasterization. Returns the number of intersections (and elements +// written to x). +/* + To rasterize, do something like this: + + double res = 0.099; + for (double y = y0; y < y1; y += res) { + double xs[zarray_size(poly)]; + + int xsz = g2d_polygon_rasterize(poly, y, xs); + int xpos = 0; + int inout = 0; // start off "out" + + for (double x = x0; x < x1; x += res) { + while (x > xs[xpos] && xpos < xsz) { + xpos++; + inout ^= 1; + } + + if (inout) + printf("y"); + else + printf(" "); + } + printf("\n"); +*/ + +// returns the number of x intercepts +int g2d_polygon_rasterize(const zarray_t *poly, double y, double *x) +{ + int sz = zarray_size(poly); + + g2d_line_t line; + if (1) { + double p0[2] = { 0, y }; + double p1[2] = { 1, y }; + + g2d_line_init_from_points(&line, p0, p1); + } + + int xpos = 0; + + for (int i = 0; i < sz; i++) { + g2d_line_segment_t seg; + double *p0, *p1; + zarray_get_volatile(poly, i, &p0); + zarray_get_volatile(poly, (i+1)%sz, &p1); + + g2d_line_segment_init_from_points(&seg, p0, p1); + + double q[2]; + if (g2d_line_segment_intersect_line(&seg, &line, q)) + x[xpos++] = q[0]; + } + + qsort(x, xpos, sizeof(double), double_sort_up); + + return xpos; +} + +/* + /---(1,5) + (-2,4)-/ | + \ | + \ (1,2)--(2,2)\ + \ \ + \ \ + (0,0)------------------(4,0) +*/ +#if 0 + +#include "timeprofile.h" + +int main(int argc, char *argv[]) +{ + timeprofile_t *tp = timeprofile_create(); + + zarray_t *polya = g2d_polygon_create_data((double[][2]) { + { 0, 0}, + { 4, 0}, + { 2, 2}, + { 1, 2}, + { 1, 5}, + { -2,4} }, 6); + + zarray_t *polyb = g2d_polygon_create_data((double[][2]) { + { .1, .1}, + { .5, .1}, + { .1, .5 } }, 3); + + zarray_t *polyc = g2d_polygon_create_data((double[][2]) { + { 3, 0}, + { 5, 0}, + { 5, 1} }, 3); + + zarray_t *polyd = g2d_polygon_create_data((double[][2]) { + { 5, 5}, + { 6, 6}, + { 5, 6} }, 3); + +/* + 5 L---K + 4 |I--J + 3 |H-G + 2 |E-F + 1 |D--C + 0 A---B + 01234 +*/ + zarray_t *polyE = g2d_polygon_create_data((double[][2]) { + {0,0}, {4,0}, {4, 1}, {1,1}, + {1,2}, {3,2}, {3,3}, {1,3}, + {1,4}, {4,4}, {4,5}, {0,5}}, 12); + + srand(0); + + timeprofile_stamp(tp, "begin"); + + if (1) { + int niters = 100000; + + for (int i = 0; i < niters; i++) { + double q[2]; + q[0] = 10.0f * random() / RAND_MAX - 2; + q[1] = 10.0f * random() / RAND_MAX - 2; + + g2d_polygon_contains_point(polyE, q); + } + + timeprofile_stamp(tp, "fast"); + + for (int i = 0; i < niters; i++) { + double q[2]; + q[0] = 10.0f * random() / RAND_MAX - 2; + q[1] = 10.0f * random() / RAND_MAX - 2; + + g2d_polygon_contains_point_ref(polyE, q); + } + + timeprofile_stamp(tp, "slow"); + + for (int i = 0; i < niters; i++) { + double q[2]; + q[0] = 10.0f * random() / RAND_MAX - 2; + q[1] = 10.0f * random() / RAND_MAX - 2; + + int v0 = g2d_polygon_contains_point(polyE, q); + int v1 = g2d_polygon_contains_point_ref(polyE, q); + assert(v0 == v1); + } + + timeprofile_stamp(tp, "both"); + timeprofile_display(tp); + } + + if (1) { + zarray_t *poly = polyE; + + double res = 0.399; + for (double y = 5.2; y >= -.5; y -= res) { + double xs[zarray_size(poly)]; + + int xsz = g2d_polygon_rasterize(poly, y, xs); + int xpos = 0; + int inout = 0; // start off "out" + for (double x = -3; x < 6; x += res) { + while (x > xs[xpos] && xpos < xsz) { + xpos++; + inout ^= 1; + } + + if (inout) + printf("y"); + else + printf(" "); + } + printf("\n"); + + for (double x = -3; x < 6; x += res) { + double q[2] = {x, y}; + if (g2d_polygon_contains_point(poly, q)) + printf("X"); + else + printf(" "); + } + printf("\n"); + } + } + + + +/* +// CW order +double p[][2] = { { 0, 0}, +{ -2, 4}, +{1, 5}, +{1, 2}, +{2, 2}, +{4, 0} }; +*/ + + double q[2] = { 10, 10 }; + printf("0==%d\n", g2d_polygon_contains_point(polya, q)); + + q[0] = 1; q[1] = 1; + printf("1==%d\n", g2d_polygon_contains_point(polya, q)); + + q[0] = 3; q[1] = .5; + printf("1==%d\n", g2d_polygon_contains_point(polya, q)); + + q[0] = 1.2; q[1] = 2.1; + printf("0==%d\n", g2d_polygon_contains_point(polya, q)); + + printf("0==%d\n", g2d_polygon_contains_polygon(polya, polyb)); + + printf("0==%d\n", g2d_polygon_contains_polygon(polya, polyc)); + + printf("0==%d\n", g2d_polygon_contains_polygon(polya, polyd)); + + //////////////////////////////////////////////////////// + // Test convex hull + if (1) { + zarray_t *hull = g2d_convex_hull(polyE); + + for (int k = 0; k < zarray_size(hull); k++) { + double *h; + zarray_get_volatile(hull, k, &h); + + printf("%15f, %15f\n", h[0], h[1]); + } + } + + for (int i = 0; i < 100000; i++) { + zarray_t *points = zarray_create(sizeof(double[2])); + + for (int j = 0; j < 100; j++) { + double q[2]; + q[0] = 10.0f * random() / RAND_MAX - 2; + q[1] = 10.0f * random() / RAND_MAX - 2; + + zarray_add(points, q); + } + + zarray_t *hull = g2d_convex_hull(points); + for (int j = 0; j < zarray_size(points); j++) { + double *q; + zarray_get_volatile(points, j, &q); + + int on_edge; + + double p[2]; + g2d_polygon_closest_boundary_point(hull, q, p); + if (g2d_distance(q, p) < .00001) + on_edge = 1; + + assert(on_edge || g2d_polygon_contains_point(hull, q)); + } + + zarray_destroy(hull); + zarray_destroy(points); + } +} +#endif diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/common/homography.c b/apriltag/src/main/native/thirdparty/apriltag/src/common/homography.c new file mode 100644 index 0000000000..54ab47eb84 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/common/homography.c @@ -0,0 +1,479 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#include + +#include "common/matd.h" +#include "common/zarray.h" +#include "common/homography.h" +#include "common/math_util.h" + +// correspondences is a list of float[4]s, consisting of the points x +// and y concatenated. We will compute a homography such that y = Hx +matd_t *homography_compute(zarray_t *correspondences, int flags) +{ + // compute centroids of both sets of points (yields a better + // conditioned information matrix) + double x_cx = 0, x_cy = 0; + double y_cx = 0, y_cy = 0; + + for (int i = 0; i < zarray_size(correspondences); i++) { + float *c; + zarray_get_volatile(correspondences, i, &c); + + x_cx += c[0]; + x_cy += c[1]; + y_cx += c[2]; + y_cy += c[3]; + } + + int sz = zarray_size(correspondences); + x_cx /= sz; + x_cy /= sz; + y_cx /= sz; + y_cy /= sz; + + // NB We don't normalize scale; it seems implausible that it could + // possibly make any difference given the dynamic range of IEEE + // doubles. + + matd_t *A = matd_create(9,9); + for (int i = 0; i < zarray_size(correspondences); i++) { + float *c; + zarray_get_volatile(correspondences, i, &c); + + // (below world is "x", and image is "y") + double worldx = c[0] - x_cx; + double worldy = c[1] - x_cy; + double imagex = c[2] - y_cx; + double imagey = c[3] - y_cy; + + double a03 = -worldx; + double a04 = -worldy; + double a05 = -1; + double a06 = worldx*imagey; + double a07 = worldy*imagey; + double a08 = imagey; + + MATD_EL(A, 3, 3) += a03*a03; + MATD_EL(A, 3, 4) += a03*a04; + MATD_EL(A, 3, 5) += a03*a05; + MATD_EL(A, 3, 6) += a03*a06; + MATD_EL(A, 3, 7) += a03*a07; + MATD_EL(A, 3, 8) += a03*a08; + MATD_EL(A, 4, 4) += a04*a04; + MATD_EL(A, 4, 5) += a04*a05; + MATD_EL(A, 4, 6) += a04*a06; + MATD_EL(A, 4, 7) += a04*a07; + MATD_EL(A, 4, 8) += a04*a08; + MATD_EL(A, 5, 5) += a05*a05; + MATD_EL(A, 5, 6) += a05*a06; + MATD_EL(A, 5, 7) += a05*a07; + MATD_EL(A, 5, 8) += a05*a08; + MATD_EL(A, 6, 6) += a06*a06; + MATD_EL(A, 6, 7) += a06*a07; + MATD_EL(A, 6, 8) += a06*a08; + MATD_EL(A, 7, 7) += a07*a07; + MATD_EL(A, 7, 8) += a07*a08; + MATD_EL(A, 8, 8) += a08*a08; + + double a10 = worldx; + double a11 = worldy; + double a12 = 1; + double a16 = -worldx*imagex; + double a17 = -worldy*imagex; + double a18 = -imagex; + + MATD_EL(A, 0, 0) += a10*a10; + MATD_EL(A, 0, 1) += a10*a11; + MATD_EL(A, 0, 2) += a10*a12; + MATD_EL(A, 0, 6) += a10*a16; + MATD_EL(A, 0, 7) += a10*a17; + MATD_EL(A, 0, 8) += a10*a18; + MATD_EL(A, 1, 1) += a11*a11; + MATD_EL(A, 1, 2) += a11*a12; + MATD_EL(A, 1, 6) += a11*a16; + MATD_EL(A, 1, 7) += a11*a17; + MATD_EL(A, 1, 8) += a11*a18; + MATD_EL(A, 2, 2) += a12*a12; + MATD_EL(A, 2, 6) += a12*a16; + MATD_EL(A, 2, 7) += a12*a17; + MATD_EL(A, 2, 8) += a12*a18; + MATD_EL(A, 6, 6) += a16*a16; + MATD_EL(A, 6, 7) += a16*a17; + MATD_EL(A, 6, 8) += a16*a18; + MATD_EL(A, 7, 7) += a17*a17; + MATD_EL(A, 7, 8) += a17*a18; + MATD_EL(A, 8, 8) += a18*a18; + + double a20 = -worldx*imagey; + double a21 = -worldy*imagey; + double a22 = -imagey; + double a23 = worldx*imagex; + double a24 = worldy*imagex; + double a25 = imagex; + + MATD_EL(A, 0, 0) += a20*a20; + MATD_EL(A, 0, 1) += a20*a21; + MATD_EL(A, 0, 2) += a20*a22; + MATD_EL(A, 0, 3) += a20*a23; + MATD_EL(A, 0, 4) += a20*a24; + MATD_EL(A, 0, 5) += a20*a25; + MATD_EL(A, 1, 1) += a21*a21; + MATD_EL(A, 1, 2) += a21*a22; + MATD_EL(A, 1, 3) += a21*a23; + MATD_EL(A, 1, 4) += a21*a24; + MATD_EL(A, 1, 5) += a21*a25; + MATD_EL(A, 2, 2) += a22*a22; + MATD_EL(A, 2, 3) += a22*a23; + MATD_EL(A, 2, 4) += a22*a24; + MATD_EL(A, 2, 5) += a22*a25; + MATD_EL(A, 3, 3) += a23*a23; + MATD_EL(A, 3, 4) += a23*a24; + MATD_EL(A, 3, 5) += a23*a25; + MATD_EL(A, 4, 4) += a24*a24; + MATD_EL(A, 4, 5) += a24*a25; + MATD_EL(A, 5, 5) += a25*a25; + } + + // make symmetric + for (int i = 0; i < 9; i++) + for (int j = i+1; j < 9; j++) + MATD_EL(A, j, i) = MATD_EL(A, i, j); + + matd_t *H = matd_create(3,3); + + if (flags & HOMOGRAPHY_COMPUTE_FLAG_INVERSE) { + // compute singular vector by (carefully) inverting the rank-deficient matrix. + + if (1) { + matd_t *Ainv = matd_inverse(A); + double scale = 0; + + for (int i = 0; i < 9; i++) + scale += sq(MATD_EL(Ainv, i, 0)); + scale = sqrt(scale); + + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0) / scale; + + matd_destroy(Ainv); + } else { + + matd_t *b = matd_create_data(9, 1, (double[]) { 1, 0, 0, 0, 0, 0, 0, 0, 0 }); + matd_t *Ainv = NULL; + + if (0) { + matd_plu_t *lu = matd_plu(A); + Ainv = matd_plu_solve(lu, b); + matd_plu_destroy(lu); + } else { + matd_chol_t *chol = matd_chol(A); + Ainv = matd_chol_solve(chol, b); + matd_chol_destroy(chol); + } + + double scale = 0; + + for (int i = 0; i < 9; i++) + scale += sq(MATD_EL(Ainv, i, 0)); + scale = sqrt(scale); + + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0) / scale; + + matd_destroy(b); + matd_destroy(Ainv); + } + + } else { + // compute singular vector using SVD. A bit slower, but more accurate. + matd_svd_t svd = matd_svd_flags(A, MATD_SVD_NO_WARNINGS); + + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + MATD_EL(H, i, j) = MATD_EL(svd.U, 3*i+j, 8); + + matd_destroy(svd.U); + matd_destroy(svd.S); + matd_destroy(svd.V); + + } + + matd_t *Tx = matd_identity(3); + MATD_EL(Tx,0,2) = -x_cx; + MATD_EL(Tx,1,2) = -x_cy; + + matd_t *Ty = matd_identity(3); + MATD_EL(Ty,0,2) = y_cx; + MATD_EL(Ty,1,2) = y_cy; + + matd_t *H2 = matd_op("M*M*M", Ty, H, Tx); + + matd_destroy(A); + matd_destroy(Tx); + matd_destroy(Ty); + matd_destroy(H); + + return H2; +} + + +// assuming that the projection matrix is: +// [ fx 0 cx 0 ] +// [ 0 fy cy 0 ] +// [ 0 0 1 0 ] +// +// And that the homography is equal to the projection matrix times the +// model matrix, recover the model matrix (which is returned). Note +// that the third column of the model matrix is missing in the +// expression below, reflecting the fact that the homography assumes +// all points are at z=0 (i.e., planar) and that the element of z is +// thus omitted. (3x1 instead of 4x1). +// +// [ fx 0 cx 0 ] [ R00 R01 TX ] [ H00 H01 H02 ] +// [ 0 fy cy 0 ] [ R10 R11 TY ] = [ H10 H11 H12 ] +// [ 0 0 1 0 ] [ R20 R21 TZ ] = [ H20 H21 H22 ] +// [ 0 0 1 ] +// +// fx*R00 + cx*R20 = H00 (note, H only known up to scale; some additional adjustments required; see code.) +// fx*R01 + cx*R21 = H01 +// fx*TX + cx*TZ = H02 +// fy*R10 + cy*R20 = H10 +// fy*R11 + cy*R21 = H11 +// fy*TY + cy*TZ = H12 +// R20 = H20 +// R21 = H21 +// TZ = H22 + +matd_t *homography_to_pose(const matd_t *H, double fx, double fy, double cx, double cy) +{ + // Note that every variable that we compute is proportional to the scale factor of H. + double R20 = MATD_EL(H, 2, 0); + double R21 = MATD_EL(H, 2, 1); + double TZ = MATD_EL(H, 2, 2); + double R00 = (MATD_EL(H, 0, 0) - cx*R20) / fx; + double R01 = (MATD_EL(H, 0, 1) - cx*R21) / fx; + double TX = (MATD_EL(H, 0, 2) - cx*TZ) / fx; + double R10 = (MATD_EL(H, 1, 0) - cy*R20) / fy; + double R11 = (MATD_EL(H, 1, 1) - cy*R21) / fy; + double TY = (MATD_EL(H, 1, 2) - cy*TZ) / fy; + + // compute the scale by requiring that the rotation columns are unit length + // (Use geometric average of the two length vectors we have) + double length1 = sqrtf(R00*R00 + R10*R10 + R20*R20); + double length2 = sqrtf(R01*R01 + R11*R11 + R21*R21); + double s = 1.0 / sqrtf(length1 * length2); + + // get sign of S by requiring the tag to be in front the camera; + // we assume camera looks in the -Z direction. + if (TZ > 0) + s *= -1; + + R20 *= s; + R21 *= s; + TZ *= s; + R00 *= s; + R01 *= s; + TX *= s; + R10 *= s; + R11 *= s; + TY *= s; + + // now recover [R02 R12 R22] by noting that it is the cross product of the other two columns. + double R02 = R10*R21 - R20*R11; + double R12 = R20*R01 - R00*R21; + double R22 = R00*R11 - R10*R01; + + // Improve rotation matrix by applying polar decomposition. + if (1) { + // do polar decomposition. This makes the rotation matrix + // "proper", but probably increases the reprojection error. An + // iterative alignment step would be superior. + + matd_t *R = matd_create_data(3, 3, (double[]) { R00, R01, R02, + R10, R11, R12, + R20, R21, R22 }); + + matd_svd_t svd = matd_svd(R); + matd_destroy(R); + + R = matd_op("M*M'", svd.U, svd.V); + + matd_destroy(svd.U); + matd_destroy(svd.S); + matd_destroy(svd.V); + + R00 = MATD_EL(R, 0, 0); + R01 = MATD_EL(R, 0, 1); + R02 = MATD_EL(R, 0, 2); + R10 = MATD_EL(R, 1, 0); + R11 = MATD_EL(R, 1, 1); + R12 = MATD_EL(R, 1, 2); + R20 = MATD_EL(R, 2, 0); + R21 = MATD_EL(R, 2, 1); + R22 = MATD_EL(R, 2, 2); + + matd_destroy(R); + } + + return matd_create_data(4, 4, (double[]) { R00, R01, R02, TX, + R10, R11, R12, TY, + R20, R21, R22, TZ, + 0, 0, 0, 1 }); +} + +// Similar to above +// Recover the model view matrix assuming that the projection matrix is: +// +// [ F 0 A 0 ] (see glFrustrum) +// [ 0 G B 0 ] +// [ 0 0 C D ] +// [ 0 0 -1 0 ] + +matd_t *homography_to_model_view(const matd_t *H, double F, double G, double A, double B, double C, double D) +{ + // Note that every variable that we compute is proportional to the scale factor of H. + double R20 = -MATD_EL(H, 2, 0); + double R21 = -MATD_EL(H, 2, 1); + double TZ = -MATD_EL(H, 2, 2); + double R00 = (MATD_EL(H, 0, 0) - A*R20) / F; + double R01 = (MATD_EL(H, 0, 1) - A*R21) / F; + double TX = (MATD_EL(H, 0, 2) - A*TZ) / F; + double R10 = (MATD_EL(H, 1, 0) - B*R20) / G; + double R11 = (MATD_EL(H, 1, 1) - B*R21) / G; + double TY = (MATD_EL(H, 1, 2) - B*TZ) / G; + + // compute the scale by requiring that the rotation columns are unit length + // (Use geometric average of the two length vectors we have) + double length1 = sqrtf(R00*R00 + R10*R10 + R20*R20); + double length2 = sqrtf(R01*R01 + R11*R11 + R21*R21); + double s = 1.0 / sqrtf(length1 * length2); + + // get sign of S by requiring the tag to be in front of the camera + // (which is Z < 0) for our conventions. + if (TZ > 0) + s *= -1; + + R20 *= s; + R21 *= s; + TZ *= s; + R00 *= s; + R01 *= s; + TX *= s; + R10 *= s; + R11 *= s; + TY *= s; + + // now recover [R02 R12 R22] by noting that it is the cross product of the other two columns. + double R02 = R10*R21 - R20*R11; + double R12 = R20*R01 - R00*R21; + double R22 = R00*R11 - R10*R01; + + // TODO XXX: Improve rotation matrix by applying polar decomposition. + + return matd_create_data(4, 4, (double[]) { R00, R01, R02, TX, + R10, R11, R12, TY, + R20, R21, R22, TZ, + 0, 0, 0, 1 }); +} + +// Only uses the upper 3x3 matrix. +/* +static void matrix_to_quat(const matd_t *R, double q[4]) +{ + // see: "from quaternion to matrix and back" + + // trace: get the same result if R is 4x4 or 3x3: + double T = MATD_EL(R, 0, 0) + MATD_EL(R, 1, 1) + MATD_EL(R, 2, 2) + 1; + double S = 0; + + double m0 = MATD_EL(R, 0, 0); + double m1 = MATD_EL(R, 1, 0); + double m2 = MATD_EL(R, 2, 0); + double m4 = MATD_EL(R, 0, 1); + double m5 = MATD_EL(R, 1, 1); + double m6 = MATD_EL(R, 2, 1); + double m8 = MATD_EL(R, 0, 2); + double m9 = MATD_EL(R, 1, 2); + double m10 = MATD_EL(R, 2, 2); + + if (T > 0.0000001) { + S = sqrtf(T) * 2; + q[1] = -( m9 - m6 ) / S; + q[2] = -( m2 - m8 ) / S; + q[3] = -( m4 - m1 ) / S; + q[0] = 0.25 * S; + } else if ( m0 > m5 && m0 > m10 ) { // Column 0: + S = sqrtf( 1.0 + m0 - m5 - m10 ) * 2; + q[1] = -0.25 * S; + q[2] = -(m4 + m1 ) / S; + q[3] = -(m2 + m8 ) / S; + q[0] = (m9 - m6 ) / S; + } else if ( m5 > m10 ) { // Column 1: + S = sqrtf( 1.0 + m5 - m0 - m10 ) * 2; + q[1] = -(m4 + m1 ) / S; + q[2] = -0.25 * S; + q[3] = -(m9 + m6 ) / S; + q[0] = (m2 - m8 ) / S; + } else { + // Column 2: + S = sqrtf( 1.0 + m10 - m0 - m5 ) * 2; + q[1] = -(m2 + m8 ) / S; + q[2] = -(m9 + m6 ) / S; + q[3] = -0.25 * S; + q[0] = (m4 - m1 ) / S; + } + + double mag2 = 0; + for (int i = 0; i < 4; i++) + mag2 += q[i]*q[i]; + double norm = 1.0 / sqrtf(mag2); + for (int i = 0; i < 4; i++) + q[i] *= norm; +} +*/ + +// overwrites upper 3x3 area of matrix M. Doesn't touch any other elements of M. +void quat_to_matrix(const double q[4], matd_t *M) +{ + double w = q[0], x = q[1], y = q[2], z = q[3]; + + MATD_EL(M, 0, 0) = w*w + x*x - y*y - z*z; + MATD_EL(M, 0, 1) = 2*x*y - 2*w*z; + MATD_EL(M, 0, 2) = 2*x*z + 2*w*y; + + MATD_EL(M, 1, 0) = 2*x*y + 2*w*z; + MATD_EL(M, 1, 1) = w*w - x*x + y*y - z*z; + MATD_EL(M, 1, 2) = 2*y*z - 2*w*x; + + MATD_EL(M, 2, 0) = 2*x*z - 2*w*y; + MATD_EL(M, 2, 1) = 2*y*z + 2*w*x; + MATD_EL(M, 2, 2) = w*w - x*x - y*y + z*z; +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/common/image_u8.c b/apriltag/src/main/native/thirdparty/apriltag/src/common/image_u8.c new file mode 100644 index 0000000000..eef6c046ea --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/common/image_u8.c @@ -0,0 +1,557 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#include +#include +#include +#include +#include + +#include "common/image_u8.h" +#include "common/pnm.h" +#include "common/math_util.h" + +// least common multiple of 64 (sandy bridge cache line) and 24 (stride +// needed for RGB in 8-wide vector processing) +#define DEFAULT_ALIGNMENT_U8 96 + +image_u8_t *image_u8_create_stride(unsigned int width, unsigned int height, unsigned int stride) +{ + uint8_t *buf = calloc(height*stride, sizeof(uint8_t)); + + // const initializer + image_u8_t tmp = { .width = width, .height = height, .stride = stride, .buf = buf }; + + image_u8_t *im = calloc(1, sizeof(image_u8_t)); + memcpy(im, &tmp, sizeof(image_u8_t)); + return im; +} + +image_u8_t *image_u8_create(unsigned int width, unsigned int height) +{ + return image_u8_create_alignment(width, height, DEFAULT_ALIGNMENT_U8); +} + +image_u8_t *image_u8_create_alignment(unsigned int width, unsigned int height, unsigned int alignment) +{ + int stride = width; + + if ((stride % alignment) != 0) + stride += alignment - (stride % alignment); + + return image_u8_create_stride(width, height, stride); +} + +image_u8_t *image_u8_copy(const image_u8_t *in) +{ + uint8_t *buf = malloc(in->height*in->stride*sizeof(uint8_t)); + memcpy(buf, in->buf, in->height*in->stride*sizeof(uint8_t)); + + // const initializer + image_u8_t tmp = { .width = in->width, .height = in->height, .stride = in->stride, .buf = buf }; + + image_u8_t *copy = calloc(1, sizeof(image_u8_t)); + memcpy(copy, &tmp, sizeof(image_u8_t)); + return copy; +} + +void image_u8_destroy(image_u8_t *im) +{ + if (!im) + return; + + free(im->buf); + free(im); +} + +//////////////////////////////////////////////////////////// +// PNM file i/o +image_u8_t *image_u8_create_from_pnm(const char *path) +{ + return image_u8_create_from_pnm_alignment(path, DEFAULT_ALIGNMENT_U8); +} + +image_u8_t *image_u8_create_from_pnm_alignment(const char *path, int alignment) +{ + pnm_t *pnm = pnm_create_from_file(path); + if (pnm == NULL) + return NULL; + + image_u8_t *im = NULL; + + switch (pnm->format) { + case PNM_FORMAT_GRAY: { + im = image_u8_create_alignment(pnm->width, pnm->height, alignment); + + if (pnm->max == 255) { + for (int y = 0; y < im->height; y++) + memcpy(&im->buf[y*im->stride], &pnm->buf[y*im->width], im->width); + } else if (pnm->max == 65535) { + for (int y = 0; y < im->height; y++) + for (int x = 0; x < im->width; x++) + im->buf[y*im->stride + x] = pnm->buf[2*(y*im->width + x)]; + } else { + assert(0); + } + + break; + } + + case PNM_FORMAT_RGB: { + im = image_u8_create_alignment(pnm->width, pnm->height, alignment); + + if (pnm->max == 255) { + // Gray conversion for RGB is gray = (r + g + g + b)/4 + for (int y = 0; y < im->height; y++) { + for (int x = 0; x < im->width; x++) { + uint8_t gray = (pnm->buf[y*im->width*3 + 3*x+0] + // r + pnm->buf[y*im->width*3 + 3*x+1] + // g + pnm->buf[y*im->width*3 + 3*x+1] + // g + pnm->buf[y*im->width*3 + 3*x+2]) // b + / 4; + + im->buf[y*im->stride + x] = gray; + } + } + } else if (pnm->max == 65535) { + for (int y = 0; y < im->height; y++) { + for (int x = 0; x < im->width; x++) { + int r = pnm->buf[6*(y*im->width + x) + 0]; + int g = pnm->buf[6*(y*im->width + x) + 2]; + int b = pnm->buf[6*(y*im->width + x) + 4]; + + im->buf[y*im->stride + x] = (r + g + g + b) / 4; + } + } + } else { + assert(0); + } + + break; + } + + case PNM_FORMAT_BINARY: { + im = image_u8_create_alignment(pnm->width, pnm->height, alignment); + + // image is padded to be whole bytes on each row. + + // how many bytes per row on the input? + int pbmstride = (im->width + 7) / 8; + + for (int y = 0; y < im->height; y++) { + for (int x = 0; x < im->width; x++) { + int byteidx = y * pbmstride + x / 8; + int bitidx = 7 - (x & 7); + + // ack, black is one according to pbm docs! + if ((pnm->buf[byteidx] >> bitidx) & 1) + im->buf[y*im->stride + x] = 0; + else + im->buf[y*im->stride + x] = 255; + } + } + break; + } + } + + pnm_destroy(pnm); + return im; +} + +image_u8_t *image_u8_create_from_f32(image_f32_t *fim) +{ + image_u8_t *im = image_u8_create(fim->width, fim->height); + + for (int y = 0; y < fim->height; y++) { + for (int x = 0; x < fim->width; x++) { + float v = fim->buf[y*fim->stride + x]; + im->buf[y*im->stride + x] = (int) (255 * v); + } + } + + return im; +} + + +int image_u8_write_pnm(const image_u8_t *im, const char *path) +{ + FILE *f = fopen(path, "wb"); + int res = 0; + + if (f == NULL) { + res = -1; + goto finish; + } + + // Only outputs to grayscale + fprintf(f, "P5\n%d %d\n255\n", im->width, im->height); + + for (int y = 0; y < im->height; y++) { + if (im->width != fwrite(&im->buf[y*im->stride], 1, im->width, f)) { + res = -2; + goto finish; + } + } + + finish: + if (f != NULL) + fclose(f); + + return res; +} + +void image_u8_draw_circle(image_u8_t *im, float x0, float y0, float r, int v) +{ + r = r*r; + + for (int y = y0-r; y <= y0+r; y++) { + for (int x = x0-r; x <= x0+r; x++) { + float d = (x-x0)*(x-x0) + (y-y0)*(y-y0); + if (d > r) + continue; + + if (x >= 0 && x < im->width && y >= 0 && y < im->height) { + int idx = y*im->stride + x; + im->buf[idx] = v; + } + } + } +} + +void image_u8_draw_annulus(image_u8_t *im, float x0, float y0, float r0, float r1, int v) +{ + r0 = r0*r0; + r1 = r1*r1; + + assert(r0 < r1); + + for (int y = y0-r1; y <= y0+r1; y++) { + for (int x = x0-r1; x <= x0+r1; x++) { + float d = (x-x0)*(x-x0) + (y-y0)*(y-y0); + if (d < r0 || d > r1) + continue; + + int idx = y*im->stride + x; + im->buf[idx] = v; + } + } +} + +// only widths 1 and 3 supported (and 3 only badly) +void image_u8_draw_line(image_u8_t *im, float x0, float y0, float x1, float y1, int v, int width) +{ + double dist = sqrtf((y1-y0)*(y1-y0) + (x1-x0)*(x1-x0)); + double delta = 0.5 / dist; + + // terrible line drawing code + for (float f = 0; f <= 1; f += delta) { + int x = ((int) (x1 + (x0 - x1) * f)); + int y = ((int) (y1 + (y0 - y1) * f)); + + if (x < 0 || y < 0 || x >= im->width || y >= im->height) + continue; + + int idx = y*im->stride + x; + im->buf[idx] = v; + if (width > 1) { + im->buf[idx+1] = v; + im->buf[idx+im->stride] = v; + im->buf[idx+1+im->stride] = v; + } + } +} + +void image_u8_darken(image_u8_t *im) +{ + for (int y = 0; y < im->height; y++) { + for (int x = 0; x < im->width; x++) { + im->buf[im->stride*y+x] /= 2; + } + } +} + +static void convolve(const uint8_t *x, uint8_t *y, int sz, const uint8_t *k, int ksz) +{ + assert((ksz&1)==1); + + for (int i = 0; i < ksz/2 && i < sz; i++) + y[i] = x[i]; + + for (int i = 0; i < sz - ksz; i++) { + uint32_t acc = 0; + + for (int j = 0; j < ksz; j++) + acc += k[j]*x[i+j]; + + y[ksz/2 + i] = acc >> 8; + } + + for (int i = sz - ksz + ksz/2; i < sz; i++) + y[i] = x[i]; +} + +void image_u8_convolve_2D(image_u8_t *im, const uint8_t *k, int ksz) +{ + assert((ksz & 1) == 1); // ksz must be odd. + + for (int y = 0; y < im->height; y++) { + + uint8_t *x = malloc(sizeof(uint8_t)*im->stride); + memcpy(x, &im->buf[y*im->stride], im->stride); + + convolve(x, &im->buf[y*im->stride], im->width, k, ksz); + free(x); + } + + for (int x = 0; x < im->width; x++) { + uint8_t *xb = malloc(sizeof(uint8_t)*im->height); + uint8_t *yb = malloc(sizeof(uint8_t)*im->height); + + for (int y = 0; y < im->height; y++) + xb[y] = im->buf[y*im->stride + x]; + + convolve(xb, yb, im->height, k, ksz); + free(xb); + + for (int y = 0; y < im->height; y++) + im->buf[y*im->stride + x] = yb[y]; + free(yb); + } +} + +void image_u8_gaussian_blur(image_u8_t *im, double sigma, int ksz) +{ + if (sigma == 0) + return; + + assert((ksz & 1) == 1); // ksz must be odd. + + // build the kernel. + double *dk = malloc(sizeof(double)*ksz); + + // for kernel of length 5: + // dk[0] = f(-2), dk[1] = f(-1), dk[2] = f(0), dk[3] = f(1), dk[4] = f(2) + for (int i = 0; i < ksz; i++) { + int x = -ksz/2 + i; + double v = exp(-.5*sq(x / sigma)); + dk[i] = v; + } + + // normalize + double acc = 0; + for (int i = 0; i < ksz; i++) + acc += dk[i]; + + for (int i = 0; i < ksz; i++) + dk[i] /= acc; + + uint8_t *k = malloc(sizeof(uint8_t)*ksz); + for (int i = 0; i < ksz; i++) + k[i] = dk[i]*255; + + if (0) { + for (int i = 0; i < ksz; i++) + printf("%d %15f %5d\n", i, dk[i], k[i]); + } + free(dk); + + image_u8_convolve_2D(im, k, ksz); + free(k); +} + +image_u8_t *image_u8_rotate(const image_u8_t *in, double rad, uint8_t pad) +{ + int iwidth = in->width, iheight = in->height; + rad = -rad; // interpret y as being "down" + + float c = cos(rad), s = sin(rad); + + float p[][2] = { { 0, 0}, { iwidth, 0 }, { iwidth, iheight }, { 0, iheight} }; + + float xmin = HUGE_VALF, xmax = -HUGE_VALF, ymin = HUGE_VALF, ymax = -HUGE_VALF; + float icx = iwidth / 2.0, icy = iheight / 2.0; + + for (int i = 0; i < 4; i++) { + float px = p[i][0] - icx; + float py = p[i][1] - icy; + + float nx = px*c - py*s; + float ny = px*s + py*c; + + xmin = fmin(xmin, nx); + xmax = fmax(xmax, nx); + ymin = fmin(ymin, ny); + ymax = fmax(ymax, ny); + } + + int owidth = ceil(xmax-xmin), oheight = ceil(ymax - ymin); + image_u8_t *out = image_u8_create(owidth, oheight); + + // iterate over output pixels. + for (int oy = 0; oy < oheight; oy++) { + for (int ox = 0; ox < owidth; ox++) { + // work backwards from destination coordinates... + // sample pixel centers. + float sx = ox - owidth / 2.0 + .5; + float sy = oy - oheight / 2.0 + .5; + + // project into input-image space + int ix = floor(sx*c + sy*s + icx); + int iy = floor(-sx*s + sy*c + icy); + + if (ix >= 0 && iy >= 0 && ix < iwidth && iy < iheight) + out->buf[oy*out->stride+ox] = in->buf[iy*in->stride + ix]; + else + out->buf[oy*out->stride+ox] = pad; + } + } + + return out; +} + +image_u8_t *image_u8_decimate(image_u8_t *im, float ffactor) +{ + int width = im->width, height = im->height; + + if (ffactor == 1.5) { + int swidth = width / 3 * 2, sheight = height / 3 * 2; + + image_u8_t *decim = image_u8_create(swidth, sheight); + + int y = 0, sy = 0; + while (sy < sheight) { + int x = 0, sx = 0; + while (sx < swidth) { + + // a b c + // d e f + // g h i + uint8_t a = im->buf[(y+0)*im->stride + (x+0)]; + uint8_t b = im->buf[(y+0)*im->stride + (x+1)]; + uint8_t c = im->buf[(y+0)*im->stride + (x+2)]; + + uint8_t d = im->buf[(y+1)*im->stride + (x+0)]; + uint8_t e = im->buf[(y+1)*im->stride + (x+1)]; + uint8_t f = im->buf[(y+1)*im->stride + (x+2)]; + + uint8_t g = im->buf[(y+2)*im->stride + (x+0)]; + uint8_t h = im->buf[(y+2)*im->stride + (x+1)]; + uint8_t i = im->buf[(y+2)*im->stride + (x+2)]; + + decim->buf[(sy+0)*decim->stride + (sx + 0)] = + (4*a+2*b+2*d+e)/9; + decim->buf[(sy+0)*decim->stride + (sx + 1)] = + (4*c+2*b+2*f+e)/9; + + decim->buf[(sy+1)*decim->stride + (sx + 0)] = + (4*g+2*d+2*h+e)/9; + decim->buf[(sy+1)*decim->stride + (sx + 1)] = + (4*i+2*f+2*h+e)/9; + + x += 3; + sx += 2; + } + + y += 3; + sy += 2; + } + + return decim; + } + + int factor = (int) ffactor; + + int swidth = 1 + (width - 1)/factor; + int sheight = 1 + (height - 1)/factor; + image_u8_t *decim = image_u8_create(swidth, sheight); + int sy = 0; + for (int y = 0; y < height; y += factor) { + int sx = 0; + for (int x = 0; x < width; x += factor) { + decim->buf[sy*decim->stride + sx] = im->buf[y*im->stride + x]; + sx++; + } + sy++; + } + return decim; +} + +void image_u8_fill_line_max(image_u8_t *im, const image_u8_lut_t *lut, const float *xy0, const float *xy1) +{ + // what is the maximum distance that will result in drawing into our LUT? + float max_dist2 = (lut->nvalues-1)/lut->scale; + float max_dist = sqrt(max_dist2); + + // the orientation of the line + double theta = atan2(xy1[1]-xy0[1], xy1[0]-xy0[0]); + double v = sin(theta), u = cos(theta); + + int ix0 = iclamp(fmin(xy0[0], xy1[0]) - max_dist, 0, im->width-1); + int ix1 = iclamp(fmax(xy0[0], xy1[0]) + max_dist, 0, im->width-1); + + int iy0 = iclamp(fmin(xy0[1], xy1[1]) - max_dist, 0, im->height-1); + int iy1 = iclamp(fmax(xy0[1], xy1[1]) + max_dist, 0, im->height-1); + + // the line segment xy0---xy1 can be parameterized in terms of line coordinates. + // We fix xy0 to be at line coordinate 0. + float xy1_line_coord = (xy1[0]-xy0[0])*u + (xy1[1]-xy0[1])*v; + + float min_line_coord = fmin(0, xy1_line_coord); + float max_line_coord = fmax(0, xy1_line_coord); + + for (int iy = iy0; iy <= iy1; iy++) { + float y = iy+.5; + + for (int ix = ix0; ix <= ix1; ix++) { + float x = ix+.5; + + // compute line coordinate of this pixel. + float line_coord = (x - xy0[0])*u + (y - xy0[1])*v; + + // find point on line segment closest to our current pixel. + if (line_coord < min_line_coord) + line_coord = min_line_coord; + else if (line_coord > max_line_coord) + line_coord = max_line_coord; + + float px = xy0[0] + line_coord*u; + float py = xy0[1] + line_coord*v; + + double dist2 = (x-px)*(x-px) + (y-py)*(y-py); + + // not in our LUT? + int idx = dist2 * lut->scale; + if (idx >= lut->nvalues) + continue; + + uint8_t lut_value = lut->values[idx]; + uint8_t old_value = im->buf[iy*im->stride + ix]; + if (lut_value > old_value) + im->buf[iy*im->stride + ix] = lut_value; + } + } +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/common/image_u8x3.c b/apriltag/src/main/native/thirdparty/apriltag/src/common/image_u8x3.c new file mode 100644 index 0000000000..1463e5f1a7 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/common/image_u8x3.c @@ -0,0 +1,274 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#include +#include +#include +#include +#include + +#include "math_util.h" +#include "pnm.h" + +#include "image_u8x3.h" + +// least common multiple of 64 (sandy bridge cache line) and 48 (stride needed +// for 16byte-wide RGB processing). (It's possible that 48 would be enough). +#define DEFAULT_ALIGNMENT_U8X3 192 + +image_u8x3_t *image_u8x3_create(unsigned int width, unsigned int height) +{ + return image_u8x3_create_alignment(width, height, DEFAULT_ALIGNMENT_U8X3); +} + +image_u8x3_t *image_u8x3_create_alignment(unsigned int width, unsigned int height, unsigned int alignment) +{ + int stride = 3*width; + + if ((stride % alignment) != 0) + stride += alignment - (stride % alignment); + + uint8_t *buf = calloc(height*stride, sizeof(uint8_t)); + + // const initializer + image_u8x3_t tmp = { .width = width, .height = height, .stride = stride, .buf = buf }; + + image_u8x3_t *im = calloc(1, sizeof(image_u8x3_t)); + memcpy(im, &tmp, sizeof(image_u8x3_t)); + return im; +} + +image_u8x3_t *image_u8x3_copy(const image_u8x3_t *in) +{ + uint8_t *buf = malloc(in->height*in->stride*sizeof(uint8_t)); + memcpy(buf, in->buf, in->height*in->stride*sizeof(uint8_t)); + + // const initializer + image_u8x3_t tmp = { .width = in->width, .height = in->height, .stride = in->stride, .buf = buf }; + + image_u8x3_t *copy = calloc(1, sizeof(image_u8x3_t)); + memcpy(copy, &tmp, sizeof(image_u8x3_t)); + return copy; +} + +void image_u8x3_destroy(image_u8x3_t *im) +{ + if (!im) + return; + + free(im->buf); + free(im); +} + +//////////////////////////////////////////////////////////// +// PNM file i/o + +// Create an RGB image from PNM +image_u8x3_t *image_u8x3_create_from_pnm(const char *path) +{ + pnm_t *pnm = pnm_create_from_file(path); + if (pnm == NULL) + return NULL; + + image_u8x3_t *im = NULL; + + switch (pnm->format) { + case PNM_FORMAT_GRAY: { + im = image_u8x3_create(pnm->width, pnm->height); + + for (int y = 0; y < im->height; y++) { + for (int x = 0; x < im->width; x++) { + uint8_t gray = pnm->buf[y*im->width + x]; + im->buf[y*im->stride + x*3 + 0] = gray; + im->buf[y*im->stride + x*3 + 1] = gray; + im->buf[y*im->stride + x*3 + 2] = gray; + } + } + + break; + } + + case PNM_FORMAT_RGB: { + im = image_u8x3_create(pnm->width, pnm->height); + + for (int y = 0; y < im->height; y++) { + for (int x = 0; x < im->width; x++) { + uint8_t r = pnm->buf[y*im->width*3 + 3*x]; + uint8_t g = pnm->buf[y*im->width*3 + 3*x+1]; + uint8_t b = pnm->buf[y*im->width*3 + 3*x+2]; + + im->buf[y*im->stride + x*3 + 0] = r; + im->buf[y*im->stride + x*3 + 1] = g; + im->buf[y*im->stride + x*3 + 2] = b; + } + } + + break; + } + } + + pnm_destroy(pnm); + return im; +} + +int image_u8x3_write_pnm(const image_u8x3_t *im, const char *path) +{ + FILE *f = fopen(path, "wb"); + int res = 0; + + if (f == NULL) { + res = -1; + goto finish; + } + + // Only outputs to RGB + fprintf(f, "P6\n%d %d\n255\n", im->width, im->height); + int linesz = im->width * 3; + for (int y = 0; y < im->height; y++) { + if (linesz != fwrite(&im->buf[y*im->stride], 1, linesz, f)) { + res = -1; + goto finish; + } + } + +finish: + if (f != NULL) + fclose(f); + + return res; +} + +// only width 1 supported +void image_u8x3_draw_line(image_u8x3_t *im, float x0, float y0, float x1, float y1, uint8_t rgb[3], int width) +{ + double dist = sqrtf((y1-y0)*(y1-y0) + (x1-x0)*(x1-x0)); + double delta = 0.5 / dist; + + // terrible line drawing code + for (float f = 0; f <= 1; f += delta) { + int x = ((int) (x1 + (x0 - x1) * f)); + int y = ((int) (y1 + (y0 - y1) * f)); + + if (x < 0 || y < 0 || x >= im->width || y >= im->height) + continue; + + int idx = y*im->stride + 3*x; + for (int i = 0; i < 3; i++) + im->buf[idx + i] = rgb[i]; + } +} + +static void convolve(const uint8_t *x, uint8_t *y, int sz, const uint8_t *k, int ksz) +{ + assert((ksz&1)==1); + + for (int i = 0; i < ksz/2 && i < sz; i++) + y[i] = x[i]; + + for (int i = 0; i < sz - ksz; i++) { + uint32_t acc = 0; + + for (int j = 0; j < ksz; j++) + acc += k[j]*x[i+j]; + + y[ksz/2 + i] = acc >> 8; + } + + for (int i = sz - ksz + ksz/2; i < sz; i++) + y[i] = x[i]; +} + +void image_u8x3_gaussian_blur(image_u8x3_t *im, double sigma, int ksz) +{ + if (sigma == 0) + return; + + assert((ksz & 1) == 1); // ksz must be odd. + + // build the kernel. + double *dk = malloc(sizeof(double)*ksz); + + // for kernel of length 5: + // dk[0] = f(-2), dk[1] = f(-1), dk[2] = f(0), dk[3] = f(1), dk[4] = f(2) + for (int i = 0; i < ksz; i++) { + int x = -ksz/2 + i; + double v = exp(-.5*sq(x / sigma)); + dk[i] = v; + } + + // normalize + double acc = 0; + for (int i = 0; i < ksz; i++) + acc += dk[i]; + + for (int i = 0; i < ksz; i++) + dk[i] /= acc; + + uint8_t *k = malloc(sizeof(uint8_t)*ksz); + for (int i = 0; i < ksz; i++) + k[i] = dk[i]*255; + + if (0) { + for (int i = 0; i < ksz; i++) + printf("%d %15f %5d\n", i, dk[i], k[i]); + } + free(dk); + + for (int c = 0; c < 3; c++) { + for (int y = 0; y < im->height; y++) { + + uint8_t *in = malloc(sizeof(uint8_t)*im->stride); + uint8_t *out = malloc(sizeof(uint8_t)*im->stride); + + for (int x = 0; x < im->width; x++) + in[x] = im->buf[y*im->stride + 3 * x + c]; + + convolve(in, out, im->width, k, ksz); + free(in); + + for (int x = 0; x < im->width; x++) + im->buf[y*im->stride + 3 * x + c] = out[x]; + free(out); + } + + for (int x = 0; x < im->width; x++) { + uint8_t *in = malloc(sizeof(uint8_t)*im->height); + uint8_t *out = malloc(sizeof(uint8_t)*im->height); + + for (int y = 0; y < im->height; y++) + in[y] = im->buf[y*im->stride + 3*x + c]; + + convolve(in, out, im->height, k, ksz); + free(in); + + for (int y = 0; y < im->height; y++) + im->buf[y*im->stride + 3*x + c] = out[y]; + free(out); + } + } + free(k); +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/common/image_u8x4.c b/apriltag/src/main/native/thirdparty/apriltag/src/common/image_u8x4.c new file mode 100644 index 0000000000..3520537447 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/common/image_u8x4.c @@ -0,0 +1,233 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#include +#include +#include +#include + +#include "pam.h" +#include "pnm.h" +#include "image_u8x4.h" + +// least common multiple of 64 (sandy bridge cache line) and 64 (stride needed +// for 16byte-wide RGBA processing). +#define DEFAULT_ALIGNMENT_U8X4 64 + +image_u8x4_t *image_u8x4_create(unsigned int width, unsigned int height) +{ + return image_u8x4_create_alignment(width, height, DEFAULT_ALIGNMENT_U8X4); +} + +image_u8x4_t *image_u8x4_create_alignment(unsigned int width, unsigned int height, unsigned int alignment) +{ + int stride = 4*width; + + if ((stride % alignment) != 0) + stride += alignment - (stride % alignment); + + uint8_t *buf = calloc(height*stride, sizeof(uint8_t)); + + // const initializer + image_u8x4_t tmp = { .width = width, .height = height, .stride = stride, .buf = buf }; + + image_u8x4_t *im = calloc(1, sizeof(image_u8x4_t)); + memcpy(im, &tmp, sizeof(image_u8x4_t)); + return im; +} + +image_u8x4_t *image_u8x4_copy(const image_u8x4_t *in) +{ + uint8_t *buf = malloc(in->height*in->stride*sizeof(uint8_t)); + memcpy(buf, in->buf, in->height*in->stride*sizeof(uint8_t)); + + // const initializer + image_u8x4_t tmp = { .width = in->width, .height = in->height, .stride = in->stride, .buf = buf }; + + image_u8x4_t *copy = calloc(1, sizeof(image_u8x4_t)); + memcpy(copy, &tmp, sizeof(image_u8x4_t)); + return copy; +} + +void image_u8x4_destroy(image_u8x4_t *im) +{ + if (!im) + return; + + free(im->buf); + free(im); +} + +//////////////////////////////////////////////////////////// +image_u8x4_t *image_u8x4_create_from_pam(const char *inpath) +{ + pam_t *pam = pam_create_from_file(inpath); + if (!pam) + return NULL; + + image_u8x4_t *im = image_u8x4_create(pam->width, pam->height); + + for (int y = 0; y < pam->height; y++) { + if (pam->depth == 1) { + for (int x = 0; x < pam->width; x++) { + im->buf[y*im->stride + 4*x + 0] = pam->data[pam->width*y + x + 0]; + im->buf[y*im->stride + 4*x + 1] = pam->data[pam->width*y + x + 0]; + im->buf[y*im->stride + 4*x + 2] = pam->data[pam->width*y + x + 0]; + im->buf[y*im->stride + 4*x + 3] = 255; + } + } else if (pam->depth == 3) { + for (int x = 0; x < pam->width; x++) { + im->buf[y*im->stride + 4*x + 0] = pam->data[3*pam->width*y + 3*x + 0]; + im->buf[y*im->stride + 4*x + 1] = pam->data[3*pam->width*y + 3*x + 1]; + im->buf[y*im->stride + 4*x + 2] = pam->data[3*pam->width*y + 3*x + 2]; + im->buf[y*im->stride + 4*x + 3] = 255; + } + } else if (pam->depth == 4) { + memcpy(&im->buf[y*im->stride], &pam->data[4*pam->width*y], 4*pam->width); + } else { + assert(0); // not implemented + } + } + + pam_destroy(pam); + return im; +} +//////////////////////////////////////////////////////////// +// PNM file i/o + +// Create an RGBA image from PNM +image_u8x4_t *image_u8x4_create_from_pnm(const char *path) +{ + pnm_t *pnmp = pnm_create_from_file(path); + if (pnmp == NULL) + return NULL; + + pnm_t pnm = *pnmp; + image_u8x4_t *imp = NULL; + + switch (pnm.format) { + case PNM_FORMAT_GRAY: { + imp = image_u8x4_create(pnm.width, pnm.height); + + // copy struct by value for common subexpression elimination + const image_u8x4_t im = *imp; + + for (int y = 0; y < im.height; y++) { + for (int x = 0; x < im.width; x++) { + uint8_t gray = pnm.buf[y*pnm.width + x]; + im.buf[y*im.stride + 4*x + 0] = gray; + im.buf[y*im.stride + 4*x + 1] = gray; + im.buf[y*im.stride + 4*x + 2] = gray; + im.buf[y*im.stride + 4*x + 3] = 0xff; + } + } + + break; + } + + case PNM_FORMAT_RGB: { + imp = image_u8x4_create(pnm.width, pnm.height); + + // copy struct by value for common subexpression elimination + const image_u8x4_t im = *imp; + + // Gray conversion for RGB is gray = (r + g + g + b)/4 + for (int y = 0; y < im.height; y++) { + for (int x = 0; x < im.width; x++) { + + uint8_t r = pnm.buf[y*pnm.width*3 + 3*x + 0]; + uint8_t g = pnm.buf[y*pnm.width*3 + 3*x + 1]; + uint8_t b = pnm.buf[y*pnm.width*3 + 3*x + 2]; + + im.buf[y*im.stride + 4*x + 0] = r; + im.buf[y*im.stride + 4*x + 1] = g; + im.buf[y*im.stride + 4*x + 2] = b; + im.buf[y*im.stride + 4*x + 3] = 0xff; + } + } + + break; + } + } + + pnm_destroy(pnmp); + return imp; +} + +int image_u8x4_write_pnm(const image_u8x4_t *imp, const char *path) +{ + // copy struct by value to ensure common subexpression elimination occurs + const image_u8x4_t im = *imp; + + FILE *f = fopen(path, "wb"); + int res = 0; + + if (f == NULL) { + res = -1; + goto finish; + } + + // Only outputs to RGB + fprintf(f, "P6\n%d %d\n255\n", im.width, im.height); + + for (int y = im.height-1; y >= 0; y--) { + for (int x = 0; x < im.width; x++) { + + uint8_t r = im.buf[y*im.stride + 4*x + 0]; + uint8_t g = im.buf[y*im.stride + 4*x + 1]; + uint8_t b = im.buf[y*im.stride + 4*x + 2]; + + fwrite(&r, 1, 1, f); + fwrite(&g, 1, 1, f); + fwrite(&b, 1, 1, f); + } + } + + finish: + if (f != NULL) + fclose(f); + + return res; +} + +void image_u8x4_write_pam(const image_u8x4_t *im, const char *path) +{ + FILE *f = fopen(path, "w"); + fprintf(f, "P7\n"); + fprintf(f, "WIDTH %d\n", im->width); + fprintf(f, "HEIGHT %d\n", im->height); + fprintf(f, "DEPTH 4\n"); + fprintf(f, "MAXVAL 255\n"); + fprintf(f, "TUPLTYPE RGB_ALPHA\n"); + fprintf(f, "ENDHDR\n"); + + for (int y = 0; y < im->height; y++) + fwrite(&im->buf[y*im->stride], 1, 4*im->width, f); + + fclose(f); + +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/common/matd.c b/apriltag/src/main/native/thirdparty/apriltag/src/common/matd.c new file mode 100644 index 0000000000..f426890416 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/common/matd.c @@ -0,0 +1,2029 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#include +#include +#include +#include +#include +#include +#include + +#include "common/math_util.h" +#include "common/svd22.h" +#include "common/matd.h" +#include "common/debug_print.h" + +// a matd_t with rows=0 cols=0 is a SCALAR. + +// to ease creating mati, matf, etc. in the future. +#define TYPE double + +matd_t *matd_create(int rows, int cols) +{ + assert(rows >= 0); + assert(cols >= 0); + + if (rows == 0 || cols == 0) + return matd_create_scalar(0); + + matd_t *m = calloc(1, sizeof(matd_t) + (rows*cols*sizeof(double))); + m->nrows = rows; + m->ncols = cols; + + return m; +} + +matd_t *matd_create_scalar(TYPE v) +{ + matd_t *m = calloc(1, sizeof(matd_t) + sizeof(double)); + m->nrows = 0; + m->ncols = 0; + m->data[0] = v; + + return m; +} + +matd_t *matd_create_data(int rows, int cols, const TYPE *data) +{ + if (rows == 0 || cols == 0) + return matd_create_scalar(data[0]); + + matd_t *m = matd_create(rows, cols); + for (int i = 0; i < rows * cols; i++) + m->data[i] = data[i]; + + return m; +} + +matd_t *matd_create_dataf(int rows, int cols, const float *data) +{ + if (rows == 0 || cols == 0) + return matd_create_scalar(data[0]); + + matd_t *m = matd_create(rows, cols); + for (int i = 0; i < rows * cols; i++) + m->data[i] = (double)data[i]; + + return m; +} + +matd_t *matd_identity(int dim) +{ + if (dim == 0) + return matd_create_scalar(1); + + matd_t *m = matd_create(dim, dim); + for (int i = 0; i < dim; i++) + MATD_EL(m, i, i) = 1; + + return m; +} + +// row and col are zero-based +TYPE matd_get(const matd_t *m, int row, int col) +{ + assert(m != NULL); + assert(!matd_is_scalar(m)); + assert(row >= 0); + assert(row < m->nrows); + assert(col >= 0); + assert(col < m->ncols); + + return MATD_EL(m, row, col); +} + +// row and col are zero-based +void matd_put(matd_t *m, int row, int col, TYPE value) +{ + assert(m != NULL); + + if (matd_is_scalar(m)) { + matd_put_scalar(m, value); + return; + } + + assert(row >= 0); + assert(row < m->nrows); + assert(col >= 0); + assert(col < m->ncols); + + MATD_EL(m, row, col) = value; +} + +TYPE matd_get_scalar(const matd_t *m) +{ + assert(m != NULL); + assert(matd_is_scalar(m)); + + return (m->data[0]); +} + +void matd_put_scalar(matd_t *m, TYPE value) +{ + assert(m != NULL); + assert(matd_is_scalar(m)); + + m->data[0] = value; +} + +matd_t *matd_copy(const matd_t *m) +{ + assert(m != NULL); + + matd_t *x = matd_create(m->nrows, m->ncols); + if (matd_is_scalar(m)) + x->data[0] = m->data[0]; + else + memcpy(x->data, m->data, sizeof(TYPE)*m->ncols*m->nrows); + + return x; +} + +matd_t *matd_select(const matd_t * a, int r0, int r1, int c0, int c1) +{ + assert(a != NULL); + + assert(r0 >= 0 && r0 < a->nrows); + assert(c0 >= 0 && c0 < a->ncols); + + int nrows = r1 - r0 + 1; + int ncols = c1 - c0 + 1; + + matd_t * r = matd_create(nrows, ncols); + + for (int row = r0; row <= r1; row++) + for (int col = c0; col <= c1; col++) + MATD_EL(r,row-r0,col-c0) = MATD_EL(a,row,col); + + return r; +} + +void matd_print(const matd_t *m, const char *fmt) +{ + assert(m != NULL); + assert(fmt != NULL); + + if (matd_is_scalar(m)) { + printf(fmt, MATD_EL(m, 0, 0)); + printf("\n"); + } else { + for (int i = 0; i < m->nrows; i++) { + for (int j = 0; j < m->ncols; j++) { + printf(fmt, MATD_EL(m, i, j)); + } + printf("\n"); + } + } +} + +void matd_print_transpose(const matd_t *m, const char *fmt) +{ + assert(m != NULL); + assert(fmt != NULL); + + if (matd_is_scalar(m)) { + printf(fmt, MATD_EL(m, 0, 0)); + printf("\n"); + } else { + for (int j = 0; j < m->ncols; j++) { + for (int i = 0; i < m->nrows; i++) { + printf(fmt, MATD_EL(m, i, j)); + } + printf("\n"); + } + } +} + +void matd_destroy(matd_t *m) +{ + if (!m) + return; + + assert(m != NULL); + free(m); +} + +matd_t *matd_multiply(const matd_t *a, const matd_t *b) +{ + assert(a != NULL); + assert(b != NULL); + + if (matd_is_scalar(a)) + return matd_scale(b, a->data[0]); + if (matd_is_scalar(b)) + return matd_scale(a, b->data[0]); + + assert(a->ncols == b->nrows); + matd_t *m = matd_create(a->nrows, b->ncols); + + for (int i = 0; i < m->nrows; i++) { + for (int j = 0; j < m->ncols; j++) { + TYPE acc = 0; + for (int k = 0; k < a->ncols; k++) { + acc += MATD_EL(a, i, k) * MATD_EL(b, k, j); + } + MATD_EL(m, i, j) = acc; + } + } + + return m; +} + +matd_t *matd_scale(const matd_t *a, double s) +{ + assert(a != NULL); + + if (matd_is_scalar(a)) + return matd_create_scalar(a->data[0] * s); + + matd_t *m = matd_create(a->nrows, a->ncols); + + for (int i = 0; i < m->nrows; i++) { + for (int j = 0; j < m->ncols; j++) { + MATD_EL(m, i, j) = s * MATD_EL(a, i, j); + } + } + + return m; +} + +void matd_scale_inplace(matd_t *a, double s) +{ + assert(a != NULL); + + if (matd_is_scalar(a)) { + a->data[0] *= s; + return; + } + + for (int i = 0; i < a->nrows; i++) { + for (int j = 0; j < a->ncols; j++) { + MATD_EL(a, i, j) *= s; + } + } +} + +matd_t *matd_add(const matd_t *a, const matd_t *b) +{ + assert(a != NULL); + assert(b != NULL); + assert(a->nrows == b->nrows); + assert(a->ncols == b->ncols); + + if (matd_is_scalar(a)) + return matd_create_scalar(a->data[0] + b->data[0]); + + matd_t *m = matd_create(a->nrows, a->ncols); + + for (int i = 0; i < m->nrows; i++) { + for (int j = 0; j < m->ncols; j++) { + MATD_EL(m, i, j) = MATD_EL(a, i, j) + MATD_EL(b, i, j); + } + } + + return m; +} + +void matd_add_inplace(matd_t *a, const matd_t *b) +{ + assert(a != NULL); + assert(b != NULL); + assert(a->nrows == b->nrows); + assert(a->ncols == b->ncols); + + if (matd_is_scalar(a)) { + a->data[0] += b->data[0]; + return; + } + + for (int i = 0; i < a->nrows; i++) { + for (int j = 0; j < a->ncols; j++) { + MATD_EL(a, i, j) += MATD_EL(b, i, j); + } + } +} + + +matd_t *matd_subtract(const matd_t *a, const matd_t *b) +{ + assert(a != NULL); + assert(b != NULL); + assert(a->nrows == b->nrows); + assert(a->ncols == b->ncols); + + if (matd_is_scalar(a)) + return matd_create_scalar(a->data[0] - b->data[0]); + + matd_t *m = matd_create(a->nrows, a->ncols); + + for (int i = 0; i < m->nrows; i++) { + for (int j = 0; j < m->ncols; j++) { + MATD_EL(m, i, j) = MATD_EL(a, i, j) - MATD_EL(b, i, j); + } + } + + return m; +} + +void matd_subtract_inplace(matd_t *a, const matd_t *b) +{ + assert(a != NULL); + assert(b != NULL); + assert(a->nrows == b->nrows); + assert(a->ncols == b->ncols); + + if (matd_is_scalar(a)) { + a->data[0] -= b->data[0]; + return; + } + + for (int i = 0; i < a->nrows; i++) { + for (int j = 0; j < a->ncols; j++) { + MATD_EL(a, i, j) -= MATD_EL(b, i, j); + } + } +} + + +matd_t *matd_transpose(const matd_t *a) +{ + assert(a != NULL); + + if (matd_is_scalar(a)) + return matd_create_scalar(a->data[0]); + + matd_t *m = matd_create(a->ncols, a->nrows); + + for (int i = 0; i < a->nrows; i++) { + for (int j = 0; j < a->ncols; j++) { + MATD_EL(m, j, i) = MATD_EL(a, i, j); + } + } + return m; +} + +static +double matd_det_general(const matd_t *a) +{ + // Use LU decompositon to calculate the determinant + matd_plu_t *mlu = matd_plu(a); + matd_t *L = matd_plu_l(mlu); + matd_t *U = matd_plu_u(mlu); + + // The determinants of the L and U matrices are the products of + // their respective diagonal elements + double detL = 1; double detU = 1; + for (int i = 0; i < a->nrows; i++) { + detL *= matd_get(L, i, i); + detU *= matd_get(U, i, i); + } + + // The determinant of a can be calculated as + // epsilon*det(L)*det(U), + // where epsilon is just the sign of the corresponding permutation + // (which is +1 for an even number of permutations and is −1 + // for an uneven number of permutations). + double det = mlu->pivsign * detL * detU; + + // Cleanup + matd_plu_destroy(mlu); + matd_destroy(L); + matd_destroy(U); + + return det; +} + +double matd_det(const matd_t *a) +{ + assert(a != NULL); + assert(a->nrows == a->ncols); + + switch(a->nrows) { + case 0: + // scalar: invalid + assert(a->nrows > 0); + break; + + case 1: + // 1x1 matrix + return a->data[0]; + + case 2: + // 2x2 matrix + return a->data[0] * a->data[3] - a->data[1] * a->data[2]; + + case 3: + // 3x3 matrix + return a->data[0]*a->data[4]*a->data[8] + - a->data[0]*a->data[5]*a->data[7] + + a->data[1]*a->data[5]*a->data[6] + - a->data[1]*a->data[3]*a->data[8] + + a->data[2]*a->data[3]*a->data[7] + - a->data[2]*a->data[4]*a->data[6]; + + case 4: { + // 4x4 matrix + double m00 = MATD_EL(a,0,0), m01 = MATD_EL(a,0,1), m02 = MATD_EL(a,0,2), m03 = MATD_EL(a,0,3); + double m10 = MATD_EL(a,1,0), m11 = MATD_EL(a,1,1), m12 = MATD_EL(a,1,2), m13 = MATD_EL(a,1,3); + double m20 = MATD_EL(a,2,0), m21 = MATD_EL(a,2,1), m22 = MATD_EL(a,2,2), m23 = MATD_EL(a,2,3); + double m30 = MATD_EL(a,3,0), m31 = MATD_EL(a,3,1), m32 = MATD_EL(a,3,2), m33 = MATD_EL(a,3,3); + + return m00 * m11 * m22 * m33 - m00 * m11 * m23 * m32 - + m00 * m21 * m12 * m33 + m00 * m21 * m13 * m32 + m00 * m31 * m12 * m23 - + m00 * m31 * m13 * m22 - m10 * m01 * m22 * m33 + + m10 * m01 * m23 * m32 + m10 * m21 * m02 * m33 - + m10 * m21 * m03 * m32 - m10 * m31 * m02 * m23 + + m10 * m31 * m03 * m22 + m20 * m01 * m12 * m33 - + m20 * m01 * m13 * m32 - m20 * m11 * m02 * m33 + + m20 * m11 * m03 * m32 + m20 * m31 * m02 * m13 - + m20 * m31 * m03 * m12 - m30 * m01 * m12 * m23 + + m30 * m01 * m13 * m22 + m30 * m11 * m02 * m23 - + m30 * m11 * m03 * m22 - m30 * m21 * m02 * m13 + + m30 * m21 * m03 * m12; + } + + default: + return matd_det_general(a); + } + + assert(0); + return 0; +} + +// returns NULL if the matrix is (exactly) singular. Caller is +// otherwise responsible for knowing how to cope with badly +// conditioned matrices. +matd_t *matd_inverse(const matd_t *x) +{ + matd_t *m = NULL; + + assert(x != NULL); + assert(x->nrows == x->ncols); + + if (matd_is_scalar(x)) { + if (x->data[0] == 0) + return NULL; + + return matd_create_scalar(1.0 / x->data[0]); + } + + switch(x->nrows) { + case 1: { + double det = x->data[0]; + if (det == 0) + return NULL; + + double invdet = 1.0 / det; + + m = matd_create(x->nrows, x->nrows); + MATD_EL(m, 0, 0) = 1.0 * invdet; + return m; + } + + case 2: { + double det = x->data[0] * x->data[3] - x->data[1] * x->data[2]; + if (det == 0) + return NULL; + + double invdet = 1.0 / det; + + m = matd_create(x->nrows, x->nrows); + MATD_EL(m, 0, 0) = MATD_EL(x, 1, 1) * invdet; + MATD_EL(m, 0, 1) = - MATD_EL(x, 0, 1) * invdet; + MATD_EL(m, 1, 0) = - MATD_EL(x, 1, 0) * invdet; + MATD_EL(m, 1, 1) = MATD_EL(x, 0, 0) * invdet; + return m; + } + + default: { + matd_plu_t *plu = matd_plu(x); + + matd_t *inv = NULL; + if (!plu->singular) { + matd_t *ident = matd_identity(x->nrows); + inv = matd_plu_solve(plu, ident); + matd_destroy(ident); + } + + matd_plu_destroy(plu); + + return inv; + } + } + + return NULL; // unreachable +} + + + +// TODO Optimization: Some operations we could perform in-place, +// saving some memory allocation work. E.g., ADD, SUBTRACT. Just need +// to make sure that we don't do an in-place modification on a matrix +// that was an input argument! + +// handle right-associative operators, greedily consuming them. These +// include transpose and inverse. This is called by the main recursion +// method. +static inline matd_t *matd_op_gobble_right(const char *expr, int *pos, matd_t *acc, matd_t **garb, int *garbpos) +{ + while (expr[*pos] != 0) { + + switch (expr[*pos]) { + + case '\'': { + assert(acc != NULL); // either a syntax error or a math op failed, producing null + matd_t *res = matd_transpose(acc); + garb[*garbpos] = res; + (*garbpos)++; + acc = res; + + (*pos)++; + break; + } + + // handle inverse ^-1. No other exponents are allowed. + case '^': { + assert(acc != NULL); + assert(expr[*pos+1] == '-'); + assert(expr[*pos+2] == '1'); + + matd_t *res = matd_inverse(acc); + garb[*garbpos] = res; + (*garbpos)++; + acc = res; + + (*pos)+=3; + break; + } + + default: + return acc; + } + } + + return acc; +} + +// @garb, garbpos A list of every matrix allocated during evaluation... used to assist cleanup. +// @oneterm: we should return at the end of this term (i.e., stop at a PLUS, MINUS, LPAREN). +static matd_t *matd_op_recurse(const char *expr, int *pos, matd_t *acc, matd_t **args, int *argpos, + matd_t **garb, int *garbpos, int oneterm) +{ + while (expr[*pos] != 0) { + + switch (expr[*pos]) { + + case '(': { + if (oneterm && acc != NULL) + return acc; + (*pos)++; + matd_t *rhs = matd_op_recurse(expr, pos, NULL, args, argpos, garb, garbpos, 0); + rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos); + + if (acc == NULL) { + acc = rhs; + } else { + matd_t *res = matd_multiply(acc, rhs); + garb[*garbpos] = res; + (*garbpos)++; + acc = res; + } + + break; + } + + case ')': { + if (oneterm) + return acc; + + (*pos)++; + return acc; + } + + case '*': { + (*pos)++; + + matd_t *rhs = matd_op_recurse(expr, pos, NULL, args, argpos, garb, garbpos, 1); + rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos); + + if (acc == NULL) { + acc = rhs; + } else { + matd_t *res = matd_multiply(acc, rhs); + garb[*garbpos] = res; + (*garbpos)++; + acc = res; + } + + break; + } + + case 'F': { + matd_t *rhs = args[*argpos]; + garb[*garbpos] = rhs; + (*garbpos)++; + + (*pos)++; + (*argpos)++; + + rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos); + + if (acc == NULL) { + acc = rhs; + } else { + matd_t *res = matd_multiply(acc, rhs); + garb[*garbpos] = res; + (*garbpos)++; + acc = res; + } + + break; + } + + case 'M': { + matd_t *rhs = args[*argpos]; + + (*pos)++; + (*argpos)++; + + rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos); + + if (acc == NULL) { + acc = rhs; + } else { + matd_t *res = matd_multiply(acc, rhs); + garb[*garbpos] = res; + (*garbpos)++; + acc = res; + } + + break; + } + +/* + case 'D': { + int rows = expr[*pos+1]-'0'; + int cols = expr[*pos+2]-'0'; + + matd_t *rhs = matd_create(rows, cols); + + break; + } +*/ + // a constant (SCALAR) defined inline. Treat just like M, creating a matd_t on the fly. + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + case '.': { + const char *start = &expr[*pos]; + char *end; + double s = strtod(start, &end); + (*pos) += (end - start); + matd_t *rhs = matd_create_scalar(s); + garb[*garbpos] = rhs; + (*garbpos)++; + + rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos); + + if (acc == NULL) { + acc = rhs; + } else { + matd_t *res = matd_multiply(acc, rhs); + garb[*garbpos] = res; + (*garbpos)++; + acc = res; + } + + break; + } + + case '+': { + if (oneterm && acc != NULL) + return acc; + + // don't support unary plus + assert(acc != NULL); + (*pos)++; + matd_t *rhs = matd_op_recurse(expr, pos, NULL, args, argpos, garb, garbpos, 1); + rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos); + + matd_t *res = matd_add(acc, rhs); + + garb[*garbpos] = res; + (*garbpos)++; + acc = res; + break; + } + + case '-': { + if (oneterm && acc != NULL) + return acc; + + if (acc == NULL) { + // unary minus + (*pos)++; + matd_t *rhs = matd_op_recurse(expr, pos, NULL, args, argpos, garb, garbpos, 1); + rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos); + + matd_t *res = matd_scale(rhs, -1); + garb[*garbpos] = res; + (*garbpos)++; + acc = res; + } else { + // subtract + (*pos)++; + matd_t *rhs = matd_op_recurse(expr, pos, NULL, args, argpos, garb, garbpos, 1); + rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos); + + matd_t *res = matd_subtract(acc, rhs); + garb[*garbpos] = res; + (*garbpos)++; + acc = res; + } + break; + } + + case ' ': { + // nothing to do. spaces are meaningless. + (*pos)++; + break; + } + + default: { + debug_print("Unknown character: '%c'\n", expr[*pos]); + assert(expr[*pos] != expr[*pos]); + } + } + } + return acc; +} + +// always returns a new matrix. +matd_t *matd_op(const char *expr, ...) +{ + int nargs = 0; + int exprlen = 0; + + assert(expr != NULL); + + for (const char *p = expr; *p != 0; p++) { + if (*p == 'M' || *p == 'F') + nargs++; + exprlen++; + } + + assert(nargs > 0); + + if (!exprlen) // expr = "" + return NULL; + + va_list ap; + va_start(ap, expr); + + matd_t **args = malloc(sizeof(matd_t*)*nargs); + for (int i = 0; i < nargs; i++) { + args[i] = va_arg(ap, matd_t*); + // XXX: sanity check argument; emit warning/error if args[i] + // doesn't look like a matd_t*. + } + + va_end(ap); + + int pos = 0; + int argpos = 0; + int garbpos = 0; + + // can't create more than 2 new result per character + // one result, and possibly one argument to free + matd_t **garb = malloc(sizeof(matd_t*)*2*exprlen); + + matd_t *res = matd_op_recurse(expr, &pos, NULL, args, &argpos, garb, &garbpos, 0); + free(args); + + // 'res' may need to be freed as part of garbage collection (i.e. expr = "F") + matd_t *res_copy = (res ? matd_copy(res) : NULL); + + for (int i = 0; i < garbpos; i++) { + matd_destroy(garb[i]); + } + free(garb); + + return res_copy; +} + +double matd_vec_mag(const matd_t *a) +{ + assert(a != NULL); + assert(matd_is_vector(a)); + + double mag = 0.0; + int len = a->nrows*a->ncols; + for (int i = 0; i < len; i++) + mag += sq(a->data[i]); + return sqrt(mag); +} + +double matd_vec_dist(const matd_t *a, const matd_t *b) +{ + assert(a != NULL); + assert(b != NULL); + assert(matd_is_vector(a) && matd_is_vector(b)); + assert(a->nrows*a->ncols == b->nrows*b->ncols); + + int lena = a->nrows*a->ncols; + return matd_vec_dist_n(a, b, lena); +} + +double matd_vec_dist_n(const matd_t *a, const matd_t *b, int n) +{ + assert(a != NULL); + assert(b != NULL); + assert(matd_is_vector(a) && matd_is_vector(b)); + + int lena = a->nrows*a->ncols; + int lenb = b->nrows*b->ncols; + (void) lena; + (void) lenb; + assert(n <= lena && n <= lenb); + + double mag = 0.0; + for (int i = 0; i < n; i++) + mag += sq(a->data[i] - b->data[i]); + return sqrt(mag); +} + +// find the index of the off-diagonal element with the largest mag +static inline int max_idx(const matd_t *A, int row, int maxcol) +{ + int maxi = 0; + double maxv = -1; + + for (int i = 0; i < maxcol; i++) { + if (i == row) + continue; + double v = fabs(MATD_EL(A, row, i)); + if (v > maxv) { + maxi = i; + maxv = v; + } + } + + return maxi; +} + +double matd_vec_dot_product(const matd_t *a, const matd_t *b) +{ + assert(a != NULL); + assert(b != NULL); + assert(matd_is_vector(a) && matd_is_vector(b)); + int adim = a->ncols*a->nrows; + int bdim = b->ncols*b->nrows; + (void) bdim; + assert(adim == bdim); + + double acc = 0; + for (int i = 0; i < adim; i++) { + acc += a->data[i] * b->data[i]; + } + return acc; +} + + +matd_t *matd_vec_normalize(const matd_t *a) +{ + assert(a != NULL); + assert(matd_is_vector(a)); + + double mag = matd_vec_mag(a); + assert(mag > 0); + + matd_t *b = matd_create(a->nrows, a->ncols); + + int len = a->nrows*a->ncols; + for(int i = 0; i < len; i++) + b->data[i] = a->data[i] / mag; + + return b; +} + +matd_t *matd_crossproduct(const matd_t *a, const matd_t *b) +{ // only defined for vecs (col or row) of length 3 + assert(a != NULL); + assert(b != NULL); + assert(matd_is_vector_len(a, 3) && matd_is_vector_len(b, 3)); + + matd_t * r = matd_create(a->nrows, a->ncols); + + r->data[0] = a->data[1] * b->data[2] - a->data[2] * b->data[1]; + r->data[1] = a->data[2] * b->data[0] - a->data[0] * b->data[2]; + r->data[2] = a->data[0] * b->data[1] - a->data[1] * b->data[0]; + + return r; +} + +TYPE matd_err_inf(const matd_t *a, const matd_t *b) +{ + assert(a->nrows == b->nrows); + assert(a->ncols == b->ncols); + + TYPE maxf = 0; + + for (int i = 0; i < a->nrows; i++) { + for (int j = 0; j < a->ncols; j++) { + TYPE av = MATD_EL(a, i, j); + TYPE bv = MATD_EL(b, i, j); + + TYPE err = fabs(av - bv); + maxf = fmax(maxf, err); + } + } + + return maxf; +} + +// Computes an SVD for square or tall matrices. This code doesn't work +// for wide matrices, because the bidiagonalization results in one +// non-zero element too far to the right for us to rotate away. +// +// Caller is responsible for destroying U, S, and V. +static matd_svd_t matd_svd_tall(matd_t *A, int flags) +{ + matd_t *B = matd_copy(A); + + // Apply householder reflections on each side to reduce A to + // bidiagonal form. Specifically: + // + // A = LS*B*RS' + // + // Where B is bidiagonal, and LS/RS are unitary. + // + // Why are we doing this? Some sort of transformation is necessary + // to reduce the matrix's nz elements to a square region. QR could + // work too. We need nzs confined to a square region so that the + // subsequent iterative process, which is based on rotations, can + // work. (To zero out a term at (i,j), our rotations will also + // affect (j,i). + // + // We prefer bidiagonalization over QR because it gets us "closer" + // to the SVD, which should mean fewer iterations. + + // LS: cumulative left-handed transformations + matd_t *LS = matd_identity(A->nrows); + + // RS: cumulative right-handed transformations. + matd_t *RS = matd_identity(A->ncols); + + for (int hhidx = 0; hhidx < A->nrows; hhidx++) { + + if (hhidx < A->ncols) { + // We construct the normal of the reflection plane: let u + // be the vector to reflect, x =[ M 0 0 0 ] the target + // location for u (u') after reflection (with M = ||u||). + // + // The normal vector is then n = (u - x), but since we + // could equally have the target location be x = [-M 0 0 0 + // ], we could use n = (u + x). + // + // We then normalize n. To ensure a reasonable magnitude, + // we select the sign of M so as to maximize the magnitude + // of the first element of (x +/- M). (Otherwise, we could + // end up with a divide-by-zero if u[0] and M cancel.) + // + // The householder reflection matrix is then H=(I - nn'), and + // u' = Hu. + // + // + int vlen = A->nrows - hhidx; + + double *v = malloc(sizeof(double)*vlen); + + double mag2 = 0; + for (int i = 0; i < vlen; i++) { + v[i] = MATD_EL(B, hhidx+i, hhidx); + mag2 += v[i]*v[i]; + } + + double oldv0 = v[0]; + if (oldv0 < 0) + v[0] -= sqrt(mag2); + else + v[0] += sqrt(mag2); + + mag2 += -oldv0*oldv0 + v[0]*v[0]; + + // normalize v + double mag = sqrt(mag2); + + // this case arises with matrices of all zeros, for example. + if (mag == 0) { + free(v); + continue; + } + + for (int i = 0; i < vlen; i++) + v[i] /= mag; + + // Q = I - 2vv' + //matd_t *Q = matd_identity(A->nrows); + //for (int i = 0; i < vlen; i++) + // for (int j = 0; j < vlen; j++) + // MATD_EL(Q, i+hhidx, j+hhidx) -= 2*v[i]*v[j]; + + + // LS = matd_op("F*M", LS, Q); + // Implementation: take each row of LS, compute dot product with n, + // subtract n (scaled by dot product) from it. + for (int i = 0; i < LS->nrows; i++) { + double dot = 0; + for (int j = 0; j < vlen; j++) + dot += MATD_EL(LS, i, hhidx+j) * v[j]; + for (int j = 0; j < vlen; j++) + MATD_EL(LS, i, hhidx+j) -= 2*dot*v[j]; + } + + // B = matd_op("M*F", Q, B); // should be Q', but Q is symmetric. + for (int i = 0; i < B->ncols; i++) { + double dot = 0; + for (int j = 0; j < vlen; j++) + dot += MATD_EL(B, hhidx+j, i) * v[j]; + for (int j = 0; j < vlen; j++) + MATD_EL(B, hhidx+j, i) -= 2*dot*v[j]; + } + + free(v); + } + + if (hhidx+2 < A->ncols) { + int vlen = A->ncols - hhidx - 1; + + double *v = malloc(sizeof(double)*vlen); + + double mag2 = 0; + for (int i = 0; i < vlen; i++) { + v[i] = MATD_EL(B, hhidx, hhidx+i+1); + mag2 += v[i]*v[i]; + } + + double oldv0 = v[0]; + if (oldv0 < 0) + v[0] -= sqrt(mag2); + else + v[0] += sqrt(mag2); + + mag2 += -oldv0*oldv0 + v[0]*v[0]; + + // compute magnitude of ([1 0 0..]+v) + double mag = sqrt(mag2); + + // this case can occur when the vectors are already perpendicular + if (mag == 0) { + free(v); + continue; + } + + for (int i = 0; i < vlen; i++) + v[i] /= mag; + + // TODO: optimize these multiplications + // matd_t *Q = matd_identity(A->ncols); + // for (int i = 0; i < vlen; i++) + // for (int j = 0; j < vlen; j++) + // MATD_EL(Q, i+1+hhidx, j+1+hhidx) -= 2*v[i]*v[j]; + + // RS = matd_op("F*M", RS, Q); + for (int i = 0; i < RS->nrows; i++) { + double dot = 0; + for (int j = 0; j < vlen; j++) + dot += MATD_EL(RS, i, hhidx+1+j) * v[j]; + for (int j = 0; j < vlen; j++) + MATD_EL(RS, i, hhidx+1+j) -= 2*dot*v[j]; + } + + // B = matd_op("F*M", B, Q); // should be Q', but Q is symmetric. + for (int i = 0; i < B->nrows; i++) { + double dot = 0; + for (int j = 0; j < vlen; j++) + dot += MATD_EL(B, i, hhidx+1+j) * v[j]; + for (int j = 0; j < vlen; j++) + MATD_EL(B, i, hhidx+1+j) -= 2*dot*v[j]; + } + + free(v); + } + } + + // empirically, we find a roughly linear worst-case number of iterations + // as a function of rows*cols. maxiters ~= 1.5*nrows*ncols + // we're a bit conservative below. + int maxiters = 200 + 2 * A->nrows * A->ncols; + assert(maxiters > 0); // reassure clang + int iter; + + double maxv; // maximum non-zero value being reduced this iteration + + double tol = 1E-10; + + // which method will we use to find the largest off-diagonal + // element of B? + const int find_max_method = 1; //(B->ncols < 6) ? 2 : 1; + + // for each of the first B->ncols rows, which index has the + // maximum absolute value? (used by method 1) + int *maxrowidx = malloc(sizeof(int)*B->ncols); + int lastmaxi, lastmaxj; + + if (find_max_method == 1) { + for (int i = 2; i < B->ncols; i++) + maxrowidx[i] = max_idx(B, i, B->ncols); + + // note that we started the array at 2. That's because by setting + // these values below, we'll recompute first two entries on the + // first iteration! + lastmaxi = 0, lastmaxj = 1; + } + + for (iter = 0; iter < maxiters; iter++) { + + // No diagonalization required for 0x0 and 1x1 matrices. + if (B->ncols < 2) + break; + + // find the largest off-diagonal element of B, and put its + // coordinates in maxi, maxj. + int maxi, maxj; + + if (find_max_method == 1) { + // method 1 is the "smarter" method which does at least + // 4*ncols work. More work might be needed (up to + // ncols*ncols), depending on data. Thus, this might be a + // bit slower than the default method for very small + // matrices. + maxi = -1; + maxv = -1; + + // every iteration, we must deal with the fact that rows + // and columns lastmaxi and lastmaxj have been + // modified. Update maxrowidx accordingly. + + // now, EVERY row also had columns lastmaxi and lastmaxj modified. + for (int rowi = 0; rowi < B->ncols; rowi++) { + + // the magnitude of the largest off-diagonal element + // in this row. + double thismaxv; + + // row 'lastmaxi' and 'lastmaxj' have been completely + // changed. compute from scratch. + if (rowi == lastmaxi || rowi == lastmaxj) { + maxrowidx[rowi] = max_idx(B, rowi, B->ncols); + thismaxv = fabs(MATD_EL(B, rowi, maxrowidx[rowi])); + goto endrowi; + } + + // our maximum entry was just modified. We don't know + // if it went up or down, and so we don't know if it + // is still the maximum. We have to update from + // scratch. + if (maxrowidx[rowi] == lastmaxi || maxrowidx[rowi] == lastmaxj) { + maxrowidx[rowi] = max_idx(B, rowi, B->ncols); + thismaxv = fabs(MATD_EL(B, rowi, maxrowidx[rowi])); + goto endrowi; + } + + // This row is unchanged, except for columns + // 'lastmaxi' and 'lastmaxj', and those columns were + // not previously the largest entry... just check to + // see if they are now the maximum entry in their + // row. (Remembering to consider off-diagonal entries + // only!) + thismaxv = fabs(MATD_EL(B, rowi, maxrowidx[rowi])); + + // check column lastmaxi. Is it now the maximum? + if (lastmaxi != rowi) { + double v = fabs(MATD_EL(B, rowi, lastmaxi)); + if (v > thismaxv) { + thismaxv = v; + maxrowidx[rowi] = lastmaxi; + } + } + + // check column lastmaxj + if (lastmaxj != rowi) { + double v = fabs(MATD_EL(B, rowi, lastmaxj)); + if (v > thismaxv) { + thismaxv = v; + maxrowidx[rowi] = lastmaxj; + } + } + + // does this row have the largest value we've seen so far? + endrowi: + if (thismaxv > maxv) { + maxv = thismaxv; + maxi = rowi; + } + } + + assert(maxi >= 0); + maxj = maxrowidx[maxi]; + + // save these for the next iteration. + lastmaxi = maxi; + lastmaxj = maxj; + + if (maxv < tol) + break; + + } else if (find_max_method == 2) { + // brute-force (reference) version. + maxv = -1; + + // only search top "square" portion + for (int i = 0; i < B->ncols; i++) { + for (int j = 0; j < B->ncols; j++) { + if (i == j) + continue; + + double v = fabs(MATD_EL(B, i, j)); + + if (v > maxv) { + maxi = i; + maxj = j; + maxv = v; + } + } + } + + // termination condition. + if (maxv < tol) + break; + } else { + assert(0); + } + +// printf(">>> %5d %3d, %3d %15g\n", maxi, maxj, iter, maxv); + + // Now, solve the 2x2 SVD problem for the matrix + // [ A0 A1 ] + // [ A2 A3 ] + double A0 = MATD_EL(B, maxi, maxi); + double A1 = MATD_EL(B, maxi, maxj); + double A2 = MATD_EL(B, maxj, maxi); + double A3 = MATD_EL(B, maxj, maxj); + + if (1) { + double AQ[4]; + AQ[0] = A0; + AQ[1] = A1; + AQ[2] = A2; + AQ[3] = A3; + + double U[4], S[2], V[4]; + svd22(AQ, U, S, V); + +/* Reference (slow) implementation... + + // LS = LS * ROT(theta) = LS * QL + matd_t *QL = matd_identity(A->nrows); + MATD_EL(QL, maxi, maxi) = U[0]; + MATD_EL(QL, maxi, maxj) = U[1]; + MATD_EL(QL, maxj, maxi) = U[2]; + MATD_EL(QL, maxj, maxj) = U[3]; + + matd_t *QR = matd_identity(A->ncols); + MATD_EL(QR, maxi, maxi) = V[0]; + MATD_EL(QR, maxi, maxj) = V[1]; + MATD_EL(QR, maxj, maxi) = V[2]; + MATD_EL(QR, maxj, maxj) = V[3]; + + LS = matd_op("F*M", LS, QL); + RS = matd_op("F*M", RS, QR); // remember we'll transpose RS. + B = matd_op("M'*F*M", QL, B, QR); + + matd_destroy(QL); + matd_destroy(QR); +*/ + + // LS = matd_op("F*M", LS, QL); + for (int i = 0; i < LS->nrows; i++) { + double vi = MATD_EL(LS, i, maxi); + double vj = MATD_EL(LS, i, maxj); + + MATD_EL(LS, i, maxi) = U[0]*vi + U[2]*vj; + MATD_EL(LS, i, maxj) = U[1]*vi + U[3]*vj; + } + + // RS = matd_op("F*M", RS, QR); // remember we'll transpose RS. + for (int i = 0; i < RS->nrows; i++) { + double vi = MATD_EL(RS, i, maxi); + double vj = MATD_EL(RS, i, maxj); + + MATD_EL(RS, i, maxi) = V[0]*vi + V[2]*vj; + MATD_EL(RS, i, maxj) = V[1]*vi + V[3]*vj; + } + + // B = matd_op("M'*F*M", QL, B, QR); + // The QL matrix mixes rows of B. + for (int i = 0; i < B->ncols; i++) { + double vi = MATD_EL(B, maxi, i); + double vj = MATD_EL(B, maxj, i); + + MATD_EL(B, maxi, i) = U[0]*vi + U[2]*vj; + MATD_EL(B, maxj, i) = U[1]*vi + U[3]*vj; + } + + // The QR matrix mixes columns of B. + for (int i = 0; i < B->nrows; i++) { + double vi = MATD_EL(B, i, maxi); + double vj = MATD_EL(B, i, maxj); + + MATD_EL(B, i, maxi) = V[0]*vi + V[2]*vj; + MATD_EL(B, i, maxj) = V[1]*vi + V[3]*vj; + } + } + } + + free(maxrowidx); + + if (!(flags & MATD_SVD_NO_WARNINGS) && iter == maxiters) { + debug_print("WARNING: maximum iters (maximum = %d, matrix %d x %d, max=%.15f)\n", + iter, A->nrows, A->ncols, maxv); + +// matd_print(A, "%15f"); + } + + // them all positive by flipping the corresponding columns of + // U/LS. + int *idxs = malloc(sizeof(int)*A->ncols); + double *vals = malloc(sizeof(double)*A->ncols); + for (int i = 0; i < A->ncols; i++) { + idxs[i] = i; + vals[i] = MATD_EL(B, i, i); + } + + // A bubble sort. Seriously. + int changed; + do { + changed = 0; + + for (int i = 0; i + 1 < A->ncols; i++) { + if (fabs(vals[i+1]) > fabs(vals[i])) { + int tmpi = idxs[i]; + idxs[i] = idxs[i+1]; + idxs[i+1] = tmpi; + + double tmpv = vals[i]; + vals[i] = vals[i+1]; + vals[i+1] = tmpv; + + changed = 1; + } + } + } while (changed); + + matd_t *LP = matd_identity(A->nrows); + matd_t *RP = matd_identity(A->ncols); + + for (int i = 0; i < A->ncols; i++) { + MATD_EL(LP, idxs[i], idxs[i]) = 0; // undo the identity above + MATD_EL(RP, idxs[i], idxs[i]) = 0; + + MATD_EL(LP, idxs[i], i) = vals[i] < 0 ? -1 : 1; + MATD_EL(RP, idxs[i], i) = 1; //vals[i] < 0 ? -1 : 1; + } + free(idxs); + free(vals); + + // we've factored: + // LP*(something)*RP' + + // solve for (something) + B = matd_op("M'*F*M", LP, B, RP); + + // update LS and RS, remembering that RS will be transposed. + LS = matd_op("F*M", LS, LP); + RS = matd_op("F*M", RS, RP); + + matd_destroy(LP); + matd_destroy(RP); + + matd_svd_t res; + memset(&res, 0, sizeof(res)); + + // make B exactly diagonal + + for (int i = 0; i < B->nrows; i++) { + for (int j = 0; j < B->ncols; j++) { + if (i != j) + MATD_EL(B, i, j) = 0; + } + } + + res.U = LS; + res.S = B; + res.V = RS; + + return res; +} + +matd_svd_t matd_svd(matd_t *A) +{ + return matd_svd_flags(A, 0); +} + +matd_svd_t matd_svd_flags(matd_t *A, int flags) +{ + matd_svd_t res; + + if (A->ncols <= A->nrows) { + res = matd_svd_tall(A, flags); + } else { + matd_t *At = matd_transpose(A); + + // A =U S V' + // A'=V S' U' + + matd_svd_t tmp = matd_svd_tall(At, flags); + + memset(&res, 0, sizeof(res)); + res.U = tmp.V; //matd_transpose(tmp.V); + res.S = matd_transpose(tmp.S); + res.V = tmp.U; //matd_transpose(tmp.U); + + matd_destroy(tmp.S); + matd_destroy(At); + } + +/* + matd_t *check = matd_op("M*M*M'-M", res.U, res.S, res.V, A); + double maxerr = 0; + + for (int i = 0; i < check->nrows; i++) + for (int j = 0; j < check->ncols; j++) + maxerr = fmax(maxerr, fabs(MATD_EL(check, i, j))); + + matd_destroy(check); + + if (maxerr > 1e-7) { + printf("bad maxerr: %15f\n", maxerr); + } + + if (maxerr > 1e-5) { + printf("bad maxerr: %15f\n", maxerr); + matd_print(A, "%15f"); + assert(0); + } + +*/ + return res; +} + + +matd_plu_t *matd_plu(const matd_t *a) +{ + unsigned int *piv = calloc(a->nrows, sizeof(unsigned int)); + int pivsign = 1; + matd_t *lu = matd_copy(a); + + // only for square matrices. + assert(a->nrows == a->ncols); + + matd_plu_t *mlu = calloc(1, sizeof(matd_plu_t)); + + for (int i = 0; i < a->nrows; i++) + piv[i] = i; + + for (int j = 0; j < a->ncols; j++) { + for (int i = 0; i < a->nrows; i++) { + int kmax = i < j ? i : j; // min(i,j) + + // compute dot product of row i with column j (up through element kmax) + double acc = 0; + for (int k = 0; k < kmax; k++) + acc += MATD_EL(lu, i, k) * MATD_EL(lu, k, j); + + MATD_EL(lu, i, j) -= acc; + } + + // find pivot and exchange if necessary. + int p = j; + if (1) { + for (int i = j+1; i < lu->nrows; i++) { + if (fabs(MATD_EL(lu,i,j)) > fabs(MATD_EL(lu, p, j))) { + p = i; + } + } + } + + // swap rows p and j? + if (p != j) { + TYPE *tmp = malloc(sizeof(TYPE)*lu->ncols); + memcpy(tmp, &MATD_EL(lu, p, 0), sizeof(TYPE) * lu->ncols); + memcpy(&MATD_EL(lu, p, 0), &MATD_EL(lu, j, 0), sizeof(TYPE) * lu->ncols); + memcpy(&MATD_EL(lu, j, 0), tmp, sizeof(TYPE) * lu->ncols); + int k = piv[p]; + piv[p] = piv[j]; + piv[j] = k; + pivsign = -pivsign; + free(tmp); + } + + double LUjj = MATD_EL(lu, j, j); + + // If our pivot is very small (which means the matrix is + // singular or nearly singular), replace with a new pivot of the + // right sign. + if (fabs(LUjj) < MATD_EPS) { +/* + if (LUjj < 0) + LUjj = -MATD_EPS; + else + LUjj = MATD_EPS; + + MATD_EL(lu, j, j) = LUjj; +*/ + mlu->singular = 1; + } + + if (j < lu->ncols && j < lu->nrows && LUjj != 0) { + LUjj = 1.0 / LUjj; + for (int i = j+1; i < lu->nrows; i++) + MATD_EL(lu, i, j) *= LUjj; + } + } + + mlu->lu = lu; + mlu->piv = piv; + mlu->pivsign = pivsign; + + return mlu; +} + +void matd_plu_destroy(matd_plu_t *mlu) +{ + matd_destroy(mlu->lu); + free(mlu->piv); + memset(mlu, 0, sizeof(matd_plu_t)); + free(mlu); +} + +double matd_plu_det(const matd_plu_t *mlu) +{ + matd_t *lu = mlu->lu; + double det = mlu->pivsign; + + if (lu->nrows == lu->ncols) { + for (int i = 0; i < lu->ncols; i++) + det *= MATD_EL(lu, i, i); + } + + return det; +} + +matd_t *matd_plu_p(const matd_plu_t *mlu) +{ + matd_t *lu = mlu->lu; + matd_t *P = matd_create(lu->nrows, lu->nrows); + + for (int i = 0; i < lu->nrows; i++) { + MATD_EL(P, mlu->piv[i], i) = 1; + } + + return P; +} + +matd_t *matd_plu_l(const matd_plu_t *mlu) +{ + matd_t *lu = mlu->lu; + + matd_t *L = matd_create(lu->nrows, lu->ncols); + for (int i = 0; i < lu->nrows; i++) { + MATD_EL(L, i, i) = 1; + + for (int j = 0; j < i; j++) { + MATD_EL(L, i, j) = MATD_EL(lu, i, j); + } + } + + return L; +} + +matd_t *matd_plu_u(const matd_plu_t *mlu) +{ + matd_t *lu = mlu->lu; + + matd_t *U = matd_create(lu->ncols, lu->ncols); + for (int i = 0; i < lu->ncols; i++) { + for (int j = 0; j < lu->ncols; j++) { + if (i <= j) + MATD_EL(U, i, j) = MATD_EL(lu, i, j); + } + } + + return U; +} + +// PLU = A +// Ax = B +// PLUx = B +// LUx = P'B +matd_t *matd_plu_solve(const matd_plu_t *mlu, const matd_t *b) +{ + matd_t *x = matd_copy(b); + + // permute right hand side + for (int i = 0; i < mlu->lu->nrows; i++) + memcpy(&MATD_EL(x, i, 0), &MATD_EL(b, mlu->piv[i], 0), sizeof(TYPE) * b->ncols); + + // solve Ly = b + for (int k = 0; k < mlu->lu->nrows; k++) { + for (int i = k+1; i < mlu->lu->nrows; i++) { + double LUik = -MATD_EL(mlu->lu, i, k); + for (int t = 0; t < b->ncols; t++) + MATD_EL(x, i, t) += MATD_EL(x, k, t) * LUik; + } + } + + // solve Ux = y + for (int k = mlu->lu->ncols-1; k >= 0; k--) { + double LUkk = 1.0 / MATD_EL(mlu->lu, k, k); + for (int t = 0; t < b->ncols; t++) + MATD_EL(x, k, t) *= LUkk; + + for (int i = 0; i < k; i++) { + double LUik = -MATD_EL(mlu->lu, i, k); + for (int t = 0; t < b->ncols; t++) + MATD_EL(x, i, t) += MATD_EL(x, k, t) *LUik; + } + } + + return x; +} + +matd_t *matd_solve(matd_t *A, matd_t *b) +{ + matd_plu_t *mlu = matd_plu(A); + matd_t *x = matd_plu_solve(mlu, b); + + matd_plu_destroy(mlu); + return x; +} + +#if 0 + +static int randi() +{ + int v = random()&31; + v -= 15; + return v; +} + +static double randf() +{ + double v = 1.0 *random() / RAND_MAX; + return 2*v - 1; +} + +int main(int argc, char *argv[]) +{ + if (1) { + int maxdim = 16; + matd_t *A = matd_create(maxdim, maxdim); + + for (int iter = 0; 1; iter++) { + srand(iter); + + if (iter % 1000 == 0) + printf("%d\n", iter); + + int m = 1 + (random()%(maxdim-1)); + int n = 1 + (random()%(maxdim-1)); + + for (int i = 0; i < m*n; i++) + A->data[i] = randi(); + + A->nrows = m; + A->ncols = n; + +// printf("%d %d ", m, n); + matd_svd_t svd = matd_svd(A); + matd_destroy(svd.U); + matd_destroy(svd.S); + matd_destroy(svd.V); + + } + +/* matd_t *A = matd_create_data(2, 5, (double[]) { 1, 5, 2, 6, + 3, 3, 0, 7, + 1, 1, 0, -2, + 4, 0, 9, 9, 2, 6, 1, 3, 2, 5, 5, 4, -1, 2, 5, 9, 8, 2 }); + + matd_svd(A); +*/ + return 0; + } + + + struct svd22 s; + + srand(0); + + matd_t *A = matd_create(2, 2); + MATD_EL(A,0,0) = 4; + MATD_EL(A,0,1) = 7; + MATD_EL(A,1,0) = 2; + MATD_EL(A,1,1) = 6; + + matd_t *U = matd_create(2, 2); + matd_t *V = matd_create(2, 2); + matd_t *S = matd_create(2, 2); + + for (int iter = 0; 1; iter++) { + if (iter % 100000 == 0) + printf("%d\n", iter); + + MATD_EL(A,0,0) = randf(); + MATD_EL(A,0,1) = randf(); + MATD_EL(A,1,0) = randf(); + MATD_EL(A,1,1) = randf(); + + matd_svd22_impl(A->data, &s); + + memcpy(U->data, s.U, 4*sizeof(double)); + memcpy(V->data, s.V, 4*sizeof(double)); + MATD_EL(S,0,0) = s.S[0]; + MATD_EL(S,1,1) = s.S[1]; + + assert(s.S[0] >= s.S[1]); + assert(s.S[0] >= 0); + assert(s.S[1] >= 0); + if (s.S[0] == 0) { +// printf("*"); fflush(NULL); +// printf("%15f %15f %15f %15f\n", MATD_EL(A,0,0), MATD_EL(A,0,1), MATD_EL(A,1,0), MATD_EL(A,1,1)); + } + if (s.S[1] == 0) { +// printf("#"); fflush(NULL); + } + + matd_t *USV = matd_op("M*M*M'", U, S, V); + + double maxerr = 0; + for (int i = 0; i < 4; i++) + maxerr = fmax(maxerr, fabs(USV->data[i] - A->data[i])); + + if (0) { + printf("------------------------------------\n"); + printf("A:\n"); + matd_print(A, "%15f"); + printf("\nUSV':\n"); + matd_print(USV, "%15f"); + printf("maxerr: %.15f\n", maxerr); + printf("\n\n"); + } + + matd_destroy(USV); + + assert(maxerr < 0.00001); + } +} + +#endif + +// XXX NGV Cholesky +/*static double *matd_cholesky_raw(double *A, int n) + { + double *L = (double*)calloc(n * n, sizeof(double)); + + for (int i = 0; i < n; i++) { + for (int j = 0; j < (i+1); j++) { + double s = 0; + for (int k = 0; k < j; k++) + s += L[i * n + k] * L[j * n + k]; + L[i * n + j] = (i == j) ? + sqrt(A[i * n + i] - s) : + (1.0 / L[j * n + j] * (A[i * n + j] - s)); + } + } + + return L; + } + + matd_t *matd_cholesky(const matd_t *A) + { + assert(A->nrows == A->ncols); + double *L_data = matd_cholesky_raw(A->data, A->nrows); + matd_t *L = matd_create_data(A->nrows, A->ncols, L_data); + free(L_data); + return L; + }*/ + +// NOTE: The below implementation of Cholesky is different from the one +// used in NGV. +matd_chol_t *matd_chol(matd_t *A) +{ + assert(A->nrows == A->ncols); + int N = A->nrows; + + // make upper right + matd_t *U = matd_copy(A); + + // don't actually need to clear lower-left... we won't touch it. +/* for (int i = 0; i < U->nrows; i++) { + for (int j = 0; j < i; j++) { +// assert(MATD_EL(U, i, j) == MATD_EL(U, j, i)); +MATD_EL(U, i, j) = 0; +} +} +*/ + int is_spd = 1; // (A->nrows == A->ncols); + + for (int i = 0; i < N; i++) { + double d = MATD_EL(U, i, i); + is_spd &= (d > 0); + + if (d < MATD_EPS) + d = MATD_EPS; + d = 1.0 / sqrt(d); + + for (int j = i; j < N; j++) + MATD_EL(U, i, j) *= d; + + for (int j = i+1; j < N; j++) { + double s = MATD_EL(U, i, j); + + if (s == 0) + continue; + + for (int k = j; k < N; k++) { + MATD_EL(U, j, k) -= MATD_EL(U, i, k)*s; + } + } + } + + matd_chol_t *chol = calloc(1, sizeof(matd_chol_t)); + chol->is_spd = is_spd; + chol->u = U; + return chol; +} + +void matd_chol_destroy(matd_chol_t *chol) +{ + matd_destroy(chol->u); + free(chol); +} + +// Solve: (U')x = b, U is upper triangular +void matd_ltransposetriangle_solve(matd_t *u, const TYPE *b, TYPE *x) +{ + int n = u->ncols; + memcpy(x, b, n*sizeof(TYPE)); + for (int i = 0; i < n; i++) { + x[i] /= MATD_EL(u, i, i); + + for (int j = i+1; j < u->ncols; j++) { + x[j] -= x[i] * MATD_EL(u, i, j); + } + } +} + +// Solve: Lx = b, L is lower triangular +void matd_ltriangle_solve(matd_t *L, const TYPE *b, TYPE *x) +{ + int n = L->ncols; + + for (int i = 0; i < n; i++) { + double acc = b[i]; + + for (int j = 0; j < i; j++) { + acc -= MATD_EL(L, i, j)*x[j]; + } + + x[i] = acc / MATD_EL(L, i, i); + } +} + +// solve Ux = b, U is upper triangular +void matd_utriangle_solve(matd_t *u, const TYPE *b, TYPE *x) +{ + for (int i = u->ncols-1; i >= 0; i--) { + double bi = b[i]; + + double diag = MATD_EL(u, i, i); + + for (int j = i+1; j < u->ncols; j++) + bi -= MATD_EL(u, i, j)*x[j]; + + x[i] = bi / diag; + } +} + +matd_t *matd_chol_solve(const matd_chol_t *chol, const matd_t *b) +{ + matd_t *u = chol->u; + + matd_t *x = matd_copy(b); + + // LUx = b + + // solve Ly = b ==> (U')y = b + + for (int i = 0; i < u->nrows; i++) { + for (int j = 0; j < i; j++) { + // b[i] -= L[i,j]*x[j]... replicated across columns of b + // ==> i.e., ==> + // b[i,k] -= L[i,j]*x[j,k] + for (int k = 0; k < b->ncols; k++) { + MATD_EL(x, i, k) -= MATD_EL(u, j, i)*MATD_EL(x, j, k); + } + } + // x[i] = b[i] / L[i,i] + for (int k = 0; k < b->ncols; k++) { + MATD_EL(x, i, k) /= MATD_EL(u, i, i); + } + } + + // solve Ux = y + for (int k = u->ncols-1; k >= 0; k--) { + double LUkk = 1.0 / MATD_EL(u, k, k); + for (int t = 0; t < b->ncols; t++) + MATD_EL(x, k, t) *= LUkk; + + for (int i = 0; i < k; i++) { + double LUik = -MATD_EL(u, i, k); + for (int t = 0; t < b->ncols; t++) + MATD_EL(x, i, t) += MATD_EL(x, k, t) *LUik; + } + } + + return x; +} + +/*void matd_chol_solve(matd_chol_t *chol, const TYPE *b, TYPE *x) + { + matd_t *u = chol->u; + + TYPE y[u->ncols]; + matd_ltransposetriangle_solve(u, b, y); + matd_utriangle_solve(u, y, x); + } +*/ +// only sensible on PSD matrices. had expected it to be faster than +// inverse via LU... for now, doesn't seem to be. +matd_t *matd_chol_inverse(matd_t *a) +{ + assert(a->nrows == a->ncols); + + matd_chol_t *chol = matd_chol(a); + + matd_t *eye = matd_identity(a->nrows); + matd_t *inv = matd_chol_solve(chol, eye); + matd_destroy(eye); + matd_chol_destroy(chol); + + return inv; +} + +double matd_max(matd_t *m) +{ + double d = -DBL_MAX; + for(int x=0; xnrows; x++) { + for(int y=0; yncols; y++) { + if(MATD_EL(m, x, y) > d) + d = MATD_EL(m, x, y); + } + } + + return d; +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/common/pam.c b/apriltag/src/main/native/thirdparty/apriltag/src/common/pam.c new file mode 100644 index 0000000000..a86b3fe26a --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/common/pam.c @@ -0,0 +1,256 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#include +#include +#include +#include + +#include "pam.h" + +pam_t *pam_create_from_file(const char *inpath) +{ + FILE *infile = fopen(inpath, "r"); + if (infile == NULL) { + printf("pam.c: couldn't open input file: %s\n", inpath); + return NULL; + } + + pam_t *pam = calloc(1, sizeof(pam_t)); + pam->width = -1; + pam->height = -1; + pam->depth = -1; + pam->maxval = -1; + pam->type = -1; + + int linenumber = 0; + + while (1) { + char line[1024]; + if (!fgets(line, sizeof(line), infile)) { + printf("pam.c: unexpected EOF\n"); + goto fail; + } + linenumber++; + + char *tok0 = line; + char *tok1 = NULL; + + if (line[0] == '#') // comment + continue; + + size_t linelen = strlen(line); + for (size_t idx = 0; idx < linelen; idx++) { + if (line[idx] == ' ') { + line[idx] = 0; + if (tok1) { + printf("pam.c: More than two tokens, %s:%d\n", inpath, linenumber); + } + + tok1 = &line[idx+1]; + } + if (line[idx] == '\n') + line[idx] = 0; + } + + if (!strcmp(tok0, "P7")) + continue; + + if (!strcmp(tok0, "ENDHDR")) + break; + + if (!strcmp(tok0, "WIDTH") && tok1) { + pam->width = atoi(tok1); + continue; + } + + if (!strcmp(tok0, "HEIGHT") && tok1) { + pam->height = atoi(tok1); + continue; + } + + if (!strcmp(tok0, "DEPTH") && tok1) { + pam->depth = atoi(tok1); + continue; + } + + if (!strcmp(tok0, "MAXVAL") && tok1) { + pam->maxval = atoi(tok1); + continue; + } + + if (!strcmp(tok0, "TUPLTYPE") && tok1) { + if (!strcmp(tok1, "GRAYSCALE_ALPHA")) { + pam->type = PAM_GRAYSCALE_ALPHA; + continue; + } + + if (!strcmp(tok1, "RGB_ALPHA")) { + pam->type = PAM_RGB_ALPHA; + continue; + } + + if (!strcmp(tok1, "RGB")) { + pam->type = PAM_RGB; + continue; + } + + if (!strcmp(tok1, "GRAYSCALE")) { + pam->type = PAM_GRAYSCALE; + continue; + } + + printf("pam.c: unrecognized tupl type %s\n", tok1); + continue; + } + + printf("pam.c: unrecognized attribute %s\n", tok0); + } + + if (pam->width < 0 || pam->height < 0 || pam->depth < 0 || + pam->maxval < 0 || pam->type < 0) { + printf("pam.c: missing required metadata field\n"); + goto fail; + } + + assert(pam->maxval == 255); + + pam->datalen = pam->width * pam->height * pam->depth; + pam->data = malloc(pam->datalen); + if (pam->datalen != fread(pam->data, 1, pam->datalen, infile)) { + printf("pam.c: couldn't read body\n"); + goto fail; + } + + fclose(infile); + return pam; + + fail: + free(pam); + fclose(infile); + return NULL; +} + +int pam_write_file(pam_t *pam, const char *outpath) +{ + FILE *f = fopen(outpath, "w+"); + if (!f) + return -1; + + const char *tupl = NULL; + switch (pam->type) { + case PAM_GRAYSCALE_ALPHA: + tupl = "GRAYSCALE_ALPHA"; + break; + case PAM_RGB_ALPHA: + tupl = "RGB_ALPHA"; + break; + case PAM_RGB: + tupl = "RGB"; + break; + case PAM_GRAYSCALE: + tupl = "GRAYSCALE"; + break; + default: + assert(0); + } + + fprintf(f, "P7\nWIDTH %d\nHEIGHT %d\nDEPTH %d\nMAXVAL %d\nTUPLTYPE %s\nENDHDR\n", + pam->width, pam->height, pam->depth, pam->maxval, tupl); + int len = pam->width * pam->height * pam->depth; + if (len != fwrite(pam->data, 1, len, f)) { + fclose(f); + return -2; + } + + fclose(f); + + return 0; +} + +void pam_destroy(pam_t *pam) +{ + if (!pam) + return; + + free(pam->data); + free(pam); +} + +pam_t *pam_copy(pam_t *pam) +{ + pam_t *copy = calloc(1, sizeof(pam_t)); + copy->width = pam->width; + copy->height = pam->height; + copy->depth = pam->depth; + copy->maxval = pam->maxval; + copy->type = pam->type; + + copy->datalen = pam->datalen; + copy->data = malloc(pam->datalen); + memcpy(copy->data, pam->data, pam->datalen); + + return copy; +} + +pam_t *pam_convert(pam_t *in, int type) +{ + if (type == in->type) + return pam_copy(in); + + assert(type == PAM_RGB_ALPHA); // we don't support a lot yet + assert(in->maxval == 255); + + int w = in->width; + int h = in->height; + + pam_t *out = calloc(1, sizeof(pam_t)); + out->type = type; + out->width = w; + out->height = h; + out->maxval = in->maxval; + out->depth = 4; + out->datalen = 4 * w * h; + out->data = malloc(out->datalen); + + if (in->type == PAM_RGB) { + assert(in->depth == 3); + for (int y = 0; y < h; y++) { + for (int x = 0; x < w; x++) { + out->data[y*4*w + 4*x + 0] = in->data[y*3*w + 3*x + 0]; + out->data[y*4*w + 4*x + 1] = in->data[y*3*w + 3*x + 1]; + out->data[y*4*w + 4*x + 2] = in->data[y*3*w + 3*x + 2]; + out->data[y*4*w + 4*x + 3] = 255; + } + } + } else { + printf("pam.c unsupported type %d\n", in->type); + assert(0); + } + + return out; +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/common/pjpeg-idct.c b/apriltag/src/main/native/thirdparty/apriltag/src/common/pjpeg-idct.c new file mode 100644 index 0000000000..ddbdde5037 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/common/pjpeg-idct.c @@ -0,0 +1,388 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#include +#include + +#ifndef M_PI +# define M_PI 3.141592653589793238462643383279502884196 +#endif + +// 8 bits of fixed-point output +// +// This implementation has a worst-case complexity of 22 multiplies +// and 64 adds. This makes it significantly worse (about 2x) than the +// best-known fast inverse cosine transform methods. HOWEVER, zero +// coefficients can be skipped over, and since that's common (often +// more than half the coefficients are zero). +// +// The output is scaled by a factor of 256 (due to our fixed-point +// integer arithmetic).. +static inline void idct_1D_u32(int32_t *in, int instride, int32_t *out, int outstride) +{ + for (int x = 0; x < 8; x++) + out[x*outstride] = 0; + + int32_t c; + + c = in[0*instride]; + if (c) { + // 181 181 181 181 181 181 181 181 + int32_t c181 = c * 181; + out[0*outstride] += c181; + out[1*outstride] += c181; + out[2*outstride] += c181; + out[3*outstride] += c181; + out[4*outstride] += c181; + out[5*outstride] += c181; + out[6*outstride] += c181; + out[7*outstride] += c181; + } + + c = in[1*instride]; + if (c) { + // 251 212 142 49 -49 -142 -212 -251 + int32_t c251 = c * 251; + int32_t c212 = c * 212; + int32_t c142 = c * 142; + int32_t c49 = c * 49; + out[0*outstride] += c251; + out[1*outstride] += c212; + out[2*outstride] += c142; + out[3*outstride] += c49; + out[4*outstride] -= c49; + out[5*outstride] -= c142; + out[6*outstride] -= c212; + out[7*outstride] -= c251; + } + + c = in[2*instride]; + if (c) { + // 236 97 -97 -236 -236 -97 97 236 + int32_t c236 = c*236; + int32_t c97 = c*97; + out[0*outstride] += c236; + out[1*outstride] += c97; + out[2*outstride] -= c97; + out[3*outstride] -= c236; + out[4*outstride] -= c236; + out[5*outstride] -= c97; + out[6*outstride] += c97; + out[7*outstride] += c236; + } + + c = in[3*instride]; + if (c) { + // 212 -49 -251 -142 142 251 49 -212 + int32_t c212 = c*212; + int32_t c49 = c*49; + int32_t c251 = c*251; + int32_t c142 = c*142; + out[0*outstride] += c212; + out[1*outstride] -= c49; + out[2*outstride] -= c251; + out[3*outstride] -= c142; + out[4*outstride] += c142; + out[5*outstride] += c251; + out[6*outstride] += c49; + out[7*outstride] -= c212; + } + + c = in[4*instride]; + if (c) { + // 181 -181 -181 181 181 -181 -181 181 + int32_t c181 = c*181; + out[0*outstride] += c181; + out[1*outstride] -= c181; + out[2*outstride] -= c181; + out[3*outstride] += c181; + out[4*outstride] += c181; + out[5*outstride] -= c181; + out[6*outstride] -= c181; + out[7*outstride] += c181; + } + + c = in[5*instride]; + if (c) { + // 142 -251 49 212 -212 -49 251 -142 + int32_t c142 = c*142; + int32_t c251 = c*251; + int32_t c49 = c*49; + int32_t c212 = c*212; + out[0*outstride] += c142; + out[1*outstride] -= c251; + out[2*outstride] += c49; + out[3*outstride] += c212; + out[4*outstride] -= c212; + out[5*outstride] -= c49; + out[6*outstride] += c251; + out[7*outstride] -= c142; + } + + c = in[6*instride]; + if (c) { + // 97 -236 236 -97 -97 236 -236 97 + int32_t c97 = c*97; + int32_t c236 = c*236; + out[0*outstride] += c97; + out[1*outstride] -= c236; + out[2*outstride] += c236; + out[3*outstride] -= c97; + out[4*outstride] -= c97; + out[5*outstride] += c236; + out[6*outstride] -= c236; + out[7*outstride] += c97; + } + + c = in[7*instride]; + if (c) { + // 49 -142 212 -251 251 -212 142 -49 + int32_t c49 = c*49; + int32_t c142 = c*142; + int32_t c212 = c*212; + int32_t c251 = c*251; + out[0*outstride] += c49; + out[1*outstride] -= c142; + out[2*outstride] += c212; + out[3*outstride] -= c251; + out[4*outstride] += c251; + out[5*outstride] -= c212; + out[6*outstride] += c142; + out[7*outstride] -= c49; + } +} + +void pjpeg_idct_2D_u32(int32_t in[64], uint8_t *out, uint32_t outstride) +{ + int32_t tmp[64]; + + // idct on rows + for (int y = 0; y < 8; y++) + idct_1D_u32(&in[8*y], 1, &tmp[8*y], 1); + + int32_t tmp2[64]; + + // idct on columns + for (int x = 0; x < 8; x++) + idct_1D_u32(&tmp[x], 8, &tmp2[x], 8); + + // scale, adjust bias, and clamp + for (int y = 0; y < 8; y++) { + for (int x = 0; x < 8; x++) { + int i = 8*y + x; + + // Shift of 18: the divide by 4 as part of the idct, and a shift by 16 + // to undo the fixed-point arithmetic. (We accumulated 8 bits of + // fractional precision during each of the row and column IDCTs) + // + // Originally: + // int32_t v = (tmp2[i] >> 18) + 128; + // + // Move the add before the shift and we can do rounding at + // the same time. + const int32_t offset = (128 << 18) + (1 << 17); + int32_t v = (tmp2[i] + offset) >> 18; + + if (v < 0) + v = 0; + if (v > 255) + v = 255; + + out[y*outstride + x] = v; + } + } +} + +/////////////////////////////////////////////////////// +// Below: a "as straight-forward as I can make" implementation. +static inline void idct_1D_double(double *in, int instride, double *out, int outstride) +{ + for (int x = 0; x < 8; x++) + out[x*outstride] = 0; + + // iterate over IDCT coefficients + double Cu = 1/sqrt(2); + + for (int u = 0; u < 8; u++, Cu = 1) { + + double coeff = in[u*instride]; + if (coeff == 0) + continue; + + for (int x = 0; x < 8; x++) + out[x*outstride] += Cu*cos((2*x+1)*u*M_PI/16) * coeff; + } +} + +void pjpeg_idct_2D_double(int32_t in[64], uint8_t *out, uint32_t outstride) +{ + double din[64], dout[64]; + for (int i = 0; i < 64; i++) + din[i] = in[i]; + + double tmp[64]; + + // idct on rows + for (int y = 0; y < 8; y++) + idct_1D_double(&din[8*y], 1, &tmp[8*y], 1); + + // idct on columns + for (int x = 0; x < 8; x++) + idct_1D_double(&tmp[x], 8, &dout[x], 8); + + // scale, adjust bias, and clamp + for (int y = 0; y < 8; y++) { + for (int x = 0; x < 8; x++) { + int i = 8*y + x; + + dout[i] = (dout[i] / 4) + 128; + if (dout[i] < 0) + dout[i] = 0; + if (dout[i] > 255) + dout[i] = 255; + + // XXX round by adding +.5? + out[y*outstride + x] = dout[i]; + } + } +} + +////////////////////////////////////////////// +static inline unsigned char njClip(const int x) { + return (x < 0) ? 0 : ((x > 0xFF) ? 0xFF : (unsigned char) x); +} + +#define W1 2841 +#define W2 2676 +#define W3 2408 +#define W5 1609 +#define W6 1108 +#define W7 565 + +static inline void njRowIDCT(int* blk) { + int x0, x1, x2, x3, x4, x5, x6, x7, x8; + if (!((x1 = blk[4] << 11) + | (x2 = blk[6]) + | (x3 = blk[2]) + | (x4 = blk[1]) + | (x5 = blk[7]) + | (x6 = blk[5]) + | (x7 = blk[3]))) + { + blk[0] = blk[1] = blk[2] = blk[3] = blk[4] = blk[5] = blk[6] = blk[7] = blk[0] << 3; + return; + } + x0 = (blk[0] << 11) + 128; + x8 = W7 * (x4 + x5); + x4 = x8 + (W1 - W7) * x4; + x5 = x8 - (W1 + W7) * x5; + x8 = W3 * (x6 + x7); + x6 = x8 - (W3 - W5) * x6; + x7 = x8 - (W3 + W5) * x7; + x8 = x0 + x1; + x0 -= x1; + x1 = W6 * (x3 + x2); + x2 = x1 - (W2 + W6) * x2; + x3 = x1 + (W2 - W6) * x3; + x1 = x4 + x6; + x4 -= x6; + x6 = x5 + x7; + x5 -= x7; + x7 = x8 + x3; + x8 -= x3; + x3 = x0 + x2; + x0 -= x2; + x2 = (181 * (x4 + x5) + 128) >> 8; + x4 = (181 * (x4 - x5) + 128) >> 8; + blk[0] = (x7 + x1) >> 8; + blk[1] = (x3 + x2) >> 8; + blk[2] = (x0 + x4) >> 8; + blk[3] = (x8 + x6) >> 8; + blk[4] = (x8 - x6) >> 8; + blk[5] = (x0 - x4) >> 8; + blk[6] = (x3 - x2) >> 8; + blk[7] = (x7 - x1) >> 8; +} + +static inline void njColIDCT(const int* blk, unsigned char *out, int stride) { + int x0, x1, x2, x3, x4, x5, x6, x7, x8; + if (!((x1 = blk[8*4] << 8) + | (x2 = blk[8*6]) + | (x3 = blk[8*2]) + | (x4 = blk[8*1]) + | (x5 = blk[8*7]) + | (x6 = blk[8*5]) + | (x7 = blk[8*3]))) + { + x1 = njClip(((blk[0] + 32) >> 6) + 128); + for (x0 = 8; x0; --x0) { + *out = (unsigned char) x1; + out += stride; + } + return; + } + x0 = (blk[0] << 8) + 8192; + x8 = W7 * (x4 + x5) + 4; + x4 = (x8 + (W1 - W7) * x4) >> 3; + x5 = (x8 - (W1 + W7) * x5) >> 3; + x8 = W3 * (x6 + x7) + 4; + x6 = (x8 - (W3 - W5) * x6) >> 3; + x7 = (x8 - (W3 + W5) * x7) >> 3; + x8 = x0 + x1; + x0 -= x1; + x1 = W6 * (x3 + x2) + 4; + x2 = (x1 - (W2 + W6) * x2) >> 3; + x3 = (x1 + (W2 - W6) * x3) >> 3; + x1 = x4 + x6; + x4 -= x6; + x6 = x5 + x7; + x5 -= x7; + x7 = x8 + x3; + x8 -= x3; + x3 = x0 + x2; + x0 -= x2; + x2 = (181 * (x4 + x5) + 128) >> 8; + x4 = (181 * (x4 - x5) + 128) >> 8; + *out = njClip(((x7 + x1) >> 14) + 128); out += stride; + *out = njClip(((x3 + x2) >> 14) + 128); out += stride; + *out = njClip(((x0 + x4) >> 14) + 128); out += stride; + *out = njClip(((x8 + x6) >> 14) + 128); out += stride; + *out = njClip(((x8 - x6) >> 14) + 128); out += stride; + *out = njClip(((x0 - x4) >> 14) + 128); out += stride; + *out = njClip(((x3 - x2) >> 14) + 128); out += stride; + *out = njClip(((x7 - x1) >> 14) + 128); +} + +void pjpeg_idct_2D_nanojpeg(int32_t in[64], uint8_t *out, uint32_t outstride) +{ + int coef; + + for (coef = 0; coef < 64; coef += 8) + njRowIDCT(&in[coef]); + for (coef = 0; coef < 8; ++coef) + njColIDCT(&in[coef], &out[coef], outstride); +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/common/pjpeg.c b/apriltag/src/main/native/thirdparty/apriltag/src/common/pjpeg.c new file mode 100644 index 0000000000..1941c27b77 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/common/pjpeg.c @@ -0,0 +1,894 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#include +#include +#include +#include +#include + +#include "pjpeg.h" + +#include "image_u8.h" +#include "image_u8x3.h" +#include "debug_print.h" + +// https://www.w3.org/Graphics/JPEG/itu-t81.pdf + +void pjpeg_idct_2D_double(int32_t in[64], uint8_t *out, uint32_t outstride); +void pjpeg_idct_2D_u32(int32_t in[64], uint8_t *out, uint32_t outstride); +void pjpeg_idct_2D_nanojpeg(int32_t in[64], uint8_t *out, uint32_t outstride); + +struct pjpeg_huffman_code +{ + uint8_t nbits; // how many bits should we actually consume? + uint8_t code; // what is the symbol that was encoded? (not actually a DCT coefficient; see encoding) +}; + +struct pjpeg_decode_state +{ + int error; + + uint32_t width, height; + uint8_t *in; + uint32_t inlen; + + uint32_t flags; + + // to decode, we load the next 16 bits of input (generally more + // than we need). We then look up in our code book how many bits + // we have actually consumed. For example, if there was a code + // whose bit sequence was "0", the first 32768 entries would all + // be copies of {.bits=1, .value=XX}; no matter what the following + // 15 bits are, we would get the correct decode. + // + // Can be up to 8 tables; computed as (ACDC * 2 + htidx) + struct pjpeg_huffman_code huff_codes[4][65536]; + int huff_codes_present[4]; + + uint8_t qtab[4][64]; + + int ncomponents; + pjpeg_component_t *components; + + int reset_interval; + int reset_count; + int reset_next; // What reset marker do we expect next? (add 0xd0) + + int debug; +}; + +// from K.3.3.1 (page 158) +static uint8_t mjpeg_dht[] = { // header + 0xFF,0xC4,0x01,0xA2, + + ///////////////////////////////////////////////////////////// + // luminance dc coefficients. + // DC table 0 + 0x00, + // code lengths + 0x00,0x01,0x05,0x01,0x01,0x01,0x01,0x01,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + // values + 0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0A,0x0B, + + ///////////////////////////////////////////////////////////// + // chrominance DC coefficients + // DC table 1 + 0x01, + // code lengths + 0x00,0x03,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x00,0x00,0x00,0x00,0x00, + // values + 0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0A,0x0B, + + ///////////////////////////////////////////////////////////// + // luminance AC coefficients + // AC table 0 + 0x10, + // code lengths + 0x00,0x02,0x01,0x03,0x03,0x02,0x04,0x03,0x05,0x05,0x04,0x04,0x00,0x00,0x01,0x7D, + // codes + 0x01,0x02,0x03,0x00,0x04,0x11,0x05,0x12,0x21,0x31,0x41,0x06,0x13,0x51,0x61, + 0x07,0x22,0x71,0x14,0x32,0x81,0x91,0xA1,0x08,0x23,0x42,0xB1,0xC1,0x15,0x52,0xD1,0xF0,0x24, + 0x33,0x62,0x72,0x82,0x09,0x0A,0x16,0x17,0x18,0x19,0x1A,0x25,0x26,0x27,0x28,0x29,0x2A,0x34, + 0x35,0x36,0x37,0x38,0x39,0x3A,0x43,0x44,0x45,0x46,0x47,0x48,0x49,0x4A,0x53,0x54,0x55,0x56, + 0x57,0x58,0x59,0x5A,0x63,0x64,0x65,0x66,0x67,0x68,0x69,0x6A,0x73,0x74,0x75,0x76,0x77,0x78, + 0x79,0x7A,0x83,0x84,0x85,0x86,0x87,0x88,0x89,0x8A,0x92,0x93,0x94,0x95,0x96,0x97,0x98,0x99, + 0x9A,0xA2,0xA3,0xA4,0xA5,0xA6,0xA7,0xA8,0xA9,0xAA,0xB2,0xB3,0xB4,0xB5,0xB6,0xB7,0xB8,0xB9, + 0xBA,0xC2,0xC3,0xC4,0xC5,0xC6,0xC7,0xC8,0xC9,0xCA,0xD2,0xD3,0xD4,0xD5,0xD6,0xD7,0xD8,0xD9, + 0xDA,0xE1,0xE2,0xE3,0xE4,0xE5,0xE6,0xE7,0xE8,0xE9,0xEA,0xF1,0xF2,0xF3,0xF4,0xF5,0xF6,0xF7, + 0xF8,0xF9,0xFA, + + ///////////////////////////////////////////////////////////// + // chrominance DC coefficients + // DC table 1 + 0x11, + // code lengths + 0x00,0x02,0x01,0x02,0x04,0x04,0x03,0x04,0x07,0x05,0x04,0x04,0x00,0x01,0x02,0x77, + // values + 0x00,0x01,0x02,0x03,0x11,0x04,0x05,0x21,0x31,0x06,0x12,0x41,0x51,0x07,0x61,0x71, + 0x13,0x22,0x32,0x81,0x08,0x14,0x42,0x91,0xA1,0xB1,0xC1,0x09,0x23,0x33,0x52,0xF0,0x15,0x62, + 0x72,0xD1,0x0A,0x16,0x24,0x34,0xE1,0x25,0xF1,0x17,0x18,0x19,0x1A,0x26,0x27,0x28,0x29,0x2A, + 0x35,0x36,0x37,0x38,0x39,0x3A,0x43,0x44,0x45,0x46,0x47,0x48,0x49,0x4A,0x53,0x54,0x55,0x56, + 0x57,0x58,0x59,0x5A,0x63,0x64,0x65,0x66,0x67,0x68,0x69,0x6A,0x73,0x74,0x75,0x76,0x77,0x78, + 0x79,0x7A,0x82,0x83,0x84,0x85,0x86,0x87,0x88,0x89,0x8A,0x92,0x93,0x94,0x95,0x96,0x97,0x98, + 0x99,0x9A,0xA2,0xA3,0xA4,0xA5,0xA6,0xA7,0xA8,0xA9,0xAA,0xB2,0xB3,0xB4,0xB5,0xB6,0xB7,0xB8, + 0xB9,0xBA,0xC2,0xC3,0xC4,0xC5,0xC6,0xC7,0xC8,0xC9,0xCA,0xD2,0xD3,0xD4,0xD5,0xD6,0xD7,0xD8, + 0xD9,0xDA,0xE2,0xE3,0xE4,0xE5,0xE6,0xE7,0xE8,0xE9,0xEA,0xF2,0xF3,0xF4,0xF5,0xF6,0xF7,0xF8, + 0xF9,0xFA +}; + +static inline uint8_t max_u8(uint8_t a, uint8_t b) +{ + return a > b ? a : b; +} + +// order of coefficients in each DC block +static const char ZZ[64] = { 0, 1, 8, 16, 9, 2, 3, 10, + 17, 24, 32, 25, 18, 11, 4, 5, + 12, 19, 26, 33, 40, 48, 41, 34, + 27, 20, 13, 6, 7, 14, 21, 28, + 35, 42, 49, 56, 57, 50, 43, 36, + 29, 22, 15, 23, 30, 37, 44, 51, + 58, 59, 52, 45, 38, 31, 39, 46, + 53, 60, 61, 54, 47, 55, 62, 63 }; + + + +struct bit_decoder +{ + uint8_t *in; + uint32_t inpos; + uint32_t inlen; + + uint32_t bits; // the low order bits contain the next nbits_avail bits. + + int nbits_avail; // how many bits in 'bits' (left aligned) are valid? + + int error; +}; + +// ensure that at least 'nbits' of data is available in the bit decoder. +static inline void bd_ensure(struct bit_decoder *bd, int nbits) +{ + while (bd->nbits_avail < nbits) { + + if (bd->inpos >= bd->inlen) { + printf("hallucinating 1s!\n"); + // we hit end of stream hallucinate an infinite stream of 1s + bd->bits = (bd->bits << 8) | 0xff; + bd->nbits_avail += 8; + continue; + } + + uint8_t nextbyte = bd->in[bd->inpos]; + bd->inpos++; + + if (nextbyte == 0xff && bd->inpos < bd->inlen && bd->in[bd->inpos] == 0x00) { + // a stuffed byte + nextbyte = 0xff; + bd->inpos++; + } + + // it's an ordinary byte + bd->bits = (bd->bits << 8) | nextbyte; + bd->nbits_avail += 8; + } +} + +static inline uint32_t bd_peek_bits(struct bit_decoder *bd, int nbits) +{ + bd_ensure(bd, nbits); + + return (bd->bits >> (bd->nbits_avail - nbits)) & ((1 << nbits) - 1); +} + +static inline uint32_t bd_consume_bits(struct bit_decoder *bd, int nbits) +{ + assert(nbits < 32); + + bd_ensure(bd, nbits); + + uint32_t v = (bd->bits >> (bd->nbits_avail - nbits)) & ((1 << nbits) - 1); + + bd->nbits_avail -= nbits; + + return v; +} + +// discard without regard for byte stuffing! +static inline void bd_discard_bytes(struct bit_decoder *bd, int nbytes) +{ + assert(bd->nbits_avail == 0); + bd->inpos += nbytes; +} + +static inline int bd_has_more(struct bit_decoder *bd) +{ + return bd->nbits_avail > 0 || bd->inpos < bd->inlen; +} + +// throw away up to 7 bits of data so that the next data returned +// began on a byte boundary. +static inline void bd_discard_to_byte_boundary(struct bit_decoder *bd) +{ + bd->nbits_avail -= (bd->nbits_avail & 7); +} + +static inline uint32_t bd_get_offset(struct bit_decoder *bd) +{ + return bd->inpos - bd->nbits_avail / 8; +} + +static int pjpeg_decode_buffer(struct pjpeg_decode_state *pjd) +{ + // XXX TODO Include sanity check that this is actually a JPG + + struct bit_decoder bd; + memset(&bd, 0, sizeof(struct bit_decoder)); + bd.in = pjd->in; + bd.inpos = 0; + bd.inlen = pjd->inlen; + + int marker_sync_skipped = 0; + int marker_sync_skipped_from_offset = 0; + + while (bd_has_more(&bd)) { + + uint32_t marker_offset = bd_get_offset(&bd); + + // Look for the 0xff that signifies the beginning of a marker + bd_discard_to_byte_boundary(&bd); + + while (bd_consume_bits(&bd, 8) != 0xff) { + if (marker_sync_skipped == 0) + marker_sync_skipped_from_offset = marker_offset; + marker_sync_skipped++; + continue; + } + + if (marker_sync_skipped) { + printf("%08x: skipped %04x bytes\n", marker_sync_skipped_from_offset, marker_sync_skipped); + marker_sync_skipped = 0; + } + + uint8_t marker = bd_consume_bits(&bd, 8); + +// printf("marker %08x : %02x\n", marker_offset, marker); + + switch (marker) { + + case 0xd8: // start of image. Great, continue. + continue; + + // below are the markers that A) we don't care about + // that B) encode length as two bytes. + // + // Note: Other unknown fields should not be added since + // we should be able to skip over them by looking for + // the next marker byte. + case 0xe0: // JFIF header. + case 0xe1: // EXIF header (Yuck: Payload may contain 0xff 0xff!) + case 0xe2: // ICC Profile. (Yuck: payload may contain 0xff 0xff!) + case 0xe6: // some other common header + case 0xfe: // Comment + { + uint16_t length = bd_consume_bits(&bd, 16); + bd_discard_bytes(&bd, length - 2); + continue; + } + + case 0xdb: { // DQT Define Quantization Table + uint16_t length = bd_consume_bits(&bd, 16); + + if (((length-2) % 65) != 0) + return PJPEG_ERR_DQT; + + // can contain multiple DQTs + for (int offset = 0; offset < length - 2; offset += 65) { + + // pq: quant table element precision. 0=8bit, 1=16bit. + // tq: quant table destination id. + uint8_t pqtq = bd_consume_bits(&bd, 8); + + if ((pqtq & 0xf0) != 0 || (pqtq & 0x0f) >= 4) + return PJPEG_ERR_DQT; + + uint8_t id = pqtq & 3; + + for (int i = 0; i < 64; i++) + pjd->qtab[id][i] = bd_consume_bits(&bd, 8); + } + + break; + } + + case 0xc0: { // SOF, non-differential, huffman, baseline + uint16_t length = bd_consume_bits(&bd, 16); + (void) length; + + uint8_t p = bd_consume_bits(&bd, 8); // precision + if (p != 8) + return PJPEG_ERR_SOF; + + pjd->height = bd_consume_bits(&bd, 16); + pjd->width = bd_consume_bits(&bd, 16); + +// printf("%d x %d\n", pjd->height, pjd->width); + + int nf = bd_consume_bits(&bd, 8); // # image components + + if (nf < 1 || nf > 3) + return PJPEG_ERR_SOF; + + pjd->ncomponents = nf; + pjd->components = calloc(nf, sizeof(struct pjpeg_component)); + + for (int i = 0; i < nf; i++) { + // comp. identifier + pjd->components[i].id = bd_consume_bits(&bd, 8); + + // horiz/vert sampling + pjd->components[i].hv = bd_consume_bits(&bd, 8); + pjd->components[i].scaley = pjd->components[i].hv & 0x0f; + pjd->components[i].scalex = pjd->components[i].hv >> 4; + + // which quant table? + pjd->components[i].tq = bd_consume_bits(&bd, 8); + } + break; + } + + case 0xc1: // SOF, non-differential, huffman, extended DCT + case 0xc2: // SOF, non-differential, huffman, progressive DCT + case 0xc3: // SOF, non-differential, huffman, lossless + case 0xc5: // SOF, differential, huffman, baseline DCT + case 0xc6: // SOF, differential, huffman, progressive + case 0xc7: // SOF, differential, huffman, lossless + case 0xc8: // reserved + case 0xc9: // SOF, non-differential, arithmetic, extended + case 0xca: // SOF, non-differential, arithmetic, progressive + case 0xcb: // SOF, non-differential, arithmetic, lossless + case 0xcd: // SOF, differential, arithmetic, sequential + case 0xce: // SOF, differential, arithmetic, progressive + case 0xcf: // SOF, differential, arithmetic, lossless + { + printf("pjepg.c: unsupported JPEG type %02x\n", marker); + return PJEPG_ERR_UNSUPPORTED; + } + + case 0xc4: { // DHT Define Huffman Tables + // [ED: the encoding of these tables is really quite + // clever!] + uint16_t length = bd_consume_bits(&bd, 16); + length = length - 2; + + while (length > 0) { + uint8_t TcTh = bd_consume_bits(&bd, 8); + length--; + uint8_t Tc = (TcTh >> 4); + int Th = TcTh & 0x0f; // which index are we using? + + if (Tc >= 2 || Th >= 2) + // Tc must be either AC=1 or DC=0. + // Th must be less than 2 + return PJPEG_ERR_DHT; + + int htidx = Tc*2 + Th; + + uint8_t L[17]; // how many symbols of each bit length? + L[0] = 0; // no 0 bit codes :) + for (int nbits = 1; nbits <= 16; nbits++) { + L[nbits] = bd_consume_bits(&bd, 8); + length -= L[nbits]; + } + length -= 16; + + uint32_t code_pos = 0; + + for (int nbits = 1; nbits <= 16; nbits++) { + int nvalues = L[nbits]; + + // how many entries will we fill? + // (a 1 bit code will fill 32768, a 2 bit code 16384, ...) + uint32_t ncodes = (1 << (16 - nbits)); + + // consume the values... + for (int vi = 0; vi < nvalues; vi++) { + uint8_t code = bd_consume_bits(&bd, 8); + + if (code_pos + ncodes > 0xffff) + return PJPEG_ERR_DHT; + + for (int ci = 0; ci < ncodes; ci++) { + pjd->huff_codes[htidx][code_pos].nbits = nbits; + pjd->huff_codes[htidx][code_pos].code = code; + code_pos++; + } + } + } + pjd->huff_codes_present[htidx] = 1; + } + break; + } + + // a sequentially-encoded JPG has one SOS segment. A + // progressive JPG will have multiple SOS segments. + case 0xda: { // Start Of Scan (SOS) + + // Note that this marker frame (and its encoded + // length) does NOT include the bitstream that + // follows. + + uint16_t length = bd_consume_bits(&bd, 16); + (void) length; + + // number of components in this scan + uint8_t ns = bd_consume_bits(&bd, 8); + + // for each component, what is the index into our pjd->components[] array? + uint8_t *comp_idx = calloc(ns, sizeof(uint8_t)); + + for (int i = 0; i < ns; i++) { + // component name + uint8_t cs = bd_consume_bits(&bd, 8); + + int found = 0; + for (int j = 0; j < pjd->ncomponents; j++) { + + if (cs == pjd->components[j].id) { + // which huff tables will we use for + // DC (high 4 bits) and AC (low 4 bits) + pjd->components[j].tda = bd_consume_bits(&bd, 8); + comp_idx[i] = j; + found = 1; + break; + } + } + + if (!found) + return PJPEG_ERR_SOS; + } + + // start of spectral selection. baseline == 0 + uint8_t ss = bd_consume_bits(&bd, 8); + + // end of spectral selection. baseline == 0x3f + uint8_t se = bd_consume_bits(&bd, 8); + + // successive approximation bits. baseline == 0 + uint8_t Ahl = bd_consume_bits(&bd, 8); + + if (ss != 0 || se != 0x3f || Ahl != 0x00) + return PJPEG_ERR_SOS; + + // compute the dimensions of each MCU in pixels + int maxmcux = 0, maxmcuy = 0; + for (int i = 0; i < ns; i++) { + struct pjpeg_component *comp = &pjd->components[comp_idx[i]]; + + maxmcux = max_u8(maxmcux, comp->scalex * 8); + maxmcuy = max_u8(maxmcuy, comp->scaley * 8); + } + + // how many MCU blocks are required to encode the whole image? + int mcus_x = (pjd->width + maxmcux - 1) / maxmcux; + int mcus_y = (pjd->height + maxmcuy - 1) / maxmcuy; + + if (0) + printf("Image has %d x %d MCU blocks, each %d x %d pixels\n", + mcus_x, mcus_y, maxmcux, maxmcuy); + + // allocate output storage + for (int i = 0; i < ns; i++) { + struct pjpeg_component *comp = &pjd->components[comp_idx[i]]; + comp->width = mcus_x * comp->scalex * 8; + comp->height = mcus_y * comp->scaley * 8; + comp->stride = comp->width; + + int alignment = 32; + if ((comp->stride % alignment) != 0) + comp->stride += alignment - (comp->stride % alignment); + + comp->data = calloc(comp->height * comp->stride, 1); + } + + + // each component has its own DC prediction + int32_t *dcpred = calloc(ns, sizeof(int32_t)); + + pjd->reset_count = 0; + + for (int mcu_y = 0; mcu_y < mcus_y; mcu_y++) { + for (int mcu_x = 0; mcu_x < mcus_x; mcu_x++) { + + // the next two bytes in the input stream + // should be 0xff 0xdN, where N is the next + // reset counter. + // + // Our bit decoder may have already shifted + // these into the buffer. Consequently, we + // want to use our bit decoding functions to + // check for the marker. But we must first + // discard any fractional bits left. + if (pjd->reset_interval > 0 && pjd->reset_count == pjd->reset_interval) { + + // RST markers are byte-aligned, so force + // the bit-decoder to the next byte + // boundary. + bd_discard_to_byte_boundary(&bd); + + while (1) { + int32_t value = bd_consume_bits(&bd, 8); + if (bd.inpos > bd.inlen) + return PJPEG_ERR_EOF; + if (value == 0xff) + break; + printf("RST SYNC\n"); + } + + int32_t marker_32 = bd_consume_bits(&bd, 8); + +// printf("%04x: RESET? %02x\n", *bd.inpos, marker_32); + if (marker_32 != (0xd0 + pjd->reset_next)) + return PJPEG_ERR_RESET; + + pjd->reset_count = 0; + pjd->reset_next = (pjd->reset_next + 1) & 0x7; + + memset(dcpred, 0, sizeof(*dcpred)); + } + + for (int nsidx = 0; nsidx < ns; nsidx++) { + + struct pjpeg_component *comp = &pjd->components[comp_idx[nsidx]]; + + int32_t block[64]; + + int qtabidx = comp->tq; // which quant table? + + for (int sby = 0; sby < comp->scaley; sby++) { + for (int sbx = 0; sbx < comp->scalex; sbx++) { + // decode block for component nsidx + memset(block, 0, sizeof(block)); + + int dc_huff_table_idx = comp->tda >> 4; + int ac_huff_table_idx = 2 + (comp->tda & 0x0f); + + if (!pjd->huff_codes_present[dc_huff_table_idx] || + !pjd->huff_codes_present[ac_huff_table_idx]) + return PJPEG_ERR_MISSING_DHT; // probably an MJPEG. + + + if (1) { + // do DC coefficient + uint32_t next16 = bd_peek_bits(&bd, 16); + struct pjpeg_huffman_code *huff_code = &pjd->huff_codes[dc_huff_table_idx][next16]; + bd_consume_bits(&bd, huff_code->nbits); + + int ssss = huff_code->code & 0x0f; // ssss == number of additional bits to read + int32_t value = bd_consume_bits(&bd, ssss); + + // if high bit is clear, it's negative + if ((value & (1 << (ssss-1))) == 0) + value += (int32_t)(UINT32_MAX << ssss) + 1; + + dcpred[nsidx] += value; + block[0] = dcpred[nsidx] * pjd->qtab[qtabidx][0]; + } + + if (1) { + // do AC coefficients + for (int coeff = 1; coeff < 64; coeff++) { + + uint32_t next16 = bd_peek_bits(&bd, 16); + + struct pjpeg_huffman_code *huff_code = &pjd->huff_codes[ac_huff_table_idx][next16]; + bd_consume_bits(&bd, huff_code->nbits); + + if (huff_code->code == 0) { + break; // EOB + } + + int rrrr = huff_code->code >> 4; // run length of zeros + int ssss = huff_code->code & 0x0f; + + int32_t value = bd_consume_bits(&bd, ssss); + + // if high bit is clear, it's negative + if ((value & (1 << (ssss-1))) == 0) + value += (int32_t)(UINT32_MAX << ssss) + 1; + + coeff += rrrr; + + block[(int) ZZ[coeff]] = value * pjd->qtab[qtabidx][coeff]; + } + } + + // do IDCT + + // output block's upper-left + // coordinate (in pixels) is + // (comp_x, comp_y). + uint32_t comp_x = (mcu_x * comp->scalex + sbx) * 8; + uint32_t comp_y = (mcu_y * comp->scaley + sby) * 8; + uint32_t dataidx = comp_y * comp->stride + comp_x; + +// pjpeg_idct_2D_u32(block, &comp->data[dataidx], comp->stride); + pjpeg_idct_2D_nanojpeg(block, &comp->data[dataidx], comp->stride); + } + } + } + + pjd->reset_count++; +// printf("%04x: reset count %d / %d\n", pjd->inpos, pjd->reset_count, pjd->reset_interval); + + } + } + + free(dcpred); + free(comp_idx); + + break; + } + + case 0xd9: { // EOI End of Image + goto got_end_of_image; + } + + case 0xdd: { // Define Restart Interval + uint16_t length = bd_consume_bits(&bd, 16); + if (length != 4) + return PJPEG_ERR_DRI; + + // reset interval measured in the number of MCUs + pjd->reset_interval = bd_consume_bits(&bd, 16); + + break; + } + + default: { + printf("pjepg: Unknown marker %02x at offset %04x\n", marker, marker_offset); + + // try to skip it. + uint16_t length = bd_consume_bits(&bd, 16); + bd_discard_bytes(&bd, length - 2); + continue; + } + } // switch (marker) + } // while inpos < inlen + + got_end_of_image: + + return PJPEG_OKAY; +} + +void pjpeg_destroy(pjpeg_t *pj) +{ + if (!pj) + return; + + for (int i = 0; i < pj->ncomponents; i++) + free(pj->components[i].data); + free(pj->components); + + free(pj); +} + + +// just grab the first component. +image_u8_t *pjpeg_to_u8_baseline(pjpeg_t *pj) +{ + assert(pj->ncomponents > 0); + + pjpeg_component_t *comp = &pj->components[0]; + + assert(comp->width >= pj->width && comp->height >= pj->height); + + image_u8_t *im = image_u8_create(pj->width, pj->height); + for (int y = 0; y < im->height; y++) + memcpy(&im->buf[y*im->stride], &comp->data[y*comp->stride], pj->width); + + return im; +} + +static inline uint8_t clampd(double v) +{ + if (v < 0) + return 0; + if (v > 255) + return 255; + + return (uint8_t) v; +} + +static inline uint8_t clamp_u8(int32_t v) +{ + if (v < 0) + return 0; + if (v > 255) + return 255; + return v; +} + +// color conversion formulas taken from JFIF spec v 1.02 +image_u8x3_t *pjpeg_to_u8x3_baseline(pjpeg_t *pj) +{ + assert(pj->ncomponents == 3); + + pjpeg_component_t *Y = &pj->components[0]; + pjpeg_component_t *Cb = &pj->components[1]; + pjpeg_component_t *Cr = &pj->components[2]; + + int Cb_factor_y = Y->height / Cb->height; + int Cb_factor_x = Y->width / Cb->width; + + int Cr_factor_y = Y->height / Cr->height; + int Cr_factor_x = Y->width / Cr->width; + + image_u8x3_t *im = image_u8x3_create(pj->width, pj->height); + + if (Cr_factor_y == 1 && Cr_factor_x == 1 && Cb_factor_y == 1 && Cb_factor_x == 1) { + + for (int y = 0; y < pj->height; y++) { + for (int x = 0; x < pj->width; x++) { + int32_t y_val = Y->data[y*Y->stride + x] * 65536; + int32_t cb_val = Cb->data[y*Cb->stride + x] - 128; + int32_t cr_val = Cr->data[y*Cr->stride + x] - 128; + + int32_t r_val = y_val + 91881 * cr_val; + int32_t g_val = y_val + -22554 * cb_val - 46802 * cr_val; + int32_t b_val = y_val + 116130 * cb_val; + + im->buf[y*im->stride + 3*x + 0 ] = clamp_u8(r_val >> 16); + im->buf[y*im->stride + 3*x + 1 ] = clamp_u8(g_val >> 16); + im->buf[y*im->stride + 3*x + 2 ] = clamp_u8(b_val >> 16); + } + } + } else if (Cb_factor_y == Cr_factor_y && Cb_factor_x == Cr_factor_x) { + for (int by = 0; by < pj->height / Cb_factor_y; by++) { + for (int bx = 0; bx < pj->width / Cb_factor_x; bx++) { + + int32_t cb_val = Cb->data[by*Cb->stride + bx] - 128; + int32_t cr_val = Cr->data[by*Cr->stride + bx] - 128; + + int32_t r0 = 91881 * cr_val; + int32_t g0 = -22554 * cb_val - 46802 * cr_val; + int32_t b0 = 116130 * cb_val; + + for (int dy = 0; dy < Cb_factor_y; dy++) { + int y = by*Cb_factor_y + dy; + + for (int dx = 0; dx < Cb_factor_x; dx++) { + int x = bx*Cb_factor_x + dx; + + int32_t y_val = Y->data[y*Y->stride + x] * 65536; + + int32_t r_val = r0 + y_val; + int32_t g_val = g0 + y_val; + int32_t b_val = b0 + y_val; + + im->buf[y*im->stride + 3*x + 0 ] = clamp_u8(r_val >> 16); + im->buf[y*im->stride + 3*x + 1 ] = clamp_u8(g_val >> 16); + im->buf[y*im->stride + 3*x + 2 ] = clamp_u8(b_val >> 16); + } + } + } + } + } else { + + for (int y = 0; y < pj->height; y++) { + for (int x = 0; x < pj->width; x++) { + int32_t y_val = Y->data[y*Y->stride + x]; + int32_t cb_val = Cb->data[(y / Cb_factor_y)*Cb->stride + (x / Cb_factor_x)] - 128; + int32_t cr_val = Cr->data[(y / Cr_factor_y)*Cr->stride + (x / Cr_factor_x)] - 128; + + uint8_t r_val = clampd(y_val + 1.402 * cr_val); + uint8_t g_val = clampd(y_val - 0.34414 * cb_val - 0.71414 * cr_val); + uint8_t b_val = clampd(y_val + 1.772 * cb_val); + + im->buf[y*im->stride + 3*x + 0 ] = r_val; + im->buf[y*im->stride + 3*x + 1 ] = g_val; + im->buf[y*im->stride + 3*x + 2 ] = b_val; + } + } + } + + return im; +} + +/////////////////////////////////////////////////////////////////// +// returns NULL if file loading fails. +pjpeg_t *pjpeg_create_from_file(const char *path, uint32_t flags, int *error) +{ + FILE *f = fopen(path, "rb"); + if (f == NULL) + return NULL; + + fseek(f, 0, SEEK_END); + long buflen = ftell(f); + + uint8_t *buf = malloc(buflen); + fseek(f, 0, SEEK_SET); + int res = fread(buf, 1, buflen, f); + + if ( ferror(f) ){ + debug_print ("Read failed"); + clearerr(f); + } + + fclose(f); + if (res != buflen) { + free(buf); + if (error) + *error = PJPEG_ERR_FILE; + return NULL; + } + + pjpeg_t *pj = pjpeg_create_from_buffer(buf, buflen, flags, error); + + free(buf); + return pj; +} + +pjpeg_t *pjpeg_create_from_buffer(uint8_t *buf, int buflen, uint32_t flags, int *error) +{ + struct pjpeg_decode_state pjd; + memset(&pjd, 0, sizeof(pjd)); + + if (flags & PJPEG_MJPEG) { + pjd.in = mjpeg_dht; + pjd.inlen = sizeof(mjpeg_dht); + int result = pjpeg_decode_buffer(&pjd); + (void) result; + assert(result == 0); + } + + pjd.in = buf; + pjd.inlen = buflen; + pjd.flags = flags; + + int result = pjpeg_decode_buffer(&pjd); + if (error) + *error = result; + + if (result) { + for (int i = 0; i < pjd.ncomponents; i++) + free(pjd.components[i].data); + free(pjd.components); + + return NULL; + } + + pjpeg_t *pj = calloc(1, sizeof(pjpeg_t)); + + pj->width = pjd.width; + pj->height = pjd.height; + pj->ncomponents = pjd.ncomponents; + pj->components = pjd.components; + + return pj; +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/common/pnm.c b/apriltag/src/main/native/thirdparty/apriltag/src/common/pnm.c new file mode 100644 index 0000000000..fe77dde6d9 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/common/pnm.c @@ -0,0 +1,154 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#include +#include +#include + +#include "pnm.h" + +pnm_t *pnm_create_from_file(const char *path) +{ + FILE *f = fopen(path, "rb"); + if (f == NULL) + return NULL; + + pnm_t *pnm = calloc(1, sizeof(pnm_t)); + pnm->format = -1; + + char tmp[1024]; + int nparams = 0; // will be 3 when we're all done. + int params[3]; + + while (nparams < 3 && !(pnm->format == PNM_FORMAT_BINARY && nparams == 2)) { + if (fgets(tmp, sizeof(tmp), f) == NULL) + goto error; + + // skip comments + if (tmp[0]=='#') + continue; + + char *p = tmp; + + if (pnm->format == -1 && tmp[0]=='P') { + pnm->format = tmp[1]-'0'; + assert(pnm->format == PNM_FORMAT_GRAY || pnm->format == PNM_FORMAT_RGB || pnm->format == PNM_FORMAT_BINARY); + p = &tmp[2]; + } + + // pull integers out of this line until there are no more. + while (nparams < 3 && *p!=0) { + while (*p==' ') + p++; + + // encounter rubbish? (End of line?) + if (*p < '0' || *p > '9') + break; + + int acc = 0; + while (*p >= '0' && *p <= '9') { + acc = acc*10 + *p - '0'; + p++; + } + + params[nparams++] = acc; + p++; + } + } + + pnm->width = params[0]; + pnm->height = params[1]; + pnm->max = params[2]; + + switch (pnm->format) { + case PNM_FORMAT_BINARY: { + // files in the wild sometimes simply don't set max + pnm->max = 1; + + pnm->buflen = pnm->height * ((pnm->width + 7) / 8); + pnm->buf = malloc(pnm->buflen); + size_t len = fread(pnm->buf, 1, pnm->buflen, f); + if (len != pnm->buflen) + goto error; + + fclose(f); + return pnm; + } + + case PNM_FORMAT_GRAY: { + if (pnm->max == 255) + pnm->buflen = pnm->width * pnm->height; + else if (pnm->max == 65535) + pnm->buflen = 2 * pnm->width * pnm->height; + else + assert(0); + + pnm->buf = malloc(pnm->buflen); + size_t len = fread(pnm->buf, 1, pnm->buflen, f); + if (len != pnm->buflen) + goto error; + + fclose(f); + return pnm; + } + + case PNM_FORMAT_RGB: { + if (pnm->max == 255) + pnm->buflen = pnm->width * pnm->height * 3; + else if (pnm->max == 65535) + pnm->buflen = 2 * pnm->width * pnm->height * 3; + else + assert(0); + + pnm->buf = malloc(pnm->buflen); + size_t len = fread(pnm->buf, 1, pnm->buflen, f); + if (len != pnm->buflen) + goto error; + fclose(f); + return pnm; + } + } + +error: + fclose(f); + + if (pnm != NULL) { + free(pnm->buf); + free(pnm); + } + + return NULL; +} + +void pnm_destroy(pnm_t *pnm) +{ + if (pnm == NULL) + return; + + free(pnm->buf); + free(pnm); +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/common/pthreads_cross.cpp b/apriltag/src/main/native/thirdparty/apriltag/src/common/pthreads_cross.cpp new file mode 100644 index 0000000000..480e952183 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/common/pthreads_cross.cpp @@ -0,0 +1,259 @@ +/** +Copyright John Schember + +Permission is hereby granted, free of charge, to any person obtaining a copy of +this software and associated documentation files (the "Software"), to deal in +the Software without restriction, including without limitation the rights to +use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies +of the Software, and to permit persons to whom the Software is furnished to do +so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. + */ + +#include "common/pthreads_cross.h" +#include + +#ifdef _WIN32 + +typedef struct { + SRWLOCK lock; + bool exclusive; +} pthread_rwlock_t; + +int pthread_rwlock_init(pthread_rwlock_t *rwlock, const pthread_rwlockattr_t *attr); +int pthread_rwlock_destroy(pthread_rwlock_t *rwlock); +int pthread_rwlock_rdlock(pthread_rwlock_t *rwlock); +int pthread_rwlock_tryrdlock(pthread_rwlock_t *rwlock); +int pthread_rwlock_wrlock(pthread_rwlock_t *rwlock); +int pthread_rwlock_trywrlock(pthread_rwlock_t *rwlock); +int pthread_rwlock_unlock(pthread_rwlock_t *rwlock); + +int pthread_create(pthread_t *thread, pthread_attr_t *attr, void *(*start_routine)(void *), void *arg) +{ + (void) attr; + + if (thread == NULL || start_routine == NULL) + return 1; + + *thread = (HANDLE) CreateThread(NULL, 0, (LPTHREAD_START_ROUTINE)start_routine, arg, 0, NULL); + if (*thread == NULL) + return 1; + return 0; +} + +int pthread_join(pthread_t thread, void **value_ptr) +{ + (void)value_ptr; + WaitForSingleObject(thread, INFINITE); + CloseHandle(thread); + return 0; +} + +int pthread_detach(pthread_t thread) +{ + CloseHandle(thread); + return 0; +} + +int pthread_mutex_init(pthread_mutex_t *mutex, pthread_mutexattr_t *attr) +{ + (void)attr; + + if (mutex == NULL) + return 1; + + InitializeCriticalSection(mutex); + return 0; +} + +int pthread_mutex_destroy(pthread_mutex_t *mutex) +{ + if (mutex == NULL) + return 1; + DeleteCriticalSection(mutex); + return 0; +} + +int pthread_mutex_lock(pthread_mutex_t *mutex) +{ + if (mutex == NULL) + return 1; + EnterCriticalSection(mutex); + return 0; +} + +int pthread_mutex_unlock(pthread_mutex_t *mutex) +{ + if (mutex == NULL) + return 1; + LeaveCriticalSection(mutex); + return 0; +} + +int pthread_cond_init(pthread_cond_t *cond, pthread_condattr_t *attr) +{ + (void)attr; + if (cond == NULL) + return 1; + InitializeConditionVariable(cond); + return 0; +} + +int pthread_cond_destroy(pthread_cond_t *cond) +{ + /* Windows does not have a destroy for conditionals */ + (void)cond; + return 0; +} + +int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex) +{ + if (cond == NULL || mutex == NULL) + return 1; + return pthread_cond_timedwait(cond, mutex, NULL); +} + +int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex, + const struct timespec *abstime) +{ + if (cond == NULL || mutex == NULL) + return 1; + if (!SleepConditionVariableCS(cond, mutex, timespec_to_ms(abstime))) + return 1; + return 0; +} + +int pthread_cond_signal(pthread_cond_t *cond) +{ + if (cond == NULL) + return 1; + WakeConditionVariable(cond); + return 0; +} + +int pthread_cond_broadcast(pthread_cond_t *cond) +{ + if (cond == NULL) + return 1; + WakeAllConditionVariable(cond); + return 0; +} + +int pthread_rwlock_init(pthread_rwlock_t *rwlock, const pthread_rwlockattr_t *attr) +{ + (void)attr; + if (rwlock == NULL) + return 1; + InitializeSRWLock(&(rwlock->lock)); + rwlock->exclusive = false; + return 0; +} + +int pthread_rwlock_destroy(pthread_rwlock_t *rwlock) +{ + (void)rwlock; + return 0; +} + +int pthread_rwlock_rdlock(pthread_rwlock_t *rwlock) +{ + if (rwlock == NULL) + return 1; + AcquireSRWLockShared(&(rwlock->lock)); + return 0; +} + +int pthread_rwlock_tryrdlock(pthread_rwlock_t *rwlock) +{ + if (rwlock == NULL) + return 1; + return !TryAcquireSRWLockShared(&(rwlock->lock)); +} + +int pthread_rwlock_wrlock(pthread_rwlock_t *rwlock) +{ + if (rwlock == NULL) + return 1; + AcquireSRWLockExclusive(&(rwlock->lock)); + rwlock->exclusive = true; + return 0; +} + +int pthread_rwlock_trywrlock(pthread_rwlock_t *rwlock) +{ + BOOLEAN ret; + + if (rwlock == NULL) + return 1; + + ret = TryAcquireSRWLockExclusive(&(rwlock->lock)); + if (ret) + rwlock->exclusive = true; + return ret; +} + +int pthread_rwlock_unlock(pthread_rwlock_t *rwlock) +{ + if (rwlock == NULL) + return 1; + + if (rwlock->exclusive) { + rwlock->exclusive = false; + ReleaseSRWLockExclusive(&(rwlock->lock)); + } else { + ReleaseSRWLockShared(&(rwlock->lock)); + } + return 0; +} + +int sched_yield(void) { + return (int)SwitchToThread(); +} + +void ms_to_timespec(struct timespec *ts, unsigned int ms) +{ + if (ts == NULL) + return; + ts->tv_sec = (ms / 1000) + time(NULL); + ts->tv_nsec = (ms % 1000) * 1000000; +} + +unsigned int timespec_to_ms(const struct timespec *abstime) +{ + DWORD t; + + if (abstime == NULL) + return INFINITE; + + t = ((abstime->tv_sec - time(NULL)) * 1000) + (abstime->tv_nsec / 1000000); + if (t < 0) + t = 1; + return t; +} + +unsigned int pcthread_get_num_procs(void) +{ + SYSTEM_INFO sysinfo; + + GetSystemInfo(&sysinfo); + return sysinfo.dwNumberOfProcessors; +} + +#else + +#include +unsigned int pcthread_get_num_procs(void) +{ + return (unsigned int)sysconf(_SC_NPROCESSORS_ONLN); +} +#endif diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/common/string_util.c b/apriltag/src/main/native/thirdparty/apriltag/src/common/string_util.c new file mode 100644 index 0000000000..14726e7a7f --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/common/string_util.c @@ -0,0 +1,772 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#include +#include +#include +#include +#include +#include + +#include "string_util.h" +#include "zarray.h" + +struct string_buffer +{ + char *s; + int alloc; + size_t size; // as if strlen() was called; not counting terminating \0 +}; + +#define MIN_PRINTF_ALLOC 16 + +char *sprintf_alloc(const char *fmt, ...) +{ + assert(fmt != NULL); + + va_list args; + + va_start(args,fmt); + char *buf = vsprintf_alloc(fmt, args); + va_end(args); + + return buf; +} + +char *vsprintf_alloc(const char *fmt, va_list orig_args) +{ + assert(fmt != NULL); + + int size = MIN_PRINTF_ALLOC; + char *buf = malloc(size * sizeof(char)); + + int returnsize; + va_list args; + + va_copy(args, orig_args); + returnsize = vsnprintf(buf, size, fmt, args); + va_end(args); + + // it was successful + if (returnsize < size) { + return buf; + } + + // otherwise, we should try again + free(buf); + size = returnsize + 1; + buf = malloc(size * sizeof(char)); + + va_copy(args, orig_args); + returnsize = vsnprintf(buf, size, fmt, args); + va_end(args); + + assert(returnsize <= size); + return buf; +} + +char *_str_concat_private(const char *first, ...) +{ + size_t len = 0; + + // get the total length (for the allocation) + { + va_list args; + va_start(args, first); + const char *arg = first; + while(arg != NULL) { + len += strlen(arg); + arg = va_arg(args, const char *); + } + va_end(args); + } + + // write the string + char *str = malloc(len*sizeof(char) + 1); + char *ptr = str; + { + va_list args; + va_start(args, first); + const char *arg = first; + while(arg != NULL) { + while(*arg) + *ptr++ = *arg++; + arg = va_arg(args, const char *); + } + *ptr = '\0'; + va_end(args); + } + + return str; +} + +// Returns the index of the first character that differs: +int str_diff_idx(const char * a, const char * b) +{ + assert(a != NULL); + assert(b != NULL); + + int i = 0; + + size_t lena = strlen(a); + size_t lenb = strlen(b); + + size_t minlen = lena < lenb ? lena : lenb; + + for (; i < minlen; i++) + if (a[i] != b[i]) + break; + + return i; +} + + +zarray_t *str_split(const char *str, const char *delim) +{ + assert(str != NULL); + assert(delim != NULL); + + zarray_t *parts = zarray_create(sizeof(char*)); + string_buffer_t *sb = string_buffer_create(); + + size_t delim_len = strlen(delim); + size_t len = strlen(str); + size_t pos = 0; + + while (pos < len) { + if (str_starts_with(&str[pos], delim) && delim_len > 0) { + pos += delim_len; + // never add empty strings (repeated tokens) + if (string_buffer_size(sb) > 0) { + char *part = string_buffer_to_string(sb); + zarray_add(parts, &part); + } + string_buffer_reset(sb); + } else { + string_buffer_append(sb, str[pos]); + pos++; + } + } + + if (string_buffer_size(sb) > 0) { + char *part = string_buffer_to_string(sb); + zarray_add(parts, &part); + } + + string_buffer_destroy(sb); + return parts; +} + +// split on one or more spaces. +zarray_t *str_split_spaces(const char *str) +{ + zarray_t *parts = zarray_create(sizeof(char*)); + size_t len = strlen(str); + size_t pos = 0; + + while (pos < len) { + + while (pos < len && str[pos] == ' ') + pos++; + + // produce a token? + if (pos < len) { + // yes! + size_t off0 = pos; + while (pos < len && str[pos] != ' ') + pos++; + size_t off1 = pos; + + size_t len_off = off1 - off0; + char *tok = malloc(len_off + 1); + memcpy(tok, &str[off0], len_off); + tok[len_off] = 0; + zarray_add(parts, &tok); + } + } + + return parts; +} + +void str_split_destroy(zarray_t *za) +{ + if (!za) + return; + + zarray_vmap(za, free); + zarray_destroy(za); +} + +char *str_trim(char *str) +{ + assert(str != NULL); + + return str_lstrip(str_rstrip(str)); +} + +char *str_lstrip(char *str) +{ + assert(str != NULL); + + char *ptr = str; + char *end = str + strlen(str); + for(; ptr != end && isspace(*ptr); ptr++); + // shift the string to the left so the original pointer still works + memmove(str, ptr, strlen(ptr)+1); + return str; +} + +char *str_rstrip(char *str) +{ + assert(str != NULL); + + char *ptr = str + strlen(str) - 1; + for(; ptr+1 != str && isspace(*ptr); ptr--); + *(ptr+1) = '\0'; + return str; +} + +int str_indexof(const char *haystack, const char *needle) +{ + assert(haystack != NULL); + assert(needle != NULL); + + // use signed types for hlen/nlen because hlen - nlen can be negative. + int hlen = (int) strlen(haystack); + int nlen = (int) strlen(needle); + + if (nlen > hlen) return -1; + + for (int i = 0; i <= hlen - nlen; i++) { + if (!strncmp(&haystack[i], needle, nlen)) + return i; + } + + return -1; +} + +int str_last_indexof(const char *haystack, const char *needle) +{ + assert(haystack != NULL); + assert(needle != NULL); + + // use signed types for hlen/nlen because hlen - nlen can be negative. + int hlen = (int) strlen(haystack); + int nlen = (int) strlen(needle); + + int last_index = -1; + for (int i = 0; i <= hlen - nlen; i++) { + if (!strncmp(&haystack[i], needle, nlen)) + last_index = i; + } + + return last_index; +} + +// in-place modification. +char *str_tolowercase(char *s) +{ + assert(s != NULL); + + size_t slen = strlen(s); + for (int i = 0; i < slen; i++) { + if (s[i] >= 'A' && s[i] <= 'Z') + s[i] = s[i] + 'a' - 'A'; + } + + return s; +} + +char *str_touppercase(char *s) +{ + assert(s != NULL); + + size_t slen = strlen(s); + for (int i = 0; i < slen; i++) { + if (s[i] >= 'a' && s[i] <= 'z') + s[i] = s[i] - ('a' - 'A'); + } + + return s; +} + +string_buffer_t* string_buffer_create(void) +{ + string_buffer_t *sb = (string_buffer_t*) calloc(1, sizeof(string_buffer_t)); + assert(sb != NULL); + sb->alloc = 32; + sb->s = calloc(sb->alloc, 1); + return sb; +} + +void string_buffer_destroy(string_buffer_t *sb) +{ + if (sb == NULL) + return; + + if (sb->s) + free(sb->s); + + memset(sb, 0, sizeof(string_buffer_t)); + free(sb); +} + +void string_buffer_append(string_buffer_t *sb, char c) +{ + assert(sb != NULL); + + if (sb->size+2 >= sb->alloc) { + sb->alloc *= 2; + sb->s = realloc(sb->s, sb->alloc); + } + + sb->s[sb->size++] = c; + sb->s[sb->size] = 0; +} + +char string_buffer_pop_back(string_buffer_t *sb) { + assert(sb != NULL); + if (sb->size == 0) + return 0; + + char back = sb->s[--sb->size]; + sb->s[sb->size] = 0; + return back; +} + +void string_buffer_appendf(string_buffer_t *sb, const char *fmt, ...) +{ + assert(sb != NULL); + assert(fmt != NULL); + + int size = MIN_PRINTF_ALLOC; + char *buf = malloc(size * sizeof(char)); + + int returnsize; + va_list args; + + va_start(args,fmt); + returnsize = vsnprintf(buf, size, fmt, args); + va_end(args); + + if (returnsize >= size) { + // otherwise, we should try again + free(buf); + size = returnsize + 1; + buf = malloc(size * sizeof(char)); + + va_start(args, fmt); + returnsize = vsnprintf(buf, size, fmt, args); + va_end(args); + + assert(returnsize <= size); + } + + string_buffer_append_string(sb, buf); + free(buf); +} + +void string_buffer_append_string(string_buffer_t *sb, const char *str) +{ + assert(sb != NULL); + assert(str != NULL); + + size_t len = strlen(str); + + while (sb->size+len + 1 >= sb->alloc) { + sb->alloc *= 2; + sb->s = realloc(sb->s, sb->alloc); + } + + memcpy(&sb->s[sb->size], str, len); + sb->size += len; + sb->s[sb->size] = 0; +} + +bool string_buffer_ends_with(string_buffer_t *sb, const char *str) +{ + assert(sb != NULL); + assert(str != NULL); + + return str_ends_with(sb->s, str); +} + +char *string_buffer_to_string(string_buffer_t *sb) +{ + assert(sb != NULL); + + return strdup(sb->s); +} + +// returns length of string (not counting \0) +size_t string_buffer_size(string_buffer_t *sb) +{ + assert(sb != NULL); + + return sb->size; +} + +void string_buffer_reset(string_buffer_t *sb) +{ + assert(sb != NULL); + + sb->s[0] = 0; + sb->size = 0; +} + +string_feeder_t *string_feeder_create(const char *str) +{ + assert(str != NULL); + + string_feeder_t *sf = (string_feeder_t*) calloc(1, sizeof(string_feeder_t)); + sf->s = strdup(str); + sf->len = strlen(sf->s); + sf->line = 1; + sf->col = 0; + sf->pos = 0; + return sf; +} + +int string_feeder_get_line(string_feeder_t *sf) +{ + assert(sf != NULL); + return sf->line; +} + +int string_feeder_get_column(string_feeder_t *sf) +{ + assert(sf != NULL); + return sf->col; +} + +void string_feeder_destroy(string_feeder_t *sf) +{ + if (sf == NULL) + return; + + free(sf->s); + memset(sf, 0, sizeof(string_feeder_t)); + free(sf); +} + +bool string_feeder_has_next(string_feeder_t *sf) +{ + assert(sf != NULL); + + return sf->s[sf->pos] != 0 && sf->pos <= sf->len; +} + +char string_feeder_next(string_feeder_t *sf) +{ + assert(sf != NULL); + assert(sf->pos <= sf->len); + + char c = sf->s[sf->pos++]; + if (c == '\n') { + sf->line++; + sf->col = 0; + } else { + sf->col++; + } + + return c; +} + +char *string_feeder_next_length(string_feeder_t *sf, size_t length) +{ + assert(sf != NULL); + assert(length >= 0); + assert(sf->pos <= sf->len); + + if (sf->pos + length > sf->len) + length = sf->len - sf->pos; + + char *substr = calloc(length+1, sizeof(char)); + for (int i = 0 ; i < length ; i++) + substr[i] = string_feeder_next(sf); + return substr; +} + +char string_feeder_peek(string_feeder_t *sf) +{ + assert(sf != NULL); + assert(sf->pos <= sf->len); + + return sf->s[sf->pos]; +} + +char *string_feeder_peek_length(string_feeder_t *sf, size_t length) +{ + assert(sf != NULL); + assert(length >= 0); + assert(sf->pos <= sf->len); + + if (sf->pos + length > sf->len) + length = sf->len - sf->pos; + + char *substr = calloc(length+1, sizeof(char)); + memcpy(substr, &sf->s[sf->pos], length*sizeof(char)); + return substr; +} + +bool string_feeder_starts_with(string_feeder_t *sf, const char *str) +{ + assert(sf != NULL); + assert(str != NULL); + assert(sf->pos <= sf->len); + + return str_starts_with(&sf->s[sf->pos], str); +} + +void string_feeder_require(string_feeder_t *sf, const char *str) +{ + assert(sf != NULL); + assert(str != NULL); + assert(sf->pos <= sf->len); + + size_t len = strlen(str); + + for (int i = 0; i < len; i++) { + char c = string_feeder_next(sf); + (void) c; + assert(c == str[i]); + } +} + +//////////////////////////////////////////// +bool str_ends_with(const char *haystack, const char *needle) +{ + assert(haystack != NULL); + assert(needle != NULL); + + size_t lens = strlen(haystack); + size_t lenneedle = strlen(needle); + + if (lenneedle > lens) + return false; + + return !strncmp(&haystack[lens - lenneedle], needle, lenneedle); +} + +#ifndef _MSC_VER +inline +#endif +bool str_starts_with(const char *haystack, const char *needle) +{ + assert(haystack != NULL); + assert(needle != NULL); + + // haystack[pos] doesn't have to be compared to zero; if it were + // zero, it either doesn't match needle (in which case the loop + // terminates) or it matches needle[pos] (in which case the loop + // terminates). + int pos = 0; + while (haystack[pos] == needle[pos] && needle[pos] != 0) + pos++; + + return (needle[pos] == 0); +} + +bool str_starts_with_any(const char *haystack, const char **needles, int num_needles) +{ + assert(haystack != NULL); + assert(needles != NULL); + assert(num_needles >= 0); + + for (int i = 0; i < num_needles; i++) { + assert(needles[i] != NULL); + if (str_starts_with(haystack, needles[i])) + return true; + } + + return false; +} + +bool str_matches_any(const char *haystack, const char **needles, int num_needles) +{ + assert(haystack != NULL); + assert(needles != NULL); + assert(num_needles >= 0); + + for (int i = 0; i < num_needles; i++) { + assert(needles[i] != NULL); + if (!strcmp(haystack, needles[i])) + return true; + } + + return false; +} + +char *str_substring(const char *str, size_t startidx, long endidx) +{ + assert(str != NULL); + assert(startidx >= 0 && startidx <= strlen(str)+1); + assert(endidx < 0 || endidx >= startidx); + assert(endidx < 0 || endidx <= strlen(str)+1); + + if (endidx < 0) + endidx = (long) strlen(str); + + size_t blen = endidx - startidx; // not counting \0 + char *b = malloc(blen + 1); + memcpy(b, &str[startidx], blen); + b[blen] = 0; + return b; +} + +char *str_replace(const char *haystack, const char *needle, const char *replacement) +{ + assert(haystack != NULL); + assert(needle != NULL); + assert(replacement != NULL); + + string_buffer_t *sb = string_buffer_create(); + size_t haystack_len = strlen(haystack); + size_t needle_len = strlen(needle); + + size_t pos = 0; + while (pos < haystack_len) { + if (needle_len > 0 && str_starts_with(&haystack[pos], needle)) { + string_buffer_append_string(sb, replacement); + pos += needle_len; + } else { + string_buffer_append(sb, haystack[pos]); + pos++; + } + } + if (needle_len == 0 && haystack_len == 0) + string_buffer_append_string(sb, replacement); + + char *res = string_buffer_to_string(sb); + string_buffer_destroy(sb); + return res; +} + +char *str_replace_many(const char *_haystack, ...) +{ + va_list ap; + va_start(ap, _haystack); + + char *haystack = strdup(_haystack); + + while (true) { + char *needle = va_arg(ap, char*); + if (!needle) + break; + + char *replacement = va_arg(ap, char*); + char *tmp = str_replace(haystack, needle, replacement); + free(haystack); + haystack = tmp; + } + + va_end(ap); + + return haystack; +} + +static void buffer_appendf(char **_buf, int *bufpos, void *fmt, ...) +{ + char *buf = *_buf; + va_list ap; + + int salloc = 128; + char *s = malloc(salloc); + + va_start(ap, fmt); + int slen = vsnprintf(s, salloc, fmt, ap); + va_end(ap); + + if (slen >= salloc) { + s = realloc(s, slen + 1); + va_start(ap, fmt); + vsprintf((char*) s, fmt, ap); + va_end(ap); + } + + buf = realloc(buf, *bufpos + slen + 1); + *_buf = buf; + + memcpy(&buf[*bufpos], s, slen + 1); // get trailing \0 + (*bufpos) += slen; + + free(s); +} + +static int is_variable_character(char c) +{ + if (c >= 'a' && c <= 'z') + return 1; + + if (c >= 'A' && c <= 'Z') + return 1; + + if (c >= '0' && c <= '9') + return 1; + + if (c == '_') + return 1; + + return 0; +} + +char *str_expand_envs(const char *in) +{ + size_t inlen = strlen(in); + size_t inpos = 0; + + char *out = NULL; + int outpos = 0; + + while (inpos < inlen) { + + if (in[inpos] != '$') { + buffer_appendf(&out, &outpos, "%c", in[inpos]); + inpos++; + continue; + + } else { + inpos++; // consume '$' + + char *varname = NULL; + int varnamepos = 0; + + while (inpos < inlen && is_variable_character(in[inpos])) { + buffer_appendf(&varname, &varnamepos, "%c", in[inpos]); + inpos++; + } + + char *env = getenv(varname); + if (env) + buffer_appendf(&out, &outpos, "%s", env); + + free(varname); + } + } + + return out; +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/common/svd22.c b/apriltag/src/main/native/thirdparty/apriltag/src/common/svd22.c new file mode 100644 index 0000000000..ba7486ddf4 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/common/svd22.c @@ -0,0 +1,258 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#include + +/** SVD 2x2. + + Computes singular values and vectors without squaring the input + matrix. With double precision math, results are accurate to about + 1E-16. + + U = [ cos(theta) -sin(theta) ] + [ sin(theta) cos(theta) ] + + S = [ e 0 ] + [ 0 f ] + + V = [ cos(phi) -sin(phi) ] + [ sin(phi) cos(phi) ] + + + Our strategy is basically to analytically multiply everything out + and then rearrange so that we can solve for theta, phi, e, and + f. (Derivation by ebolson@umich.edu 5/2016) + + V' = [ CP SP ] + [ -SP CP ] + +USV' = [ CT -ST ][ e*CP e*SP ] + [ ST CT ][ -f*SP f*CP ] + + = [e*CT*CP + f*ST*SP e*CT*SP - f*ST*CP ] + [e*ST*CP - f*SP*CT e*SP*ST + f*CP*CT ] + +A00+A11 = e*CT*CP + f*ST*SP + e*SP*ST + f*CP*CT + = e*(CP*CT + SP*ST) + f*(SP*ST + CP*CT) + = (e+f)(CP*CT + SP*ST) +B0 = (e+f)*cos(P-T) + +A00-A11 = e*CT*CP + f*ST*SP - e*SP*ST - f*CP*CT + = e*(CP*CT - SP*ST) - f*(-ST*SP + CP*CT) + = (e-f)(CP*CT - SP*ST) +B1 = (e-f)*cos(P+T) + +A01+A10 = e*CT*SP - f*ST*CP + e*ST*CP - f*SP*CT + = e(CT*SP + ST*CP) - f*(ST*CP + SP*CT) + = (e-f)*(CT*SP + ST*CP) +B2 = (e-f)*sin(P+T) + +A01-A10 = e*CT*SP - f*ST*CP - e*ST*CP + f*SP*CT + = e*(CT*SP - ST*CP) + f(SP*CT - ST*CP) + = (e+f)*(CT*SP - ST*CP) +B3 = (e+f)*sin(P-T) + +B0 = (e+f)*cos(P-T) +B1 = (e-f)*cos(P+T) +B2 = (e-f)*sin(P+T) +B3 = (e+f)*sin(P-T) + +B3/B0 = tan(P-T) + +B2/B1 = tan(P+T) + **/ +void svd22(const double A[4], double U[4], double S[2], double V[4]) +{ + double A00 = A[0]; + double A01 = A[1]; + double A10 = A[2]; + double A11 = A[3]; + + double B0 = A00 + A11; + double B1 = A00 - A11; + double B2 = A01 + A10; + double B3 = A01 - A10; + + double PminusT = atan2(B3, B0); + double PplusT = atan2(B2, B1); + + double P = (PminusT + PplusT) / 2; + double T = (-PminusT + PplusT) / 2; + + double CP = cos(P), SP = sin(P); + double CT = cos(T), ST = sin(T); + + U[0] = CT; + U[1] = -ST; + U[2] = ST; + U[3] = CT; + + V[0] = CP; + V[1] = -SP; + V[2] = SP; + V[3] = CP; + + // C0 = e+f. There are two ways to compute C0; we pick the one + // that is better conditioned. + double CPmT = cos(P-T), SPmT = sin(P-T); + double C0 = 0; + if (fabs(CPmT) > fabs(SPmT)) + C0 = B0 / CPmT; + else + C0 = B3 / SPmT; + + // C1 = e-f. There are two ways to compute C1; we pick the one + // that is better conditioned. + double CPpT = cos(P+T), SPpT = sin(P+T); + double C1 = 0; + if (fabs(CPpT) > fabs(SPpT)) + C1 = B1 / CPpT; + else + C1 = B2 / SPpT; + + // e and f are the singular values + double e = (C0 + C1) / 2; + double f = (C0 - C1) / 2; + + if (e < 0) { + e = -e; + U[0] = -U[0]; + U[2] = -U[2]; + } + + if (f < 0) { + f = -f; + U[1] = -U[1]; + U[3] = -U[3]; + } + + // sort singular values. + if (e > f) { + // already in big-to-small order. + S[0] = e; + S[1] = f; + } else { + // Curiously, this code never seems to get invoked. Why is it + // that S[0] always ends up the dominant vector? However, + // this code has been tested (flipping the logic forces us to + // sort the singular values in ascending order). + // + // P = [ 0 1 ; 1 0 ] + // USV' = (UP)(PSP)(PV') + // = (UP)(PSP)(VP)' + // = (UP)(PSP)(P'V')' + S[0] = f; + S[1] = e; + + // exchange columns of U and V + double tmp[2]; + tmp[0] = U[0]; + tmp[1] = U[2]; + U[0] = U[1]; + U[2] = U[3]; + U[1] = tmp[0]; + U[3] = tmp[1]; + + tmp[0] = V[0]; + tmp[1] = V[2]; + V[0] = V[1]; + V[2] = V[3]; + V[1] = tmp[0]; + V[3] = tmp[1]; + } + + /* + double SM[4] = { S[0], 0, 0, S[1] }; + + doubles_print_mat(U, 2, 2, "%20.10g"); + doubles_print_mat(SM, 2, 2, "%20.10g"); + doubles_print_mat(V, 2, 2, "%20.10g"); + printf("A:\n"); + doubles_print_mat(A, 2, 2, "%20.10g"); + + double SVt[4]; + doubles_mat_ABt(SM, 2, 2, V, 2, 2, SVt, 2, 2); + double USVt[4]; + doubles_mat_AB(U, 2, 2, SVt, 2, 2, USVt, 2, 2); + + printf("USVt\n"); + doubles_print_mat(USVt, 2, 2, "%20.10g"); + + double diff[4]; + for (int i = 0; i < 4; i++) + diff[i] = A[i] - USVt[i]; + + printf("diff\n"); + doubles_print_mat(diff, 2, 2, "%20.10g"); + + */ + +} + + +// for the matrix [a b; b d] +void svd_sym_singular_values(double A00, double A01, double A11, + double *Lmin, double *Lmax) +{ + double A10 = A01; + + double B0 = A00 + A11; + double B1 = A00 - A11; + double B2 = A01 + A10; + double B3 = A01 - A10; + + double PminusT = atan2(B3, B0); + double PplusT = atan2(B2, B1); + + double P = (PminusT + PplusT) / 2; + double T = (-PminusT + PplusT) / 2; + + // C0 = e+f. There are two ways to compute C0; we pick the one + // that is better conditioned. + double CPmT = cos(P-T), SPmT = sin(P-T); + double C0 = 0; + if (fabs(CPmT) > fabs(SPmT)) + C0 = B0 / CPmT; + else + C0 = B3 / SPmT; + + // C1 = e-f. There are two ways to compute C1; we pick the one + // that is better conditioned. + double CPpT = cos(P+T), SPpT = sin(P+T); + double C1 = 0; + if (fabs(CPpT) > fabs(SPpT)) + C1 = B1 / CPpT; + else + C1 = B2 / SPpT; + + // e and f are the singular values + double e = (C0 + C1) / 2; + double f = (C0 - C1) / 2; + + *Lmin = fmin(e, f); + *Lmax = fmax(e, f); +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/common/time_util.c b/apriltag/src/main/native/thirdparty/apriltag/src/common/time_util.c new file mode 100644 index 0000000000..f3e3b0849b --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/common/time_util.c @@ -0,0 +1,162 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#include +#include +#include "time_util.h" + +struct timeutil_rest +{ + int64_t acc_time; + int64_t start_time; +}; + +timeutil_rest_t *timeutil_rest_create(void) +{ + timeutil_rest_t *rest = calloc(1, sizeof(timeutil_rest_t)); + return rest; +} + +void timeutil_rest_destroy(timeutil_rest_t *rest) +{ + free(rest); +} + +int64_t utime_now(void) // blacklist-ignore +{ + struct timeval tv; + gettimeofday (&tv, NULL); // blacklist-ignore + return (int64_t) tv.tv_sec * 1000000 + tv.tv_usec; +} + +int64_t utime_get_seconds(int64_t v) +{ + return v/1000000; +} + +int64_t utime_get_useconds(int64_t v) +{ + return v%1000000; +} + +void utime_to_timeval(int64_t v, struct timeval *tv) +{ + tv->tv_sec = (time_t) utime_get_seconds(v); + tv->tv_usec = (suseconds_t) utime_get_useconds(v); +} + +void utime_to_timespec(int64_t v, struct timespec *ts) +{ + ts->tv_sec = (time_t) utime_get_seconds(v); + ts->tv_nsec = (suseconds_t) utime_get_useconds(v)*1000; +} + +int32_t timeutil_usleep(int64_t useconds) +{ +#ifdef _WIN32 + Sleep(useconds/1000); + return 0; +#else + // unistd.h function, but usleep is obsoleted in POSIX.1-2008. + // TODO: Eventually, rewrite this to use nanosleep + return usleep(useconds); +#endif +} + +uint32_t timeutil_sleep(unsigned int seconds) +{ +#ifdef _WIN32 + Sleep(seconds*1000); + return 0; +#else + // unistd.h function + return sleep(seconds); +#endif +} + +int32_t timeutil_sleep_hz(timeutil_rest_t *rest, double hz) +{ + int64_t max_delay = 1000000L/hz; + int64_t curr_time = utime_now(); + int64_t diff = curr_time - rest->start_time; + int64_t delay = max_delay - diff; + if (delay < 0) delay = 0; + + int32_t ret = timeutil_usleep(delay); + rest->start_time = utime_now(); + + return ret; +} + +void timeutil_timer_reset(timeutil_rest_t *rest) +{ + rest->start_time = utime_now(); + rest->acc_time = 0; +} + +void timeutil_timer_start(timeutil_rest_t *rest) +{ + rest->start_time = utime_now(); +} + +void timeutil_timer_stop(timeutil_rest_t *rest) +{ + int64_t curr_time = utime_now(); + int64_t diff = curr_time - rest->start_time; + + rest->acc_time += diff; +} + +bool timeutil_timer_timeout(timeutil_rest_t *rest, double timeout_s) +{ + int64_t timeout_us = (int64_t)(1000000L*timeout_s); + return rest->acc_time > timeout_us; +} + +int64_t time_util_hhmmss_ss_to_utime(double time) +{ + int64_t utime = 0; + + int itime = ((int) time); + + double seconds = fmod(time, 100.0); + uint8_t minutes = (itime % 10000) / 100; + uint8_t hours = itime / 10000; + + utime += seconds * 100; + utime += minutes * 6000; + utime += hours *360000; + + utime *= 10000; + + return utime; +} + +int64_t timeutil_ms_to_us(int32_t ms) +{ + return ((int64_t) ms) * 1000; +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/common/unionfind.c b/apriltag/src/main/native/thirdparty/apriltag/src/common/unionfind.c new file mode 100644 index 0000000000..ae429910e6 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/common/unionfind.c @@ -0,0 +1,30 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#include "unionfind.h" +#include +#include diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/common/workerpool.c b/apriltag/src/main/native/thirdparty/apriltag/src/common/workerpool.c new file mode 100644 index 0000000000..23415eb561 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/common/workerpool.c @@ -0,0 +1,215 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ +#include + +#define _GNU_SOURCE // Possible fix for 16.04 +#define __USE_GNU +#include "common/pthreads_cross.h" +#include +#include +#include +#ifdef _WIN32 +#include +#else +#include +#endif + +#include "workerpool.h" +#include "debug_print.h" + +struct workerpool { + int nthreads; + zarray_t *tasks; + int taskspos; + + pthread_t *threads; + int *status; + + pthread_mutex_t mutex; + pthread_cond_t startcond; // used to signal the availability of work + pthread_cond_t endcond; // used to signal completion of all work + + int end_count; // how many threads are done? +}; + +struct task +{ + void (*f)(void *p); + void *p; +}; + +void *worker_thread(void *p) +{ + workerpool_t *wp = (workerpool_t*) p; + +// int cnt = 0; + + while (1) { + struct task *task; + + pthread_mutex_lock(&wp->mutex); + while (wp->taskspos == zarray_size(wp->tasks)) { + wp->end_count++; +// printf("%"PRId64" thread %d did %d\n", utime_now(), pthread_self(), cnt); + pthread_cond_broadcast(&wp->endcond); + pthread_cond_wait(&wp->startcond, &wp->mutex); +// cnt = 0; +// printf("%"PRId64" thread %d awake\n", utime_now(), pthread_self()); + } + + zarray_get_volatile(wp->tasks, wp->taskspos, &task); + wp->taskspos++; +// cnt++; + pthread_mutex_unlock(&wp->mutex); +// pthread_yield(); + sched_yield(); + + // we've been asked to exit. + if (task->f == NULL) + return NULL; + + task->f(task->p); + } + + return NULL; +} + +workerpool_t *workerpool_create(int nthreads) +{ + assert(nthreads > 0); + + workerpool_t *wp = calloc(1, sizeof(workerpool_t)); + wp->nthreads = nthreads; + wp->tasks = zarray_create(sizeof(struct task)); + + if (nthreads > 1) { + wp->threads = calloc(wp->nthreads, sizeof(pthread_t)); + + pthread_mutex_init(&wp->mutex, NULL); + pthread_cond_init(&wp->startcond, NULL); + pthread_cond_init(&wp->endcond, NULL); + + for (int i = 0; i < nthreads; i++) { + int res = pthread_create(&wp->threads[i], NULL, worker_thread, wp); + if (res != 0) { + debug_print("Insufficient system resources to create workerpool threads\n"); + // errno already set to EAGAIN by pthread_create() failure + return NULL; + } + } + } + + return wp; +} + +void workerpool_destroy(workerpool_t *wp) +{ + if (wp == NULL) + return; + + // force all worker threads to exit. + if (wp->nthreads > 1) { + for (int i = 0; i < wp->nthreads; i++) + workerpool_add_task(wp, NULL, NULL); + + pthread_mutex_lock(&wp->mutex); + pthread_cond_broadcast(&wp->startcond); + pthread_mutex_unlock(&wp->mutex); + + for (int i = 0; i < wp->nthreads; i++) + pthread_join(wp->threads[i], NULL); + + pthread_mutex_destroy(&wp->mutex); + pthread_cond_destroy(&wp->startcond); + pthread_cond_destroy(&wp->endcond); + free(wp->threads); + } + + zarray_destroy(wp->tasks); + free(wp); +} + +int workerpool_get_nthreads(workerpool_t *wp) +{ + return wp->nthreads; +} + +void workerpool_add_task(workerpool_t *wp, void (*f)(void *p), void *p) +{ + struct task t; + t.f = f; + t.p = p; + + zarray_add(wp->tasks, &t); +} + +void workerpool_run_single(workerpool_t *wp) +{ + for (int i = 0; i < zarray_size(wp->tasks); i++) { + struct task *task; + zarray_get_volatile(wp->tasks, i, &task); + task->f(task->p); + } + + zarray_clear(wp->tasks); +} + +// runs all added tasks, waits for them to complete. +void workerpool_run(workerpool_t *wp) +{ + if (wp->nthreads > 1) { + wp->end_count = 0; + + pthread_mutex_lock(&wp->mutex); + pthread_cond_broadcast(&wp->startcond); + + while (wp->end_count < wp->nthreads) { +// printf("caught %d\n", wp->end_count); + pthread_cond_wait(&wp->endcond, &wp->mutex); + } + + pthread_mutex_unlock(&wp->mutex); + + wp->taskspos = 0; + + zarray_clear(wp->tasks); + + } else { + workerpool_run_single(wp); + } +} + +int workerpool_get_nprocs(void) +{ +#ifdef WIN32 + SYSTEM_INFO sysinfo; + GetSystemInfo(&sysinfo); + return sysinfo.dwNumberOfProcessors; +#else + return sysconf (_SC_NPROCESSORS_ONLN); +#endif +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/common/zarray.c b/apriltag/src/main/native/thirdparty/apriltag/src/common/zarray.c new file mode 100644 index 0000000000..fa1f8a25b6 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/common/zarray.c @@ -0,0 +1,55 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#include +#include + +#include "zarray.h" + +int zstrcmp(const void * a_pp, const void * b_pp) +{ + assert(a_pp != NULL); + assert(b_pp != NULL); + + char * a = *(void**)a_pp; + char * b = *(void**)b_pp; + + return strcmp(a,b); +} + +void zarray_vmap(zarray_t *za, void (*f)(void*)) +{ + assert(za != NULL); + assert(f != NULL); + assert(za->el_sz == sizeof(void*)); + + for (int idx = 0; idx < za->size; idx++) { + void *pp = &za->data[idx*za->el_sz]; + void *p = *(void**) pp; + f(p); + } +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/common/zhash.c b/apriltag/src/main/native/thirdparty/apriltag/src/common/zhash.c new file mode 100644 index 0000000000..10eb6eb8a6 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/common/zhash.c @@ -0,0 +1,562 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#include +#include +#include +#include + +#include "zhash.h" + +// force a rehash when our capacity is less than this many times the size +#define ZHASH_FACTOR_CRITICAL 2 + +// When resizing, how much bigger do we want to be? (should be greater than _CRITICAL) +#define ZHASH_FACTOR_REALLOC 4 + +struct zhash +{ + size_t keysz, valuesz; + int entrysz; // valid byte (1) + keysz + values + + uint32_t(*hash)(const void *a); + + // returns 1 if equal + int(*equals)(const void *a, const void *b); + + int size; // # of items in hash table + + char *entries; // each entry of size entrysz; + int nentries; // how many entries are allocated? Never 0. +}; + +zhash_t *zhash_create_capacity(size_t keysz, size_t valuesz, + uint32_t(*hash)(const void *a), int(*equals)(const void *a, const void*b), + int capacity) +{ + assert(hash != NULL); + assert(equals != NULL); + + // resize... + int _nentries = ZHASH_FACTOR_REALLOC * capacity; + if (_nentries < 8) + _nentries = 8; + + // to a power of 2. + int nentries = _nentries; + if ((nentries & (nentries - 1)) != 0) { + nentries = 8; + while (nentries < _nentries) + nentries *= 2; + } + + zhash_t *zh = (zhash_t*) calloc(1, sizeof(zhash_t)); + zh->keysz = keysz; + zh->valuesz = valuesz; + zh->hash = hash; + zh->equals = equals; + zh->nentries = nentries; + + zh->entrysz = 1 + zh->keysz + zh->valuesz; + + zh->entries = calloc(zh->nentries, zh->entrysz); + + return zh; +} + +zhash_t *zhash_create(size_t keysz, size_t valuesz, + uint32_t(*hash)(const void *a), int(*equals)(const void *a, const void *b)) +{ + return zhash_create_capacity(keysz, valuesz, hash, equals, 8); +} + +void zhash_destroy(zhash_t *zh) +{ + if (zh == NULL) + return; + + free(zh->entries); + free(zh); +} + +int zhash_size(const zhash_t *zh) +{ + return zh->size; +} + +void zhash_clear(zhash_t *zh) +{ + memset(zh->entries, 0, zh->nentries * zh->entrysz); + zh->size = 0; +} + +int zhash_get_volatile(const zhash_t *zh, const void *key, void *out_value) +{ + uint32_t code = zh->hash(key); + uint32_t entry_idx = code & (zh->nentries - 1); + + while (zh->entries[entry_idx * zh->entrysz]) { + void *this_key = &zh->entries[entry_idx * zh->entrysz + 1]; + if (zh->equals(key, this_key)) { + *((void**) out_value) = &zh->entries[entry_idx * zh->entrysz + 1 + zh->keysz]; + return 1; + } + + entry_idx = (entry_idx + 1) & (zh->nentries - 1); + } + + return 0; +} + +int zhash_get(const zhash_t *zh, const void *key, void *out_value) +{ + void *tmp; + if (zhash_get_volatile(zh, key, &tmp)) { + memcpy(out_value, tmp, zh->valuesz); + return 1; + } + + return 0; +} + +int zhash_put(zhash_t *zh, const void *key, const void *value, void *oldkey, void *oldvalue) +{ + uint32_t code = zh->hash(key); + uint32_t entry_idx = code & (zh->nentries - 1); + + while (zh->entries[entry_idx * zh->entrysz]) { + void *this_key = &zh->entries[entry_idx * zh->entrysz + 1]; + void *this_value = &zh->entries[entry_idx * zh->entrysz + 1 + zh->keysz]; + + if (zh->equals(key, this_key)) { + // replace + if (oldkey) + memcpy(oldkey, this_key, zh->keysz); + if (oldvalue) + memcpy(oldvalue, this_value, zh->valuesz); + memcpy(this_key, key, zh->keysz); + memcpy(this_value, value, zh->valuesz); + zh->entries[entry_idx * zh->entrysz] = 1; // mark valid + return 1; + } + + entry_idx = (entry_idx + 1) & (zh->nentries - 1); + } + + // add the entry + zh->entries[entry_idx * zh->entrysz] = 1; + memcpy(&zh->entries[entry_idx * zh->entrysz + 1], key, zh->keysz); + memcpy(&zh->entries[entry_idx * zh->entrysz + 1 + zh->keysz], value, zh->valuesz); + zh->size++; + + if (zh->nentries < ZHASH_FACTOR_CRITICAL * zh->size) { + zhash_t *newhash = zhash_create_capacity(zh->keysz, zh->valuesz, + zh->hash, zh->equals, + zh->size); + + for (int idx = 0; idx < zh->nentries; idx++) { + + if (zh->entries[idx * zh->entrysz]) { + void *this_key = &zh->entries[idx * zh->entrysz + 1]; + void *this_value = &zh->entries[idx * zh->entrysz + 1 + zh->keysz]; + if (zhash_put(newhash, this_key, this_value, NULL, NULL)) + assert(0); // shouldn't already be present. + } + } + + // play switch-a-roo + zhash_t tmp; + memcpy(&tmp, zh, sizeof(zhash_t)); + memcpy(zh, newhash, sizeof(zhash_t)); + memcpy(newhash, &tmp, sizeof(zhash_t)); + zhash_destroy(newhash); + } + + return 0; +} + +int zhash_remove(zhash_t *zh, const void *key, void *old_key, void *old_value) +{ + uint32_t code = zh->hash(key); + uint32_t entry_idx = code & (zh->nentries - 1); + + while (zh->entries[entry_idx * zh->entrysz]) { + void *this_key = &zh->entries[entry_idx * zh->entrysz + 1]; + void *this_value = &zh->entries[entry_idx * zh->entrysz + 1 + zh->keysz]; + + if (zh->equals(key, this_key)) { + if (old_key) + memcpy(old_key, this_key, zh->keysz); + if (old_value) + memcpy(old_value, this_value, zh->valuesz); + + // mark this entry as available + zh->entries[entry_idx * zh->entrysz] = 0; + zh->size--; + + // reinsert any consecutive entries that follow + while (1) { + entry_idx = (entry_idx + 1) & (zh->nentries - 1); + + if (zh->entries[entry_idx * zh->entrysz]) { + // completely remove this entry + char *tmp = malloc(sizeof(char)*zh->entrysz); + memcpy(tmp, &zh->entries[entry_idx * zh->entrysz], zh->entrysz); + zh->entries[entry_idx * zh->entrysz] = 0; + zh->size--; + // reinsert it + if (zhash_put(zh, &tmp[1], &tmp[1+zh->keysz], NULL, NULL)) + assert(0); + free(tmp); + } else { + break; + } + } + return 1; + } + + entry_idx = (entry_idx + 1) & (zh->nentries - 1); + } + + return 0; +} + +zhash_t *zhash_copy(const zhash_t *zh) +{ + zhash_t *newhash = zhash_create_capacity(zh->keysz, zh->valuesz, + zh->hash, zh->equals, + zh->size); + + for (int entry_idx = 0; entry_idx < zh->nentries; entry_idx++) { + if (zh->entries[entry_idx * zh->entrysz]) { + void *this_key = &zh->entries[entry_idx * zh->entrysz + 1]; + void *this_value = &zh->entries[entry_idx * zh->entrysz + 1 + zh->keysz]; + if (zhash_put(newhash, this_key, this_value, NULL, NULL)) + assert(0); // shouldn't already be present. + } + } + + return newhash; +} + +int zhash_contains(const zhash_t *zh, const void *key) +{ + void *tmp; + return zhash_get_volatile(zh, key, &tmp); +} + +void zhash_iterator_init(zhash_t *zh, zhash_iterator_t *zit) +{ + zit->zh = zh; + zit->czh = zh; + zit->last_entry = -1; +} + +void zhash_iterator_init_const(const zhash_t *zh, zhash_iterator_t *zit) +{ + zit->zh = NULL; + zit->czh = zh; + zit->last_entry = -1; +} + +int zhash_iterator_next_volatile(zhash_iterator_t *zit, void *outkey, void *outvalue) +{ + const zhash_t *zh = zit->czh; + + while (1) { + if (zit->last_entry + 1 >= zh->nentries) + return 0; + + zit->last_entry++; + + if (zh->entries[zit->last_entry * zh->entrysz]) { + void *this_key = &zh->entries[zit->last_entry * zh->entrysz + 1]; + void *this_value = &zh->entries[zit->last_entry * zh->entrysz + 1 + zh->keysz]; + + if (outkey != NULL) + *((void**) outkey) = this_key; + if (outvalue != NULL) + *((void**) outvalue) = this_value; + + return 1; + } + } +} + +int zhash_iterator_next(zhash_iterator_t *zit, void *outkey, void *outvalue) +{ + const zhash_t *zh = zit->czh; + + void *outkeyp, *outvaluep; + + if (!zhash_iterator_next_volatile(zit, &outkeyp, &outvaluep)) + return 0; + + if (outkey != NULL) + memcpy(outkey, outkeyp, zh->keysz); + if (outvalue != NULL) + memcpy(outvalue, outvaluep, zh->valuesz); + + return 1; +} + +void zhash_iterator_remove(zhash_iterator_t *zit) +{ + assert(zit->zh); // can't call _remove on a iterator with const zhash + zhash_t *zh = zit->zh; + + zh->entries[zit->last_entry * zh->entrysz] = 0; + zh->size--; + + // re-insert following entries + int entry_idx = (zit->last_entry + 1) & (zh->nentries - 1); + while (zh->entries[entry_idx *zh->entrysz]) { + // completely remove this entry + char *tmp = malloc(sizeof(char)*zh->entrysz); + memcpy(tmp, &zh->entries[entry_idx * zh->entrysz], zh->entrysz); + zh->entries[entry_idx * zh->entrysz] = 0; + zh->size--; + + // reinsert it + if (zhash_put(zh, &tmp[1], &tmp[1+zh->keysz], NULL, NULL)) + assert(0); + free(tmp); + + entry_idx = (entry_idx + 1) & (zh->nentries - 1); + } + + zit->last_entry--; +} + +void zhash_map_keys(zhash_t *zh, void (*f)(void *)) +{ + assert(zh != NULL); + if (f == NULL) + return; + + zhash_iterator_t itr; + zhash_iterator_init(zh, &itr); + + void *key, *value; + + while(zhash_iterator_next_volatile(&itr, &key, &value)) { + f(key); + } +} + +void zhash_vmap_keys(zhash_t * zh, void (*f)(void *)) +{ + assert(zh != NULL); + if (f == NULL) + return; + + zhash_iterator_t itr; + zhash_iterator_init(zh, &itr); + + void *key, *value; + + while(zhash_iterator_next_volatile(&itr, &key, &value)) { + void *p = *(void**) key; + f(p); + } +} + +void zhash_map_values(zhash_t * zh, void (*f)(void *)) +{ + assert(zh != NULL); + if (f == NULL) + return; + + zhash_iterator_t itr; + zhash_iterator_init(zh, &itr); + + void *key, *value; + while(zhash_iterator_next_volatile(&itr, &key, &value)) { + f(value); + } +} + +void zhash_vmap_values(zhash_t * zh, void (*f)(void *)) +{ + assert(zh != NULL); + if (f == NULL) + return; + + zhash_iterator_t itr; + zhash_iterator_init(zh, &itr); + + void *key, *value; + while(zhash_iterator_next_volatile(&itr, &key, &value)) { + void *p = *(void**) value; + f(p); + } +} + +zarray_t *zhash_keys(const zhash_t *zh) +{ + assert(zh != NULL); + + zarray_t *za = zarray_create(zh->keysz); + + zhash_iterator_t itr; + zhash_iterator_init_const(zh, &itr); + + void *key, *value; + while(zhash_iterator_next_volatile(&itr, &key, &value)) { + zarray_add(za, key); + } + + return za; +} + +zarray_t *zhash_values(const zhash_t *zh) +{ + assert(zh != NULL); + + zarray_t *za = zarray_create(zh->valuesz); + + zhash_iterator_t itr; + zhash_iterator_init_const(zh, &itr); + + void *key, *value; + while(zhash_iterator_next_volatile(&itr, &key, &value)) { + zarray_add(za, value); + } + + return za; +} + + +uint32_t zhash_uint32_hash(const void *_a) +{ + assert(_a != NULL); + + uint32_t a = *((uint32_t*) _a); + return a; +} + +int zhash_uint32_equals(const void *_a, const void *_b) +{ + assert(_a != NULL); + assert(_b != NULL); + + uint32_t a = *((uint32_t*) _a); + uint32_t b = *((uint32_t*) _b); + + return a==b; +} + +uint32_t zhash_uint64_hash(const void *_a) +{ + assert(_a != NULL); + + uint64_t a = *((uint64_t*) _a); + return (uint32_t) (a ^ (a >> 32)); +} + +int zhash_uint64_equals(const void *_a, const void *_b) +{ + assert(_a != NULL); + assert(_b != NULL); + + uint64_t a = *((uint64_t*) _a); + uint64_t b = *((uint64_t*) _b); + + return a==b; +} + + +union uintpointer +{ + const void *p; + uint32_t i; +}; + +uint32_t zhash_ptr_hash(const void *a) +{ + assert(a != NULL); + + union uintpointer ip; + ip.p = * (void**)a; + + // compute a hash from the lower 32 bits of the pointer (on LE systems) + uint32_t hash = ip.i; + hash ^= (hash >> 7); + + return hash; +} + + +int zhash_ptr_equals(const void *a, const void *b) +{ + assert(a != NULL); + assert(b != NULL); + + const void * ptra = * (void**)a; + const void * ptrb = * (void**)b; + return ptra == ptrb; +} + + +int zhash_str_equals(const void *_a, const void *_b) +{ + assert(_a != NULL); + assert(_b != NULL); + + char *a = * (char**)_a; + char *b = * (char**)_b; + + return !strcmp(a, b); +} + +uint32_t zhash_str_hash(const void *_a) +{ + assert(_a != NULL); + + char *a = * (char**)_a; + + uint32_t hash = 0; + while (*a != 0) { + // optimization of hash x FNV_prime + hash += (hash << 1) + (hash << 4) + (hash << 7) + (hash << 8) + (hash << 24); + hash ^= *a; + a++; + } + + return hash; +} + + +void zhash_debug(zhash_t *zh) +{ + for (int entry_idx = 0; entry_idx < zh->nentries; entry_idx++) { + char *k, *v; + memcpy(&k, &zh->entries[entry_idx * zh->entrysz + 1], sizeof(char*)); + memcpy(&v, &zh->entries[entry_idx * zh->entrysz + 1 + zh->keysz], sizeof(char*)); + printf("%d: %d, %s => %s\n", entry_idx, zh->entries[entry_idx * zh->entrysz], k, v); + } +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/common/zmaxheap.c b/apriltag/src/main/native/thirdparty/apriltag/src/common/zmaxheap.c new file mode 100644 index 0000000000..a073ae5a17 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/common/zmaxheap.c @@ -0,0 +1,424 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#include +#include +#include +#include +#include +#include + +#include "zmaxheap.h" +#include "debug_print.h" + +#ifdef _WIN32 +static inline long int random(void) +{ + return rand(); +} +#endif + +// 0 +// 1 2 +// 3 4 5 6 +// 7 8 9 10 11 12 13 14 +// +// Children of node i: 2*i+1, 2*i+2 +// Parent of node i: (i-1) / 2 +// +// Heap property: a parent is greater than (or equal to) its children. + +#define MIN_CAPACITY 16 + +struct zmaxheap +{ + size_t el_sz; + + int size; + int alloc; + + float *values; + char *data; + + void (*swap)(zmaxheap_t *heap, int a, int b); +}; + +static inline void swap_default(zmaxheap_t *heap, int a, int b) +{ + float t = heap->values[a]; + heap->values[a] = heap->values[b]; + heap->values[b] = t; + + char *tmp = malloc(sizeof(char)*heap->el_sz); + memcpy(tmp, &heap->data[a*heap->el_sz], heap->el_sz); + memcpy(&heap->data[a*heap->el_sz], &heap->data[b*heap->el_sz], heap->el_sz); + memcpy(&heap->data[b*heap->el_sz], tmp, heap->el_sz); + free(tmp); +} + +static inline void swap_pointer(zmaxheap_t *heap, int a, int b) +{ + float t = heap->values[a]; + heap->values[a] = heap->values[b]; + heap->values[b] = t; + + void **pp = (void**) heap->data; + void *tmp = pp[a]; + pp[a] = pp[b]; + pp[b] = tmp; +} + + +zmaxheap_t *zmaxheap_create(size_t el_sz) +{ + zmaxheap_t *heap = calloc(1, sizeof(zmaxheap_t)); + heap->el_sz = el_sz; + + heap->swap = swap_default; + + if (el_sz == sizeof(void*)) + heap->swap = swap_pointer; + + return heap; +} + +void zmaxheap_destroy(zmaxheap_t *heap) +{ + free(heap->values); + free(heap->data); + memset(heap, 0, sizeof(zmaxheap_t)); + free(heap); +} + +int zmaxheap_size(zmaxheap_t *heap) +{ + return heap->size; +} + +void zmaxheap_ensure_capacity(zmaxheap_t *heap, int capacity) +{ + if (heap->alloc >= capacity) + return; + + int newcap = heap->alloc; + + while (newcap < capacity) { + if (newcap < MIN_CAPACITY) { + newcap = MIN_CAPACITY; + continue; + } + + newcap *= 2; + } + + heap->values = realloc(heap->values, newcap * sizeof(float)); + heap->data = realloc(heap->data, newcap * heap->el_sz); + heap->alloc = newcap; +} + +void zmaxheap_add(zmaxheap_t *heap, void *p, float v) +{ + + assert (isfinite(v) && "zmaxheap_add: Trying to add non-finite number to heap. NaN's prohibited, could allow INF with testing"); + zmaxheap_ensure_capacity(heap, heap->size + 1); + + int idx = heap->size; + + heap->values[idx] = v; + memcpy(&heap->data[idx*heap->el_sz], p, heap->el_sz); + + heap->size++; + + while (idx > 0) { + + int parent = (idx - 1) / 2; + + // we're done! + if (heap->values[parent] >= v) + break; + + // else, swap and recurse upwards. + heap->swap(heap, idx, parent); + idx = parent; + } +} + +void zmaxheap_vmap(zmaxheap_t *heap, void (*f)(void*)) +{ + assert(heap != NULL); + assert(f != NULL); + assert(heap->el_sz == sizeof(void*)); + + for (int idx = 0; idx < heap->size; idx++) { + void *p = NULL; + memcpy(&p, &heap->data[idx*heap->el_sz], heap->el_sz); + if (p == NULL) { + debug_print("Warning: zmaxheap_vmap item %d is NULL\n", idx); + } + f(p); + } +} + +// Removes the item in the heap at the given index. Returns 1 if the +// item existed. 0 Indicates an invalid idx (heap is smaller than +// idx). This is mostly intended to be used by zmaxheap_remove_max. +int zmaxheap_remove_index(zmaxheap_t *heap, int idx, void *p, float *v) +{ + if (idx >= heap->size) + return 0; + + // copy out the requested element from the heap. + if (v != NULL) + *v = heap->values[idx]; + if (p != NULL) + memcpy(p, &heap->data[idx*heap->el_sz], heap->el_sz); + + heap->size--; + + // If this element is already the last one, then there's nothing + // for us to do. + if (idx == heap->size) + return 1; + + // copy last element to first element. (which probably upsets + // the heap property). + heap->values[idx] = heap->values[heap->size]; + memcpy(&heap->data[idx*heap->el_sz], &heap->data[heap->el_sz * heap->size], heap->el_sz); + + // now fix the heap. Note, as we descend, we're "pushing down" + // the same node the entire time. Thus, while the index of the + // parent might change, the parent_score doesn't. + int parent = idx; + float parent_score = heap->values[idx]; + + // descend, fixing the heap. + while (parent < heap->size) { + + int left = 2*parent + 1; + int right = left + 1; + +// assert(parent_score == heap->values[parent]); + + float left_score = (left < heap->size) ? heap->values[left] : -INFINITY; + float right_score = (right < heap->size) ? heap->values[right] : -INFINITY; + + // put the biggest of (parent, left, right) as the parent. + + // already okay? + if (parent_score >= left_score && parent_score >= right_score) + break; + + // if we got here, then one of the children is bigger than the parent. + if (left_score >= right_score) { + assert(left < heap->size); + heap->swap(heap, parent, left); + parent = left; + } else { + // right_score can't be less than left_score if right_score is -INFINITY. + assert(right < heap->size); + heap->swap(heap, parent, right); + parent = right; + } + } + + return 1; +} + +int zmaxheap_remove_max(zmaxheap_t *heap, void *p, float *v) +{ + return zmaxheap_remove_index(heap, 0, p, v); +} + +void zmaxheap_iterator_init(zmaxheap_t *heap, zmaxheap_iterator_t *it) +{ + memset(it, 0, sizeof(zmaxheap_iterator_t)); + it->heap = heap; + it->in = 0; + it->out = 0; +} + +int zmaxheap_iterator_next(zmaxheap_iterator_t *it, void *p, float *v) +{ + zmaxheap_t *heap = it->heap; + + if (it->in >= zmaxheap_size(heap)) + return 0; + + *v = heap->values[it->in]; + memcpy(p, &heap->data[it->in*heap->el_sz], heap->el_sz); + + if (it->in != it->out) { + heap->values[it->out] = heap->values[it->in]; + memcpy(&heap->data[it->out*heap->el_sz], &heap->data[it->in*heap->el_sz], heap->el_sz); + } + + it->in++; + it->out++; + return 1; +} + +int zmaxheap_iterator_next_volatile(zmaxheap_iterator_t *it, void *p, float *v) +{ + zmaxheap_t *heap = it->heap; + + if (it->in >= zmaxheap_size(heap)) + return 0; + + *v = heap->values[it->in]; + *((void**) p) = &heap->data[it->in*heap->el_sz]; + + if (it->in != it->out) { + heap->values[it->out] = heap->values[it->in]; + memcpy(&heap->data[it->out*heap->el_sz], &heap->data[it->in*heap->el_sz], heap->el_sz); + } + + it->in++; + it->out++; + return 1; +} + +void zmaxheap_iterator_remove(zmaxheap_iterator_t *it) +{ + it->out--; +} + +static void maxheapify(zmaxheap_t *heap, int parent) +{ + int left = 2*parent + 1; + int right = 2*parent + 2; + + int betterchild = parent; + + if (left < heap->size && heap->values[left] > heap->values[betterchild]) + betterchild = left; + if (right < heap->size && heap->values[right] > heap->values[betterchild]) + betterchild = right; + + if (betterchild != parent) { + heap->swap(heap, parent, betterchild); + maxheapify(heap, betterchild); + return; + } +} + +#if 0 //won't compile if defined but not used +// test the heap property +static void validate(zmaxheap_t *heap) +{ + for (int parent = 0; parent < heap->size; parent++) { + int left = 2*parent + 1; + int right = 2*parent + 2; + + if (left < heap->size) { + assert(heap->values[parent] > heap->values[left]); + } + + if (right < heap->size) { + assert(heap->values[parent] > heap->values[right]); + } + } +} +#endif +void zmaxheap_iterator_finish(zmaxheap_iterator_t *it) +{ + // if nothing was removed, no work to do. + if (it->in == it->out) + return; + + zmaxheap_t *heap = it->heap; + + heap->size = it->out; + + // restore heap property + for (int i = heap->size/2 - 1; i >= 0; i--) + maxheapify(heap, i); +} + +void zmaxheap_test(void) +{ + int cap = 10000; + int sz = 0; + int32_t *vals = calloc(cap, sizeof(int32_t)); + + zmaxheap_t *heap = zmaxheap_create(sizeof(int32_t)); + + int maxsz = 0; + int zcnt = 0; + + for (int iter = 0; iter < 5000000; iter++) { + assert(sz == heap->size); + + if ((random() & 1) == 0 && sz < cap) { + // add a value + int32_t v = (int32_t) (random() / 1000); + float fv = v; + assert(v == fv); + + vals[sz] = v; + zmaxheap_add(heap, &v, fv); + sz++; + +// printf("add %d %f\n", v, fv); + } else { + // remove a value + int maxv = -1, maxi = -1; + + for (int i = 0; i < sz; i++) { + if (vals[i] > maxv) { + maxv = vals[i]; + maxi = i; + } + } + + + int32_t outv; + float outfv; + int res = zmaxheap_remove_max(heap, &outv, &outfv); + if (sz == 0) { + (void) res; + assert(res == 0); + } else { +// printf("%d %d %d %f\n", sz, maxv, outv, outfv); + assert(outv == outfv); + assert(maxv == outv); + + // shuffle erase the maximum from our list. + vals[maxi] = vals[sz - 1]; + sz--; + } + } + + if (sz > maxsz) + maxsz = sz; + + if (maxsz > 0 && sz == 0) + zcnt++; + } + + printf("max size: %d, zcount %d\n", maxsz, zcnt); + free (vals); +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/tag16h5.c b/apriltag/src/main/native/thirdparty/apriltag/src/tag16h5.c new file mode 100644 index 0000000000..e38302a1d2 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/tag16h5.c @@ -0,0 +1,117 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#include +#include "tag16h5.h" + +static uint64_t codedata[30] = { + 0x00000000000027c8UL, + 0x00000000000031b6UL, + 0x0000000000003859UL, + 0x000000000000569cUL, + 0x0000000000006c76UL, + 0x0000000000007ddbUL, + 0x000000000000af09UL, + 0x000000000000f5a1UL, + 0x000000000000fb8bUL, + 0x0000000000001cb9UL, + 0x00000000000028caUL, + 0x000000000000e8dcUL, + 0x0000000000001426UL, + 0x0000000000005770UL, + 0x0000000000009253UL, + 0x000000000000b702UL, + 0x000000000000063aUL, + 0x0000000000008f34UL, + 0x000000000000b4c0UL, + 0x00000000000051ecUL, + 0x000000000000e6f0UL, + 0x0000000000005fa4UL, + 0x000000000000dd43UL, + 0x0000000000001aaaUL, + 0x000000000000e62fUL, + 0x0000000000006dbcUL, + 0x000000000000b6ebUL, + 0x000000000000de10UL, + 0x000000000000154dUL, + 0x000000000000b57aUL, +}; +apriltag_family_t *tag16h5_create(void) +{ + apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t)); + tf->name = strdup("tag16h5"); + tf->h = 5; + tf->ncodes = 30; + tf->codes = codedata; + tf->nbits = 16; + tf->bit_x = calloc(16, sizeof(uint32_t)); + tf->bit_y = calloc(16, sizeof(uint32_t)); + tf->bit_x[0] = 1; + tf->bit_y[0] = 1; + tf->bit_x[1] = 2; + tf->bit_y[1] = 1; + tf->bit_x[2] = 3; + tf->bit_y[2] = 1; + tf->bit_x[3] = 2; + tf->bit_y[3] = 2; + tf->bit_x[4] = 4; + tf->bit_y[4] = 1; + tf->bit_x[5] = 4; + tf->bit_y[5] = 2; + tf->bit_x[6] = 4; + tf->bit_y[6] = 3; + tf->bit_x[7] = 3; + tf->bit_y[7] = 2; + tf->bit_x[8] = 4; + tf->bit_y[8] = 4; + tf->bit_x[9] = 3; + tf->bit_y[9] = 4; + tf->bit_x[10] = 2; + tf->bit_y[10] = 4; + tf->bit_x[11] = 3; + tf->bit_y[11] = 3; + tf->bit_x[12] = 1; + tf->bit_y[12] = 4; + tf->bit_x[13] = 1; + tf->bit_y[13] = 3; + tf->bit_x[14] = 1; + tf->bit_y[14] = 2; + tf->bit_x[15] = 2; + tf->bit_y[15] = 3; + tf->width_at_border = 6; + tf->total_width = 8; + tf->reversed_border = false; + return tf; +} + +void tag16h5_destroy(apriltag_family_t *tf) +{ + free(tf->bit_x); + free(tf->bit_y); + free(tf->name); + free(tf); +} diff --git a/apriltag/src/main/native/thirdparty/apriltag/src/tag36h11.c b/apriltag/src/main/native/thirdparty/apriltag/src/tag36h11.c new file mode 100644 index 0000000000..f1347a5bc1 --- /dev/null +++ b/apriltag/src/main/native/thirdparty/apriltag/src/tag36h11.c @@ -0,0 +1,714 @@ +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + +#include +#include "tag36h11.h" + +static uint64_t codedata[587] = { + 0x0000000d7e00984bUL, + 0x0000000dda664ca7UL, + 0x0000000dc4a1c821UL, + 0x0000000e17b470e9UL, + 0x0000000ef91d01b1UL, + 0x0000000f429cdd73UL, + 0x000000005da29225UL, + 0x00000001106cba43UL, + 0x0000000223bed79dUL, + 0x000000021f51213cUL, + 0x000000033eb19ca6UL, + 0x00000003f76eb0f8UL, + 0x0000000469a97414UL, + 0x000000045dcfe0b0UL, + 0x00000004a6465f72UL, + 0x000000051801db96UL, + 0x00000005eb946b4eUL, + 0x000000068a7cc2ecUL, + 0x00000006f0ba2652UL, + 0x000000078765559dUL, + 0x000000087b83d129UL, + 0x000000086cc4a5c5UL, + 0x00000008b64df90fUL, + 0x00000009c577b611UL, + 0x0000000a3810f2f5UL, + 0x0000000af4d75b83UL, + 0x0000000b59a03fefUL, + 0x0000000bb1096f85UL, + 0x0000000d1b92fc76UL, + 0x0000000d0dd509d2UL, + 0x0000000e2cfda160UL, + 0x00000002ff497c63UL, + 0x000000047240671bUL, + 0x00000005047a2e55UL, + 0x0000000635ca87c7UL, + 0x0000000691254166UL, + 0x000000068f43d94aUL, + 0x00000006ef24bdb6UL, + 0x00000008cdd8f886UL, + 0x00000009de96b718UL, + 0x0000000aff6e5a8aUL, + 0x0000000bae46f029UL, + 0x0000000d225b6d59UL, + 0x0000000df8ba8c01UL, + 0x0000000e3744a22fUL, + 0x0000000fbb59375dUL, + 0x000000018a916828UL, + 0x000000022f29c1baUL, + 0x0000000286887d58UL, + 0x000000041392322eUL, + 0x000000075d18ecd1UL, + 0x000000087c302743UL, + 0x00000008c6317ba9UL, + 0x00000009e40f36d7UL, + 0x0000000c0e5a806aUL, + 0x0000000cc78cb87cUL, + 0x000000012d2f2d01UL, + 0x0000000379f36a21UL, + 0x00000006973f59acUL, + 0x00000007789ea9f4UL, + 0x00000008f1c73e84UL, + 0x00000008dd287a20UL, + 0x000000094a4eee4cUL, + 0x0000000a455379b5UL, + 0x0000000a9e92987dUL, + 0x0000000bd25cb40bUL, + 0x0000000be98d3582UL, + 0x0000000d3d5972b2UL, + 0x000000014c53d7c7UL, + 0x00000004f1796936UL, + 0x00000004e71fed1aUL, + 0x000000066d46fae0UL, + 0x0000000a55abb933UL, + 0x0000000ebee1accaUL, + 0x00000001ad4ba6a4UL, + 0x0000000305b17571UL, + 0x0000000553611351UL, + 0x000000059ca62775UL, + 0x00000007819cb6a1UL, + 0x0000000edb7bc9ebUL, + 0x00000005b2694212UL, + 0x000000072e12d185UL, + 0x0000000ed6152e2cUL, + 0x00000005bcdadbf3UL, + 0x000000078e0aa0c6UL, + 0x0000000c60a0b909UL, + 0x0000000ef9a34b0dUL, + 0x0000000398a6621aUL, + 0x0000000a8a27c944UL, + 0x00000004b564304eUL, + 0x000000052902b4e2UL, + 0x0000000857280b56UL, + 0x0000000a91b2c84bUL, + 0x0000000e91df939bUL, + 0x00000001fa405f28UL, + 0x000000023793ab86UL, + 0x000000068c17729fUL, + 0x00000009fbf3b840UL, + 0x000000036922413cUL, + 0x00000004eb5f946eUL, + 0x0000000533fe2404UL, + 0x000000063de7d35eUL, + 0x0000000925eddc72UL, + 0x000000099b8b3896UL, + 0x0000000aace4c708UL, + 0x0000000c22994af0UL, + 0x00000008f1eae41bUL, + 0x0000000d95fb486cUL, + 0x000000013fb77857UL, + 0x00000004fe0983a3UL, + 0x0000000d559bf8a9UL, + 0x0000000e1855d78dUL, + 0x0000000fec8daaadUL, + 0x000000071ecb6d95UL, + 0x0000000dc9e50e4cUL, + 0x0000000ca3a4c259UL, + 0x0000000740d12bbfUL, + 0x0000000aeedd18e0UL, + 0x0000000b509b9c8eUL, + 0x00000005232fea1cUL, + 0x000000019282d18bUL, + 0x000000076c22d67bUL, + 0x0000000936beb34bUL, + 0x000000008a5ea8ddUL, + 0x0000000679eadc28UL, + 0x0000000a08e119c5UL, + 0x000000020a6e3e24UL, + 0x00000007eab9c239UL, + 0x000000096632c32eUL, + 0x0000000470d06e44UL, + 0x00000008a70212fbUL, + 0x00000000a7e4251bUL, + 0x00000009ec762cc0UL, + 0x0000000d8a3a1f48UL, + 0x0000000db680f346UL, + 0x00000004a1e93a9dUL, + 0x0000000638ddc04fUL, + 0x00000004c2fcc993UL, + 0x000000001ef28c95UL, + 0x0000000bf0d9792dUL, + 0x00000006d27557c3UL, + 0x0000000623f977f4UL, + 0x000000035b43be57UL, + 0x0000000bb0c428d5UL, + 0x0000000a6f01474dUL, + 0x00000005a70c9749UL, + 0x000000020ddabc3bUL, + 0x00000002eabd78cfUL, + 0x000000090aa18f88UL, + 0x0000000a9ea89350UL, + 0x00000003cdb39b22UL, + 0x0000000839a08f34UL, + 0x0000000169bb814eUL, + 0x00000001a575ab08UL, + 0x0000000a04d3d5a2UL, + 0x0000000bf7902f2bUL, + 0x0000000095a5e65cUL, + 0x000000092e8fce94UL, + 0x000000067ef48d12UL, + 0x00000006400dbcacUL, + 0x0000000b12d8fb9fUL, + 0x00000000347f45d3UL, + 0x0000000b35826f56UL, + 0x0000000c546ac6e4UL, + 0x000000081cc35b66UL, + 0x000000041d14bd57UL, + 0x00000000c052b168UL, + 0x00000007d6ce5018UL, + 0x0000000ab4ed5edeUL, + 0x00000005af817119UL, + 0x0000000d1454b182UL, + 0x00000002badb090bUL, + 0x000000003fcb4c0cUL, + 0x00000002f1c28fd8UL, + 0x000000093608c6f7UL, + 0x00000004c93ba2b5UL, + 0x000000007d950a5dUL, + 0x0000000e54b3d3fcUL, + 0x000000015560cf9dUL, + 0x0000000189e4958aUL, + 0x000000062140e9d2UL, + 0x0000000723bc1cdbUL, + 0x00000002063f26faUL, + 0x0000000fa08ab19fUL, + 0x00000007955641dbUL, + 0x0000000646b01daaUL, + 0x000000071cd427ccUL, + 0x000000009a42f7d4UL, + 0x0000000717edc643UL, + 0x000000015eb94367UL, + 0x00000008392e6bb2UL, + 0x0000000832408542UL, + 0x00000002b9b874beUL, + 0x0000000b21f4730dUL, + 0x0000000b5d8f24c9UL, + 0x00000007dbaf6931UL, + 0x00000001b4e33629UL, + 0x000000013452e710UL, + 0x0000000e974af612UL, + 0x00000001df61d29aUL, + 0x000000099f2532adUL, + 0x0000000e50ec71b4UL, + 0x00000005df0a36e8UL, + 0x00000004934e4ceaUL, + 0x0000000e34a0b4bdUL, + 0x0000000b7b26b588UL, + 0x00000000f255118dUL, + 0x0000000d0c8fa31eUL, + 0x000000006a50c94fUL, + 0x0000000f28aa9f06UL, + 0x0000000131d194d8UL, + 0x0000000622e3da79UL, + 0x0000000ac7478303UL, + 0x0000000c8f2521d7UL, + 0x00000006c9c881f5UL, + 0x000000049e38b60aUL, + 0x0000000513d8df65UL, + 0x0000000d7c2b0785UL, + 0x00000009f6f9d75aUL, + 0x00000009f6966020UL, + 0x00000001e1a54e33UL, + 0x0000000c04d63419UL, + 0x0000000946e04cd7UL, + 0x00000001bdac5902UL, + 0x000000056469b830UL, + 0x0000000ffad59569UL, + 0x000000086970e7d8UL, + 0x00000008a4b41e12UL, + 0x0000000ad4688e3bUL, + 0x000000085f8f5df4UL, + 0x0000000d833a0893UL, + 0x00000002a36fdd7cUL, + 0x0000000d6a857cf2UL, + 0x00000008829bc35cUL, + 0x00000005e50d79bcUL, + 0x0000000fbb8035e4UL, + 0x0000000c1a95bebfUL, + 0x0000000036b0baf8UL, + 0x0000000e0da964eaUL, + 0x0000000b6483689bUL, + 0x00000007c8e2f4c1UL, + 0x00000005b856a23bUL, + 0x00000002fc183995UL, + 0x0000000e914b6d70UL, + 0x0000000b31041969UL, + 0x00000001bb478493UL, + 0x0000000063e2b456UL, + 0x0000000f2a082b9cUL, + 0x00000008e5e646eaUL, + 0x000000008172f8f6UL, + 0x00000000dacd923eUL, + 0x0000000e5dcf0e2eUL, + 0x0000000bf9446baeUL, + 0x00000004822d50d1UL, + 0x000000026e710bf5UL, + 0x0000000b90ba2a24UL, + 0x0000000f3b25aa73UL, + 0x0000000809ad589bUL, + 0x000000094cc1e254UL, + 0x00000005334a3adbUL, + 0x0000000592886b2fUL, + 0x0000000bf64704aaUL, + 0x0000000566dbf24cUL, + 0x000000072203e692UL, + 0x000000064e61e809UL, + 0x0000000d7259aad6UL, + 0x00000007b924aedcUL, + 0x00000002df2184e8UL, + 0x0000000353d1eca7UL, + 0x0000000fce30d7ceUL, + 0x0000000f7b0f436eUL, + 0x000000057e8d8f68UL, + 0x00000008c79e60dbUL, + 0x00000009c8362b2bUL, + 0x000000063a5804f2UL, + 0x00000009298353dcUL, + 0x00000006f98a71c8UL, + 0x0000000a5731f693UL, + 0x000000021ca5c870UL, + 0x00000001c2107fd3UL, + 0x00000006181f6c39UL, + 0x000000019e574304UL, + 0x0000000329937606UL, + 0x0000000043d5c70dUL, + 0x00000009b18ff162UL, + 0x00000008e2ccfebfUL, + 0x000000072b7b9b54UL, + 0x00000009b71f4f3cUL, + 0x0000000935d7393eUL, + 0x000000065938881aUL, + 0x00000006a5bd6f2dUL, + 0x0000000a19783306UL, + 0x0000000e6472f4d7UL, + 0x000000081163df5aUL, + 0x0000000a838e1cbdUL, + 0x0000000982748477UL, + 0x0000000050c54febUL, + 0x00000000d82fbb58UL, + 0x00000002c4c72799UL, + 0x000000097d259ad6UL, + 0x000000022d9a43edUL, + 0x0000000fdb162a9fUL, + 0x00000000cb4a727dUL, + 0x00000004fae2e371UL, + 0x0000000535b5be8bUL, + 0x000000048795908aUL, + 0x0000000ce7c18962UL, + 0x00000004ea154d80UL, + 0x000000050c064889UL, + 0x00000008d97fc75dUL, + 0x0000000c8bd9ec61UL, + 0x000000083ee8e8bbUL, + 0x0000000c8431419aUL, + 0x00000001aa78079dUL, + 0x00000008111aa4a5UL, + 0x0000000dfa3a69feUL, + 0x000000051630d83fUL, + 0x00000002d930fb3fUL, + 0x00000002133116e5UL, + 0x0000000ae5395522UL, + 0x0000000bc07a4e8aUL, + 0x000000057bf08ba0UL, + 0x00000006cb18036aUL, + 0x0000000f0e2e4b75UL, + 0x00000003eb692b6fUL, + 0x0000000d8178a3faUL, + 0x0000000238cce6a6UL, + 0x0000000e97d5cdd7UL, + 0x0000000fe10d8d5eUL, + 0x0000000b39584a1dUL, + 0x0000000ca03536fdUL, + 0x0000000aa61f3998UL, + 0x000000072ff23ec2UL, + 0x000000015aa7d770UL, + 0x000000057a3a1282UL, + 0x0000000d1f3902dcUL, + 0x00000006554c9388UL, + 0x0000000fd01283c7UL, + 0x0000000e8baa42c5UL, + 0x000000072cee6adfUL, + 0x0000000f6614b3faUL, + 0x000000095c3778a2UL, + 0x00000007da4cea7aUL, + 0x0000000d18a5912cUL, + 0x0000000d116426e5UL, + 0x000000027c17bc1cUL, + 0x0000000b95b53bc1UL, + 0x0000000c8f937a05UL, + 0x0000000ed220c9bdUL, + 0x00000000c97d72abUL, + 0x00000008fb1217aeUL, + 0x000000025ca8a5a1UL, + 0x0000000b261b871bUL, + 0x00000001bef0a056UL, + 0x0000000806a51179UL, + 0x0000000eed249145UL, + 0x00000003f82aecebUL, + 0x0000000cc56e9acfUL, + 0x00000002e78d01ebUL, + 0x0000000102cee17fUL, + 0x000000037caad3d5UL, + 0x000000016ac5b1eeUL, + 0x00000002af164eceUL, + 0x0000000d4cd81dc9UL, + 0x000000012263a7e7UL, + 0x000000057ac7d117UL, + 0x00000009391d9740UL, + 0x00000007aedaa77fUL, + 0x00000009675a3c72UL, + 0x0000000277f25191UL, + 0x0000000ebb6e64b9UL, + 0x00000007ad3ef747UL, + 0x000000012759b181UL, + 0x0000000948257d4dUL, + 0x0000000b63a850f6UL, + 0x00000003a52a8f75UL, + 0x00000004a019532cUL, + 0x0000000a021a7529UL, + 0x0000000cc661876dUL, + 0x00000004085afd05UL, + 0x0000000e7048e089UL, + 0x00000003f979cdc6UL, + 0x0000000d9da9071bUL, + 0x0000000ed2fc5b68UL, + 0x000000079d64c3a1UL, + 0x0000000fd44e2361UL, + 0x00000008eea46a74UL, + 0x000000042233b9c2UL, + 0x0000000ae4d1765dUL, + 0x00000007303a094cUL, + 0x00000002d7033abeUL, + 0x00000003dcc2b0b4UL, + 0x00000000f0967d09UL, + 0x000000006f0cd7deUL, + 0x000000009807aca0UL, + 0x00000003a295cad3UL, + 0x00000002b106b202UL, + 0x00000003f38a828eUL, + 0x000000078af46596UL, + 0x0000000bda2dc713UL, + 0x00000009a8c8c9d9UL, + 0x00000006a0f2ddceUL, + 0x0000000a76af6fe2UL, + 0x0000000086f66fa4UL, + 0x0000000d52d63f8dUL, + 0x000000089f7a6e73UL, + 0x0000000cc6b23362UL, + 0x0000000b4ebf3c39UL, + 0x0000000564f300faUL, + 0x0000000e8de3a706UL, + 0x000000079a033b61UL, + 0x0000000765e160c5UL, + 0x0000000a266a4f85UL, + 0x0000000a68c38c24UL, + 0x0000000dca0711fbUL, + 0x000000085fba85baUL, + 0x000000037a207b46UL, + 0x0000000158fcc4d0UL, + 0x00000000569d79b3UL, + 0x00000007b1a25555UL, + 0x0000000a8ae22468UL, + 0x00000007c592bdfdUL, + 0x00000000c59a5f66UL, + 0x0000000b1115daa3UL, + 0x0000000f17c87177UL, + 0x00000006769d766bUL, + 0x00000002b637356dUL, + 0x000000013d8685acUL, + 0x0000000f24cb6ec0UL, + 0x00000000bd0b56d1UL, + 0x000000042ff0e26dUL, + 0x0000000b41609267UL, + 0x000000096f9518afUL, + 0x0000000c56f96636UL, + 0x00000004a8e10349UL, + 0x0000000863512171UL, + 0x0000000ea455d86cUL, + 0x0000000bd0e25279UL, + 0x0000000e65e3f761UL, + 0x000000036c84a922UL, + 0x000000085fd1b38fUL, + 0x0000000657c91539UL, + 0x000000015033fe04UL, + 0x000000009051c921UL, + 0x0000000ab27d80d8UL, + 0x0000000f92f7d0a1UL, + 0x00000008eb6bb737UL, + 0x000000010b5b0f63UL, + 0x00000006c9c7ad63UL, + 0x0000000f66fe70aeUL, + 0x0000000ca579bd92UL, + 0x0000000956198e4dUL, + 0x000000029e4405e5UL, + 0x0000000e44eb885cUL, + 0x000000041612456cUL, + 0x0000000ea45e0abfUL, + 0x0000000d326529bdUL, + 0x00000007b2c33cefUL, + 0x000000080bc9b558UL, + 0x00000007169b9740UL, + 0x0000000c37f99209UL, + 0x000000031ff6dab9UL, + 0x0000000c795190edUL, + 0x0000000a7636e95fUL, + 0x00000009df075841UL, + 0x000000055a083932UL, + 0x0000000a7cbdf630UL, + 0x0000000409ea4ef0UL, + 0x000000092a1991b6UL, + 0x00000004b078dee9UL, + 0x0000000ae18ce9e4UL, + 0x00000005a6e1ef35UL, + 0x00000001a403bd59UL, + 0x000000031ea70a83UL, + 0x00000002bc3c4f3aUL, + 0x00000005c921b3cbUL, + 0x0000000042da05c5UL, + 0x00000001f667d16bUL, + 0x0000000416a368cfUL, + 0x0000000fbc0a7a3bUL, + 0x00000009419f0c7cUL, + 0x000000081be2fa03UL, + 0x000000034e2c172fUL, + 0x000000028648d8aeUL, + 0x0000000c7acbb885UL, + 0x000000045f31eb6aUL, + 0x0000000d1cfc0a7bUL, + 0x000000042c4d260dUL, + 0x0000000cf6584097UL, + 0x000000094b132b14UL, + 0x00000003c5c5df75UL, + 0x00000008ae596fefUL, + 0x0000000aea8054ebUL, + 0x00000000ae9cc573UL, + 0x0000000496fb731bUL, + 0x0000000ebf105662UL, + 0x0000000af9c83a37UL, + 0x0000000c0d64cd6bUL, + 0x00000007b608159aUL, + 0x0000000e74431642UL, + 0x0000000d6fb9d900UL, + 0x0000000291e99de0UL, + 0x000000010500ba9aUL, + 0x00000005cd05d037UL, + 0x0000000a87254fb2UL, + 0x00000009d7824a37UL, + 0x00000008b2c7b47cUL, + 0x000000030c788145UL, + 0x00000002f4e5a8beUL, + 0x0000000badb884daUL, + 0x0000000026e0d5c9UL, + 0x00000006fdbaa32eUL, + 0x000000034758eb31UL, + 0x0000000565cd1b4fUL, + 0x00000002bfd90fb0UL, + 0x0000000093052a6bUL, + 0x0000000d3c13c4b9UL, + 0x00000002daea43bfUL, + 0x0000000a279762bcUL, + 0x0000000f1bd9f22cUL, + 0x00000004b7fec94fUL, + 0x0000000545761d5aUL, + 0x00000007327df411UL, + 0x00000001b52a442eUL, + 0x000000049b0ce108UL, + 0x000000024c764bc8UL, + 0x0000000374563045UL, + 0x0000000a3e8f91c6UL, + 0x00000000e6bd2241UL, + 0x0000000e0e52ee3cUL, + 0x000000007e8e3caaUL, + 0x000000096c2b7372UL, + 0x000000033acbdfdaUL, + 0x0000000b15d91e54UL, + 0x0000000464759ac1UL, + 0x00000006886a1998UL, + 0x000000057f5d3958UL, + 0x00000005a1f5c1f5UL, + 0x00000000b58158adUL, + 0x0000000e712053fbUL, + 0x00000005352ddb25UL, + 0x0000000414b98ea0UL, + 0x000000074f89f546UL, + 0x000000038a56b3c3UL, + 0x000000038db0dc17UL, + 0x0000000aa016a755UL, + 0x0000000dc72366f5UL, + 0x00000000cee93d75UL, + 0x0000000b2fe7a56bUL, + 0x0000000a847ed390UL, + 0x00000008713ef88cUL, + 0x0000000a217cc861UL, + 0x00000008bca25d7bUL, + 0x0000000455526818UL, + 0x0000000ea3a7a180UL, + 0x0000000a9536e5e0UL, + 0x00000009b64a1975UL, + 0x00000005bfc756bcUL, + 0x0000000046aa169bUL, + 0x000000053a17f76fUL, + 0x00000004d6815274UL, + 0x0000000cca9cf3f6UL, + 0x00000004013fcb8bUL, + 0x00000003d26cdfa5UL, + 0x00000005786231f7UL, + 0x00000007d4ab09abUL, + 0x0000000960b5ffbcUL, + 0x00000008914df0d4UL, + 0x00000002fc6f2213UL, + 0x0000000ac235637eUL, + 0x0000000151b28ed3UL, + 0x000000046f79b6dbUL, + 0x00000001382e0c9fUL, + 0x000000053abf983aUL, + 0x0000000383c47adeUL, + 0x00000003fcf88978UL, + 0x0000000eb9079df7UL, + 0x000000009af0714dUL, + 0x0000000da19d1bb7UL, + 0x00000009a02749f8UL, + 0x00000001c62dab9bUL, + 0x00000001a137e44bUL, + 0x00000002867718c7UL, + 0x000000035815525bUL, + 0x00000007cd35c550UL, + 0x00000002164f73a0UL, + 0x0000000e8b772fe0UL, +}; +apriltag_family_t *tag36h11_create(void) +{ + apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t)); + tf->name = strdup("tag36h11"); + tf->h = 11; + tf->ncodes = 587; + tf->codes = codedata; + tf->nbits = 36; + tf->bit_x = calloc(36, sizeof(uint32_t)); + tf->bit_y = calloc(36, sizeof(uint32_t)); + tf->bit_x[0] = 1; + tf->bit_y[0] = 1; + tf->bit_x[1] = 2; + tf->bit_y[1] = 1; + tf->bit_x[2] = 3; + tf->bit_y[2] = 1; + tf->bit_x[3] = 4; + tf->bit_y[3] = 1; + tf->bit_x[4] = 5; + tf->bit_y[4] = 1; + tf->bit_x[5] = 2; + tf->bit_y[5] = 2; + tf->bit_x[6] = 3; + tf->bit_y[6] = 2; + tf->bit_x[7] = 4; + tf->bit_y[7] = 2; + tf->bit_x[8] = 3; + tf->bit_y[8] = 3; + tf->bit_x[9] = 6; + tf->bit_y[9] = 1; + tf->bit_x[10] = 6; + tf->bit_y[10] = 2; + tf->bit_x[11] = 6; + tf->bit_y[11] = 3; + tf->bit_x[12] = 6; + tf->bit_y[12] = 4; + tf->bit_x[13] = 6; + tf->bit_y[13] = 5; + tf->bit_x[14] = 5; + tf->bit_y[14] = 2; + tf->bit_x[15] = 5; + tf->bit_y[15] = 3; + tf->bit_x[16] = 5; + tf->bit_y[16] = 4; + tf->bit_x[17] = 4; + tf->bit_y[17] = 3; + tf->bit_x[18] = 6; + tf->bit_y[18] = 6; + tf->bit_x[19] = 5; + tf->bit_y[19] = 6; + tf->bit_x[20] = 4; + tf->bit_y[20] = 6; + tf->bit_x[21] = 3; + tf->bit_y[21] = 6; + tf->bit_x[22] = 2; + tf->bit_y[22] = 6; + tf->bit_x[23] = 5; + tf->bit_y[23] = 5; + tf->bit_x[24] = 4; + tf->bit_y[24] = 5; + tf->bit_x[25] = 3; + tf->bit_y[25] = 5; + tf->bit_x[26] = 4; + tf->bit_y[26] = 4; + tf->bit_x[27] = 1; + tf->bit_y[27] = 6; + tf->bit_x[28] = 1; + tf->bit_y[28] = 5; + tf->bit_x[29] = 1; + tf->bit_y[29] = 4; + tf->bit_x[30] = 1; + tf->bit_y[30] = 3; + tf->bit_x[31] = 1; + tf->bit_y[31] = 2; + tf->bit_x[32] = 2; + tf->bit_y[32] = 5; + tf->bit_x[33] = 2; + tf->bit_y[33] = 4; + tf->bit_x[34] = 2; + tf->bit_y[34] = 3; + tf->bit_x[35] = 3; + tf->bit_y[35] = 4; + tf->width_at_border = 8; + tf->total_width = 10; + tf->reversed_border = false; + return tf; +} + +void tag36h11_destroy(apriltag_family_t *tf) +{ + free(tf->bit_x); + free(tf->bit_y); + free(tf->name); + free(tf); +} diff --git a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagDetectorTest.java b/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagDetectorTest.java index 475cbc0115..968184b001 100644 --- a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagDetectorTest.java +++ b/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagDetectorTest.java @@ -85,11 +85,6 @@ class AprilTagDetectorTest { assertDoesNotThrow(() -> detector.addFamily("tag16h5")); } - @Test - void testAdd25h9() { - assertDoesNotThrow(() -> detector.addFamily("tag25h9")); - } - @Test void testAdd36h11() { assertDoesNotThrow(() -> detector.addFamily("tag36h11")); diff --git a/apriltag/src/test/native/cpp/AprilTagDetectorTest.cpp b/apriltag/src/test/native/cpp/AprilTagDetectorTest.cpp index ab43563a6e..af7fc6ee52 100644 --- a/apriltag/src/test/native/cpp/AprilTagDetectorTest.cpp +++ b/apriltag/src/test/native/cpp/AprilTagDetectorTest.cpp @@ -41,11 +41,6 @@ TEST(AprilTagDetectorTest, Add16h5) { ASSERT_TRUE(detector.AddFamily("tag16h5")); } -TEST(AprilTagDetectorTest, Add25h9) { - AprilTagDetector detector; - ASSERT_TRUE(detector.AddFamily("tag25h9")); -} - TEST(AprilTagDetectorTest, Add36h11) { AprilTagDetector detector; ASSERT_TRUE(detector.AddFamily("tag36h11")); diff --git a/docs/build.gradle b/docs/build.gradle index 49b8f4cf7d..546f23604d 100644 --- a/docs/build.gradle +++ b/docs/build.gradle @@ -69,6 +69,9 @@ doxygen { cppIncludeRoots << '../ntcore/build/generated/main/native/include/' if (project.hasProperty('docWarningsAsErrors')) { + // apriltag + exclude 'apriltag_pose.h' + // Eigen exclude 'Eigen/**' exclude 'unsupported/**' diff --git a/shared/apriltaglib.gradle b/shared/apriltaglib.gradle deleted file mode 100644 index d0ebc6104b..0000000000 --- a/shared/apriltaglib.gradle +++ /dev/null @@ -1,13 +0,0 @@ -nativeUtils { - nativeDependencyContainer { - apriltaglib(getNativeDependencyTypeClass('WPIStaticMavenDependency')) { - groupId = "edu.wpi.first.thirdparty.frc2024" - artifactId = "apriltaglib" - headerClassifier = "headers" - sourceClassifier = "sources" - ext = "zip" - version = '3.3.0-2' - targetPlatforms.addAll(nativeUtils.wpi.platforms.allPlatforms) - } - } -} diff --git a/upstream_utils/apriltag.py b/upstream_utils/apriltag.py new file mode 100755 index 0000000000..3992693ab4 --- /dev/null +++ b/upstream_utils/apriltag.py @@ -0,0 +1,94 @@ +#!/usr/bin/env python3 + +import os +import shutil + +from upstream_utils import ( + comment_out_invalid_includes, + walk_cwd_and_copy_if, + Lib, +) + + +def remove_tag(f: str): + if "apriltag" in f: + return False + if "36h11" in f: + return False + if "16h5" in f: + return False + if "tag" in f: + return True + return False + + +def copy_upstream_src(wpilib_root): + apriltag = os.path.join(wpilib_root, "apriltag") + + # Delete old install + shutil.rmtree( + os.path.join(apriltag, "src/main/native/thirdparty/apriltag"), + ignore_errors=True, + ) + shutil.rmtree( + os.path.join(apriltag, "src/main/include/thirdparty/apriltag"), + ignore_errors=True, + ) + + # Copy apriltag source files into allwpilib + src_files = walk_cwd_and_copy_if( + lambda dp, f: (f.endswith(".c") or f.endswith(".cpp")) + and not dp.startswith("./example") + and not f.endswith("getopt.c") + and not "py" in f + and not remove_tag(f), + os.path.join(apriltag, "src/main/native/thirdparty/apriltag/src"), + ) + + # Copy apriltag header files into allwpilib + walk_cwd_and_copy_if( + lambda dp, f: f.endswith(".h") + and not f.endswith("getopt.h") + and not f.endswith("postscript_utils.h") + and not remove_tag(f), + os.path.join(apriltag, "src/main/native/thirdparty/apriltag/include"), + ) + + for f in src_files: + comment_out_invalid_includes( + f, + [ + os.path.join(apriltag, "src/main/native/thirdparty/apriltag/include"), + os.path.join( + apriltag, "src/main/native/thirdparty/apriltag/include/common" + ), + ], + ) + + +def main(): + name = "apriltag" + url = "https://github.com/AprilRobotics/apriltag.git" + tag = "ebdb2017e04b8e36f7d8a12ce60060416a905e12" + + patch_list = [ + "0001-apriltag_pose.c-Set-NULL-when-second-solution-could-.patch", + "0002-zmaxheapify-Avoid-return-of-void-expression.patch", + "0003-Avoid-unused-variable-warnings-in-release-builds.patch", + "0004-Make-orthogonal_iteration-exit-early-upon-convergenc.patch", + "0005-Fix-signed-left-shift-warning.patch", + "0006-Avoid-incompatible-pointer-warning.patch", + "0007-Fix-GCC-14-calloc-warning.patch", + "0008-Remove-calls-to-postscript_image.patch", + "0009-Fix-clang-16-warnings.patch", + ] + patch_options = { + "ignore_whitespace": True, + } + + apriltag = Lib(name, url, tag, patch_list, copy_upstream_src, patch_options) + apriltag.main() + + +if __name__ == "__main__": + main() diff --git a/upstream_utils/apriltag_patches/0001-apriltag_pose.c-Set-NULL-when-second-solution-could-.patch b/upstream_utils/apriltag_patches/0001-apriltag_pose.c-Set-NULL-when-second-solution-could-.patch new file mode 100644 index 0000000000..d073f4701a --- /dev/null +++ b/upstream_utils/apriltag_patches/0001-apriltag_pose.c-Set-NULL-when-second-solution-could-.patch @@ -0,0 +1,22 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Peter Johnson +Date: Sun, 4 Dec 2022 11:01:56 -0800 +Subject: [PATCH 1/9] apriltag_pose.c: Set NULL when second solution could not + be determined + +--- + apriltag_pose.c | 1 + + 1 file changed, 1 insertion(+) + +diff --git a/apriltag_pose.c b/apriltag_pose.c +index 6043cb857fbb28304b7e28419f62462f7f0bd05d..f0003a2d187df13236992026ee6ff7f9d6f7aff9 100644 +--- a/apriltag_pose.c ++++ b/apriltag_pose.c +@@ -513,6 +513,7 @@ void estimate_tag_pose_orthogonal_iteration( + solution2->t = matd_create(3, 1); + *err2 = orthogonal_iteration(v, p, &solution2->t, &solution2->R, 4, nIters); + } else { ++ solution2->t = NULL; + *err2 = HUGE_VAL; + } + diff --git a/upstream_utils/apriltag_patches/0002-zmaxheapify-Avoid-return-of-void-expression.patch b/upstream_utils/apriltag_patches/0002-zmaxheapify-Avoid-return-of-void-expression.patch new file mode 100644 index 0000000000..a58e5a6672 --- /dev/null +++ b/upstream_utils/apriltag_patches/0002-zmaxheapify-Avoid-return-of-void-expression.patch @@ -0,0 +1,23 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Peter Johnson +Date: Sun, 4 Dec 2022 11:25:12 -0800 +Subject: [PATCH 2/9] zmaxheapify: Avoid return of void expression + +--- + common/zmaxheap.c | 3 ++- + 1 file changed, 2 insertions(+), 1 deletion(-) + +diff --git a/common/zmaxheap.c b/common/zmaxheap.c +index e04d03efa8a79163293eb306e9f4beab6e4a27df..e410664fd57dfa5ebd30e0680d77b008bb41801c 100644 +--- a/common/zmaxheap.c ++++ b/common/zmaxheap.c +@@ -320,7 +320,8 @@ static void maxheapify(zmaxheap_t *heap, int parent) + + if (betterchild != parent) { + heap->swap(heap, parent, betterchild); +- return maxheapify(heap, betterchild); ++ maxheapify(heap, betterchild); ++ return; + } + } + diff --git a/upstream_utils/apriltag_patches/0003-Avoid-unused-variable-warnings-in-release-builds.patch b/upstream_utils/apriltag_patches/0003-Avoid-unused-variable-warnings-in-release-builds.patch new file mode 100644 index 0000000000..a07b53af90 --- /dev/null +++ b/upstream_utils/apriltag_patches/0003-Avoid-unused-variable-warnings-in-release-builds.patch @@ -0,0 +1,70 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Peter Johnson +Date: Sun, 4 Dec 2022 11:42:13 -0800 +Subject: [PATCH 3/9] Avoid unused variable warnings in release builds + +--- + common/matd.c | 4 +++- + common/pjpeg.c | 1 + + common/string_util.c | 1 + + common/zmaxheap.c | 1 + + 4 files changed, 6 insertions(+), 1 deletion(-) + +diff --git a/common/matd.c b/common/matd.c +index 54449d9f27a4dae6d1422c37c71bdeeb046c94ba..f426890416dd021d7392b78bd19959c70f44247b 100644 +--- a/common/matd.c ++++ b/common/matd.c +@@ -874,7 +874,8 @@ double matd_vec_dist_n(const matd_t *a, const matd_t *b, int n) + + int lena = a->nrows*a->ncols; + int lenb = b->nrows*b->ncols; +- ++ (void) lena; ++ (void) lenb; + assert(n <= lena && n <= lenb); + + double mag = 0.0; +@@ -909,6 +910,7 @@ double matd_vec_dot_product(const matd_t *a, const matd_t *b) + assert(matd_is_vector(a) && matd_is_vector(b)); + int adim = a->ncols*a->nrows; + int bdim = b->ncols*b->nrows; ++ (void) bdim; + assert(adim == bdim); + + double acc = 0; +diff --git a/common/pjpeg.c b/common/pjpeg.c +index 7e3d089ad849df67549a58873eb6574c85fda6ee..4a5dd028ee8a3d79fbe7a5abed30886cc6e40dae 100644 +--- a/common/pjpeg.c ++++ b/common/pjpeg.c +@@ -863,6 +863,7 @@ pjpeg_t *pjpeg_create_from_buffer(uint8_t *buf, int buflen, uint32_t flags, int + pjd.in = mjpeg_dht; + pjd.inlen = sizeof(mjpeg_dht); + int result = pjpeg_decode_buffer(&pjd); ++ (void) result; + assert(result == 0); + } + +diff --git a/common/string_util.c b/common/string_util.c +index 4f0c98056b084af62a9a04bc8425e16dbdf9041a..3d86eb2f2d602a5b00c29952808368dc73c0b0cd 100644 +--- a/common/string_util.c ++++ b/common/string_util.c +@@ -552,6 +552,7 @@ void string_feeder_require(string_feeder_t *sf, const char *str) + + for (int i = 0; i < len; i++) { + char c = string_feeder_next(sf); ++ (void) c; + assert(c == str[i]); + } + } +diff --git a/common/zmaxheap.c b/common/zmaxheap.c +index e410664fd57dfa5ebd30e0680d77b008bb41801c..2c671236bf07ed4c43e15c02f4bf10df76880cb9 100644 +--- a/common/zmaxheap.c ++++ b/common/zmaxheap.c +@@ -399,6 +399,7 @@ void zmaxheap_test() + float outfv; + int res = zmaxheap_remove_max(heap, &outv, &outfv); + if (sz == 0) { ++ (void) res; + assert(res == 0); + } else { + // printf("%d %d %d %f\n", sz, maxv, outv, outfv); diff --git a/upstream_utils/apriltag_patches/0004-Make-orthogonal_iteration-exit-early-upon-convergenc.patch b/upstream_utils/apriltag_patches/0004-Make-orthogonal_iteration-exit-early-upon-convergenc.patch new file mode 100644 index 0000000000..2a7c38f06c --- /dev/null +++ b/upstream_utils/apriltag_patches/0004-Make-orthogonal_iteration-exit-early-upon-convergenc.patch @@ -0,0 +1,141 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Tue, 10 Jan 2023 18:36:36 -0800 +Subject: [PATCH 4/9] Make orthogonal_iteration() exit early upon convergence + +The current approach wastes iterations doing no work. Exiting early can +give lower latencies and higher FPS. + +Co-authored-by: Matt +--- + apriltag_pose.c | 23 +++++++++++++++++------ + apriltag_pose.h | 3 ++- + example/apriltag_demo.c | 22 ++++++++++++++++++++++ + 3 files changed, 41 insertions(+), 7 deletions(-) + +diff --git a/apriltag_pose.c b/apriltag_pose.c +index f0003a2d187df13236992026ee6ff7f9d6f7aff9..782729225c3555edcfebb7d8a21f847aad0e328f 100644 +--- a/apriltag_pose.c ++++ b/apriltag_pose.c +@@ -35,12 +35,13 @@ double matd_to_double(matd_t *a) + * @param R In/Outparam. Should be set to initial guess at R. Will be modified to be the optimal translation. + * @param n_points Number of points. + * @param n_steps Number of iterations. ++ * @param min_improvement_per_iteration Min object-space error improvement; if less than this, solver will exit early + * + * @return Object-space error after iteration. + * + * Implementation of Orthogonal Iteration from Lu, 2000. + */ +-double orthogonal_iteration(matd_t** v, matd_t** p, matd_t** t, matd_t** R, int n_points, int n_steps) { ++double orthogonal_iteration(matd_t** v, matd_t** p, matd_t** t, matd_t** R, int n_points, int n_steps, double min_improvement_per_iteration) { + matd_t* p_mean = matd_create(3, 1); + for (int i = 0; i < n_points; i++) { + matd_add_inplace(p_mean, p[i]); +@@ -120,9 +121,16 @@ double orthogonal_iteration(matd_t** v, matd_t** p, matd_t** t, matd_t** R, int + error += matd_to_double(matd_op("M'M", err_vec, err_vec)); + matd_destroy(err_vec); + } +- prev_error = error; + + free(q); ++ ++ // Return early if the iterations converged ++ if (fabs(error - prev_error) < min_improvement_per_iteration) { ++ prev_error = error; ++ break; ++ } ++ ++ prev_error = error; + } + + matd_destroy(I3); +@@ -493,7 +501,8 @@ void estimate_tag_pose_orthogonal_iteration( + apriltag_pose_t* solution1, + double* err2, + apriltag_pose_t* solution2, +- int nIters) { ++ int nIters, ++ double min_improvement_per_iteration) { + double scale = info->tagsize/2.0; + matd_t* p[4] = { + matd_create_data(3, 1, (double[]) {-scale, scale, 0}), +@@ -507,11 +516,11 @@ void estimate_tag_pose_orthogonal_iteration( + } + + estimate_pose_for_tag_homography(info, solution1); +- *err1 = orthogonal_iteration(v, p, &solution1->t, &solution1->R, 4, nIters); ++ *err1 = orthogonal_iteration(v, p, &solution1->t, &solution1->R, 4, nIters, min_improvement_per_iteration); + solution2->R = fix_pose_ambiguities(v, p, solution1->t, solution1->R, 4); + if (solution2->R) { + solution2->t = matd_create(3, 1); +- *err2 = orthogonal_iteration(v, p, &solution2->t, &solution2->R, 4, nIters); ++ *err2 = orthogonal_iteration(v, p, &solution2->t, &solution2->R, 4, nIters, min_improvement_per_iteration); + } else { + solution2->t = NULL; + *err2 = HUGE_VAL; +@@ -529,7 +538,9 @@ void estimate_tag_pose_orthogonal_iteration( + double estimate_tag_pose(apriltag_detection_info_t* info, apriltag_pose_t* pose) { + double err1, err2; + apriltag_pose_t pose1, pose2; +- estimate_tag_pose_orthogonal_iteration(info, &err1, &pose1, &err2, &pose2, 50); ++ // 50 iterations is a good sensible default ++ // 1e-7 improvement per iteration is also pretty sane ++ estimate_tag_pose_orthogonal_iteration(info, &err1, &pose1, &err2, &pose2, 50, 1e-7); + if (err1 <= err2) { + pose->R = pose1.R; + pose->t = pose1.t; +diff --git a/apriltag_pose.h b/apriltag_pose.h +index 07ee37b2cb4185bcbdb46d1c9ccec306f0f2e96d..72993f11514199754c452369bad32157b6d64eb1 100644 +--- a/apriltag_pose.h ++++ b/apriltag_pose.h +@@ -63,7 +63,8 @@ void estimate_tag_pose_orthogonal_iteration( + apriltag_pose_t* pose1, + double* err2, + apriltag_pose_t* pose2, +- int nIters); ++ int nIters, ++ double min_improvement_per_iteration); + + /** + * Estimate tag pose. +diff --git a/example/apriltag_demo.c b/example/apriltag_demo.c +index 6de90540fe2f22f5160f725bce03d50bb3967c74..841d7788756011b0c7237e989242d87de76b14ef 100644 +--- a/example/apriltag_demo.c ++++ b/example/apriltag_demo.c +@@ -42,6 +42,7 @@ either expressed or implied, of the Regents of The University of Michigan. + #endif + + #include "apriltag.h" ++#include "apriltag_pose.h" + #include "tag36h11.h" + #include "tag25h9.h" + #include "tag16h5.h" +@@ -218,6 +219,27 @@ int main(int argc, char *argv[]) + + hamm_hist[det->hamming]++; + total_hamm_hist[det->hamming]++; ++ ++ apriltag_detection_info_t info = { det, 0.15, 1000, 1000, 1280/2, 720/2 }; ++ double err1 = HUGE_VAL; //Should get overwritten if pose estimation is happening ++ double err2 = HUGE_VAL; ++ apriltag_pose_t pose1 = { 0 }; ++ apriltag_pose_t pose2 = { 0 }; ++ int nIters = 200; ++ estimate_tag_pose_orthogonal_iteration(&info, &err1, &pose1, &err2, &pose2, nIters, 1e-7); ++ ++ printf("Primary translation %f %f %f\nerror: %f\n", ++ pose1.t->data[0], ++ pose1.t->data[1], ++ pose1.t->data[2], ++ err1 ++ ); ++ printf("Alt translation %f %f %f\nerror: %f\n", ++ pose2.t->data[0], ++ pose2.t->data[1], ++ pose2.t->data[2], ++ err2 ++ ); + } + + apriltag_detections_destroy(detections); diff --git a/upstream_utils/apriltag_patches/0005-Fix-signed-left-shift-warning.patch b/upstream_utils/apriltag_patches/0005-Fix-signed-left-shift-warning.patch new file mode 100644 index 0000000000..d621a45705 --- /dev/null +++ b/upstream_utils/apriltag_patches/0005-Fix-signed-left-shift-warning.patch @@ -0,0 +1,31 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Peter Johnson +Date: Wed, 19 Jul 2023 20:48:21 -0700 +Subject: [PATCH 5/9] Fix signed left shift warning + +--- + common/pjpeg.c | 4 ++-- + 1 file changed, 2 insertions(+), 2 deletions(-) + +diff --git a/common/pjpeg.c b/common/pjpeg.c +index 4a5dd028ee8a3d79fbe7a5abed30886cc6e40dae..1941c27b7752a6a2420e8370451b0d29268f324d 100644 +--- a/common/pjpeg.c ++++ b/common/pjpeg.c +@@ -594,7 +594,7 @@ static int pjpeg_decode_buffer(struct pjpeg_decode_state *pjd) + + // if high bit is clear, it's negative + if ((value & (1 << (ssss-1))) == 0) +- value += ((-1) << ssss) + 1; ++ value += (int32_t)(UINT32_MAX << ssss) + 1; + + dcpred[nsidx] += value; + block[0] = dcpred[nsidx] * pjd->qtab[qtabidx][0]; +@@ -620,7 +620,7 @@ static int pjpeg_decode_buffer(struct pjpeg_decode_state *pjd) + + // if high bit is clear, it's negative + if ((value & (1 << (ssss-1))) == 0) +- value += ((-1) << ssss) + 1; ++ value += (int32_t)(UINT32_MAX << ssss) + 1; + + coeff += rrrr; + diff --git a/upstream_utils/apriltag_patches/0006-Avoid-incompatible-pointer-warning.patch b/upstream_utils/apriltag_patches/0006-Avoid-incompatible-pointer-warning.patch new file mode 100644 index 0000000000..55825224e1 --- /dev/null +++ b/upstream_utils/apriltag_patches/0006-Avoid-incompatible-pointer-warning.patch @@ -0,0 +1,24 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Peter Johnson +Date: Wed, 19 Jul 2023 21:28:43 -0700 +Subject: [PATCH 6/9] Avoid incompatible pointer warning + +--- + common/getopt.c | 3 ++- + 1 file changed, 2 insertions(+), 1 deletion(-) + +diff --git a/common/getopt.c b/common/getopt.c +index 7613b69c346f3f818688bb9f4704463367d877f6..71ae57bd83b2ed50c7f80f3e3952ddfcc53cb7bc 100644 +--- a/common/getopt.c ++++ b/common/getopt.c +@@ -76,8 +76,9 @@ getopt_t *getopt_create() + return gopt; + } + +-void getopt_option_destroy(getopt_option_t *goo) ++void getopt_option_destroy(void *vgoo) + { ++ getopt_option_t* goo = (getopt_option_t*)vgoo; + free(goo->sname); + free(goo->lname); + free(goo->svalue); diff --git a/upstream_utils/apriltag_patches/0007-Fix-GCC-14-calloc-warning.patch b/upstream_utils/apriltag_patches/0007-Fix-GCC-14-calloc-warning.patch new file mode 100644 index 0000000000..04efe330f1 --- /dev/null +++ b/upstream_utils/apriltag_patches/0007-Fix-GCC-14-calloc-warning.patch @@ -0,0 +1,32 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Sun, 23 Jun 2024 05:53:42 -0700 +Subject: [PATCH 7/9] Fix GCC 14 calloc() warning +MIME-Version: 1.0 +Content-Type: text/plain; charset=UTF-8 +Content-Transfer-Encoding: 8bit + +``` +/home/tav/frc/wpilib/allwpilib/build-cmake-release/_deps/apriltaglib-src/common/zmaxheap.c: In function ‘zmaxheap_test’: +/home/tav/frc/wpilib/allwpilib/build-cmake-release/_deps/apriltaglib-src/common/zmaxheap.c:365:35: error: ‘calloc’ sizes specified with ‘sizeof’ in the earlier argument and not in the later argument [-Werror=calloc-transposed-args] + 365 | int32_t *vals = calloc(sizeof(int32_t), cap); + | ^~~~~~~ +/home/tav/frc/wpilib/allwpilib/build-cmake-release/_deps/apriltaglib-src/common/zmaxheap.c:365:35: note: earlier argument should specify number of elements, later size of each element +``` +--- + common/zmaxheap.c | 2 +- + 1 file changed, 1 insertion(+), 1 deletion(-) + +diff --git a/common/zmaxheap.c b/common/zmaxheap.c +index 2c671236bf07ed4c43e15c02f4bf10df76880cb9..ed271420d229a98b56c7027f47d9830a78a97db1 100644 +--- a/common/zmaxheap.c ++++ b/common/zmaxheap.c +@@ -362,7 +362,7 @@ void zmaxheap_test() + { + int cap = 10000; + int sz = 0; +- int32_t *vals = calloc(sizeof(int32_t), cap); ++ int32_t *vals = calloc(cap, sizeof(int32_t)); + + zmaxheap_t *heap = zmaxheap_create(sizeof(int32_t)); + diff --git a/upstream_utils/apriltag_patches/0008-Remove-calls-to-postscript_image.patch b/upstream_utils/apriltag_patches/0008-Remove-calls-to-postscript_image.patch new file mode 100644 index 0000000000..3be4beca59 --- /dev/null +++ b/upstream_utils/apriltag_patches/0008-Remove-calls-to-postscript_image.patch @@ -0,0 +1,61 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Fri, 19 Jul 2024 21:45:29 -0700 +Subject: [PATCH 8/9] Remove calls to postscript_image() + +--- + apriltag.c | 5 ----- + apriltag_quad_thresh.c | 3 --- + 2 files changed, 8 deletions(-) + +diff --git a/apriltag.c b/apriltag.c +index 3086228868281eaad5cc5382d305227757d4a5cf..b514d974971827e6a4f4f0a4fa12e6b0392d7282 100644 +--- a/apriltag.c ++++ b/apriltag.c +@@ -51,8 +51,6 @@ either expressed or implied, of the Regents of The University of Michigan. + + #include "apriltag_math.h" + +-#include "common/postscript_utils.h" +- + #ifndef M_PI + # define M_PI 3.141592653589793238462643383279502884196 + #endif +@@ -1282,7 +1280,6 @@ zarray_t *apriltag_detector_detect(apriltag_detector_t *td, image_u8_t *im_orig) + fprintf(f, "%f %f scale\n", scale, scale); + fprintf(f, "0 %d translate\n", darker->height); + fprintf(f, "1 -1 scale\n"); +- postscript_image(f, darker); + + image_u8_destroy(darker); + +@@ -1365,8 +1362,6 @@ zarray_t *apriltag_detector_detect(apriltag_detector_t *td, image_u8_t *im_orig) + fprintf(f, "0 %d translate\n", darker->height); + fprintf(f, "1 -1 scale\n"); + +- postscript_image(f, darker); +- + image_u8_destroy(darker); + + for (int i = 0; i < zarray_size(quads); i++) { +diff --git a/apriltag_quad_thresh.c b/apriltag_quad_thresh.c +index 735520c6b1dd7387c837e8922d2ecd68130b9b5c..677f365765493bcbf043b0841bb28016a0d3acde 100644 +--- a/apriltag_quad_thresh.c ++++ b/apriltag_quad_thresh.c +@@ -40,7 +40,6 @@ either expressed or implied, of the Regents of The University of Michigan. + #include "common/unionfind.h" + #include "common/timeprofile.h" + #include "common/zmaxheap.h" +-#include "common/postscript_utils.h" + #include "common/math_util.h" + + #ifdef _WIN32 +@@ -1979,8 +1978,6 @@ zarray_t *apriltag_quad_thresh(apriltag_detector_t *td, image_u8_t *im) + fprintf(f, "0 %d translate\n", im2->height); + fprintf(f, "1 -1 scale\n"); + +- postscript_image(f, im2); +- + image_u8_destroy(im2); + + for (int i = 0; i < zarray_size(quads); i++) { diff --git a/upstream_utils/apriltag_patches/0009-Fix-clang-16-warnings.patch b/upstream_utils/apriltag_patches/0009-Fix-clang-16-warnings.patch new file mode 100644 index 0000000000..4fcf7f6793 --- /dev/null +++ b/upstream_utils/apriltag_patches/0009-Fix-clang-16-warnings.patch @@ -0,0 +1,691 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Peter Johnson +Date: Thu, 29 Jun 2023 22:14:05 -0700 +Subject: [PATCH 9/9] Fix clang 16 warnings + +--- + apriltag.c | 2 +- + apriltag.h | 2 +- + common/g2d.c | 2 +- + common/g2d.h | 2 +- + common/getopt.c | 2 +- + common/getopt.h | 2 +- + common/math_util.h | 4 ++-- + common/pthreads_cross.cpp | 6 +++--- + common/pthreads_cross.h | 2 +- + common/string_util.c | 2 +- + common/string_util.h | 2 +- + common/time_util.c | 4 ++-- + common/time_util.h | 4 ++-- + common/timeprofile.h | 2 +- + common/workerpool.c | 8 ++++---- + common/workerpool.h | 2 +- + common/zarray.c | 2 +- + common/zarray.h | 2 +- + common/zhash.c | 8 ++++---- + common/zhash.h | 8 ++++---- + common/zmaxheap.c | 4 ++-- + common/zmaxheap.h | 2 +- + tag16h5.c | 2 +- + tag16h5.h | 2 +- + tag25h9.c | 2 +- + tag25h9.h | 2 +- + tag36h10.c | 2 +- + tag36h10.h | 2 +- + tag36h11.c | 2 +- + tag36h11.h | 2 +- + tagCircle21h7.c | 2 +- + tagCircle21h7.h | 2 +- + tagCircle49h12.c | 2 +- + tagCircle49h12.h | 2 +- + tagCustom48h12.c | 2 +- + tagCustom48h12.h | 2 +- + tagStandard41h12.c | 2 +- + tagStandard41h12.h | 2 +- + tagStandard52h13.c | 2 +- + tagStandard52h13.h | 2 +- + 40 files changed, 55 insertions(+), 55 deletions(-) + +diff --git a/apriltag.c b/apriltag.c +index b514d974971827e6a4f4f0a4fa12e6b0392d7282..bfff1ea9d502b8ef08d00e61d4c75a967c06462b 100644 +--- a/apriltag.c ++++ b/apriltag.c +@@ -352,7 +352,7 @@ void apriltag_detector_clear_families(apriltag_detector_t *td) + zarray_clear(td->tag_families); + } + +-apriltag_detector_t *apriltag_detector_create() ++apriltag_detector_t *apriltag_detector_create(void) + { + apriltag_detector_t *td = (apriltag_detector_t*) calloc(1, sizeof(apriltag_detector_t)); + +diff --git a/apriltag.h b/apriltag.h +index 2d772cda60185de81fb279e83134074284b8e491..eb58a71d976ee84b94a232e3c778120a52f2b609 100644 +--- a/apriltag.h ++++ b/apriltag.h +@@ -231,7 +231,7 @@ struct apriltag_detection + }; + + // don't forget to add a family! +-apriltag_detector_t *apriltag_detector_create(); ++apriltag_detector_t *apriltag_detector_create(void); + + // add a family to the apriltag detector. caller still "owns" the family. + // a single instance should only be provided to one apriltag detector instance. +diff --git a/common/g2d.c b/common/g2d.c +index 4645f206e52ff14c20c04ec9601d20725339197b..e0106437717b872b8b9823135c076da7ba0a1e84 100644 +--- a/common/g2d.c ++++ b/common/g2d.c +@@ -45,7 +45,7 @@ double g2d_distance(const double a[2], const double b[2]) + return sqrtf(sq(a[0]-b[0]) + sq(a[1]-b[1])); + } + +-zarray_t *g2d_polygon_create_empty() ++zarray_t *g2d_polygon_create_empty(void) + { + return zarray_create(sizeof(double[2])); + } +diff --git a/common/g2d.h b/common/g2d.h +index 21c21ac64d8988578f3c2d108a7a07d54e780954..5a0dd0b59869099e24453eda78ada1e565fe7aae 100644 +--- a/common/g2d.h ++++ b/common/g2d.h +@@ -96,7 +96,7 @@ zarray_t *g2d_polygon_create_data(double v[][2], int sz); + + zarray_t *g2d_polygon_create_zeros(int sz); + +-zarray_t *g2d_polygon_create_empty(); ++zarray_t *g2d_polygon_create_empty(void); + + void g2d_polygon_add(zarray_t *poly, double v[2]); + +diff --git a/common/getopt.c b/common/getopt.c +index 71ae57bd83b2ed50c7f80f3e3952ddfcc53cb7bc..458ef7705019b73dcd4c9fa03fe15555ea087fcf 100644 +--- a/common/getopt.c ++++ b/common/getopt.c +@@ -64,7 +64,7 @@ struct getopt + zarray_t *options; + }; + +-getopt_t *getopt_create() ++getopt_t *getopt_create(void) + { + getopt_t *gopt = (getopt_t*) calloc(1, sizeof(getopt_t)); + +diff --git a/common/getopt.h b/common/getopt.h +index 69dbb05c8286a9d5d301382fc44fdea8c8f7875e..75266f033a0485b22cc34ec08e894781b6658894 100644 +--- a/common/getopt.h ++++ b/common/getopt.h +@@ -36,7 +36,7 @@ extern "C" { + + typedef struct getopt getopt_t; + +-getopt_t *getopt_create(); ++getopt_t *getopt_create(void); + void getopt_destroy(getopt_t *gopt); + + // Parse args. Returns 1 on success +diff --git a/common/math_util.h b/common/math_util.h +index 9271a01834501a311f28525ca460f61117524eab..5434355a512c1a6225bbfbf335caa279d17ed7aa 100644 +--- a/common/math_util.h ++++ b/common/math_util.h +@@ -86,13 +86,13 @@ static inline double sgn(double v) + } + + // random number between [0, 1) +-static inline float randf() ++static inline float randf(void) + { + return (float)(rand() / (RAND_MAX + 1.0)); + } + + +-static inline float signed_randf() ++static inline float signed_randf(void) + { + return randf()*2 - 1; + } +diff --git a/common/pthreads_cross.cpp b/common/pthreads_cross.cpp +index f7721912f9088bf84224943aa836a69356949f18..09d6ce6e97074f4ffbcef1ef6e06fd53ee22ff0f 100644 +--- a/common/pthreads_cross.cpp ++++ b/common/pthreads_cross.cpp +@@ -216,7 +216,7 @@ int pthread_rwlock_unlock(pthread_rwlock_t *rwlock) + return 0; + } + +-int sched_yield() { ++int sched_yield(void) { + return (int)SwitchToThread(); + } + +@@ -241,7 +241,7 @@ unsigned int timespec_to_ms(const struct timespec *abstime) + return t; + } + +-unsigned int pcthread_get_num_procs() ++unsigned int pcthread_get_num_procs(void) + { + SYSTEM_INFO sysinfo; + +@@ -252,7 +252,7 @@ unsigned int pcthread_get_num_procs() + #else + + #include +-unsigned int pcthread_get_num_procs() ++unsigned int pcthread_get_num_procs(void) + { + return (unsigned int)sysconf(_SC_NPROCESSORS_ONLN); + } +diff --git a/common/pthreads_cross.h b/common/pthreads_cross.h +index 897a333573e88263b6ba58ec3d31031304c50e54..209c28f855efcbdf41424f11a302ba2794836796 100644 +--- a/common/pthreads_cross.h ++++ b/common/pthreads_cross.h +@@ -70,7 +70,7 @@ int sched_yield(void); + #ifdef __cplusplus + extern "C" { + #endif +-unsigned int pcthread_get_num_procs(); ++unsigned int pcthread_get_num_procs(void); + + void ms_to_timespec(struct timespec *ts, unsigned int ms); + unsigned int timespec_to_ms(const struct timespec *abstime); +diff --git a/common/string_util.c b/common/string_util.c +index 3d86eb2f2d602a5b00c29952808368dc73c0b0cd..14726e7a7f66a330b28ef326734cd7fd25e8d4dd 100644 +--- a/common/string_util.c ++++ b/common/string_util.c +@@ -314,7 +314,7 @@ char *str_touppercase(char *s) + return s; + } + +-string_buffer_t* string_buffer_create() ++string_buffer_t* string_buffer_create(void) + { + string_buffer_t *sb = (string_buffer_t*) calloc(1, sizeof(string_buffer_t)); + assert(sb != NULL); +diff --git a/common/string_util.h b/common/string_util.h +index 9a7cd1e4509e3eba102316330cd50df918f56aed..d0363b03f23a94c35ed3581ea40f6d8f3f2d4110 100644 +--- a/common/string_util.h ++++ b/common/string_util.h +@@ -262,7 +262,7 @@ char *str_replace(const char *haystack, const char *needle, const char *replacem + * It is the caller's responsibility to free the string buffer resources with + * a call to string_buffer_destroy() when it is no longer needed. + */ +-string_buffer_t *string_buffer_create(); ++string_buffer_t *string_buffer_create(void); + + /** + * Frees the resources associated with a string buffer object, including space +diff --git a/common/time_util.c b/common/time_util.c +index 7a25f424068d798a8c65c69c6c17dd05b2e2b950..f3e3b0849bb8442ca1be6fda29ffcc79d52ec949 100644 +--- a/common/time_util.c ++++ b/common/time_util.c +@@ -35,7 +35,7 @@ struct timeutil_rest + int64_t start_time; + }; + +-timeutil_rest_t *timeutil_rest_create() ++timeutil_rest_t *timeutil_rest_create(void) + { + timeutil_rest_t *rest = calloc(1, sizeof(timeutil_rest_t)); + return rest; +@@ -46,7 +46,7 @@ void timeutil_rest_destroy(timeutil_rest_t *rest) + free(rest); + } + +-int64_t utime_now() // blacklist-ignore ++int64_t utime_now(void) // blacklist-ignore + { + struct timeval tv; + gettimeofday (&tv, NULL); // blacklist-ignore +diff --git a/common/time_util.h b/common/time_util.h +index 207e95838126a503567bf21b4bc8a3f32774afd8..c42466b3a0acdd322bd33aa9c77df09adc6585f1 100644 +--- a/common/time_util.h ++++ b/common/time_util.h +@@ -56,10 +56,10 @@ extern "C" { + #endif + + typedef struct timeutil_rest timeutil_rest_t; +-timeutil_rest_t *timeutil_rest_create(); ++timeutil_rest_t *timeutil_rest_create(void); + void timeutil_rest_destroy(timeutil_rest_t * rest); + +-int64_t utime_now(); // blacklist-ignore ++int64_t utime_now(void); // blacklist-ignore + int64_t utime_get_seconds(int64_t v); + int64_t utime_get_useconds(int64_t v); + void utime_to_timeval(int64_t v, struct timeval *tv); +diff --git a/common/timeprofile.h b/common/timeprofile.h +index 8016386ed214de371eae619f4e887aef01852408..197d6ceb330a4414483bd9db66e091d4fe911bf2 100644 +--- a/common/timeprofile.h ++++ b/common/timeprofile.h +@@ -51,7 +51,7 @@ struct timeprofile + zarray_t *stamps; + }; + +-static inline timeprofile_t *timeprofile_create() ++static inline timeprofile_t *timeprofile_create(void) + { + timeprofile_t *tp = (timeprofile_t*) calloc(1, sizeof(timeprofile_t)); + tp->stamps = zarray_create(sizeof(struct timeprofile_entry)); +diff --git a/common/workerpool.c b/common/workerpool.c +index a0170ef87978cb4cd2d3d8a198ffea471b6e40b2..23415eb5610751ae7f861f1deb7b25c8e56774a3 100644 +--- a/common/workerpool.c ++++ b/common/workerpool.c +@@ -66,7 +66,7 @@ void *worker_thread(void *p) + { + workerpool_t *wp = (workerpool_t*) p; + +- int cnt = 0; ++// int cnt = 0; + + while (1) { + struct task *task; +@@ -77,13 +77,13 @@ void *worker_thread(void *p) + // printf("%"PRId64" thread %d did %d\n", utime_now(), pthread_self(), cnt); + pthread_cond_broadcast(&wp->endcond); + pthread_cond_wait(&wp->startcond, &wp->mutex); +- cnt = 0; ++// cnt = 0; + // printf("%"PRId64" thread %d awake\n", utime_now(), pthread_self()); + } + + zarray_get_volatile(wp->tasks, wp->taskspos, &task); + wp->taskspos++; +- cnt++; ++// cnt++; + pthread_mutex_unlock(&wp->mutex); + // pthread_yield(); + sched_yield(); +@@ -203,7 +203,7 @@ void workerpool_run(workerpool_t *wp) + } + } + +-int workerpool_get_nprocs() ++int workerpool_get_nprocs(void) + { + #ifdef WIN32 + SYSTEM_INFO sysinfo; +diff --git a/common/workerpool.h b/common/workerpool.h +index 2c32ab1eb7f9fe1e803aef187d4af3aea9284238..070a983cbb0ce24450297dba2f58a903977c5a24 100644 +--- a/common/workerpool.h ++++ b/common/workerpool.h +@@ -46,4 +46,4 @@ void workerpool_run_single(workerpool_t *wp); + + int workerpool_get_nthreads(workerpool_t *wp); + +-int workerpool_get_nprocs(); ++int workerpool_get_nprocs(void); +diff --git a/common/zarray.c b/common/zarray.c +index 43e6a7e0c6fb6d06414d6910e511d362ccc35354..fa1f8a25b6cad5661d70aa81b40b991126a9ac7c 100644 +--- a/common/zarray.c ++++ b/common/zarray.c +@@ -41,7 +41,7 @@ int zstrcmp(const void * a_pp, const void * b_pp) + return strcmp(a,b); + } + +-void zarray_vmap(zarray_t *za, void (*f)()) ++void zarray_vmap(zarray_t *za, void (*f)(void*)) + { + assert(za != NULL); + assert(f != NULL); +diff --git a/common/zarray.h b/common/zarray.h +index 22b4c2bb4773d5d80e33c14a7b4945ab18586939..411ee7b6495b4b37813d08dc42848ce96734fb39 100644 +--- a/common/zarray.h ++++ b/common/zarray.h +@@ -355,7 +355,7 @@ static inline void zarray_map(zarray_t *za, void (*f)(void*)) + * + * void map_function(element_type *element) + */ +- void zarray_vmap(zarray_t *za, void (*f)()); ++ void zarray_vmap(zarray_t *za, void (*f)(void*)); + + /** + * Removes all elements from the array and sets its size to zero. Pointers to +diff --git a/common/zhash.c b/common/zhash.c +index faf52233ed92f64b87ddbab255121c8202b13019..10eb6eb8a6e3ce70e51a117009dcbea3aa381d4c 100644 +--- a/common/zhash.c ++++ b/common/zhash.c +@@ -352,7 +352,7 @@ void zhash_iterator_remove(zhash_iterator_t *zit) + zit->last_entry--; + } + +-void zhash_map_keys(zhash_t *zh, void (*f)()) ++void zhash_map_keys(zhash_t *zh, void (*f)(void *)) + { + assert(zh != NULL); + if (f == NULL) +@@ -368,7 +368,7 @@ void zhash_map_keys(zhash_t *zh, void (*f)()) + } + } + +-void zhash_vmap_keys(zhash_t * zh, void (*f)()) ++void zhash_vmap_keys(zhash_t * zh, void (*f)(void *)) + { + assert(zh != NULL); + if (f == NULL) +@@ -385,7 +385,7 @@ void zhash_vmap_keys(zhash_t * zh, void (*f)()) + } + } + +-void zhash_map_values(zhash_t * zh, void (*f)()) ++void zhash_map_values(zhash_t * zh, void (*f)(void *)) + { + assert(zh != NULL); + if (f == NULL) +@@ -400,7 +400,7 @@ void zhash_map_values(zhash_t * zh, void (*f)()) + } + } + +-void zhash_vmap_values(zhash_t * zh, void (*f)()) ++void zhash_vmap_values(zhash_t * zh, void (*f)(void *)) + { + assert(zh != NULL); + if (f == NULL) +diff --git a/common/zhash.h b/common/zhash.h +index f3dee1aa40f6f2717fa23d7e7f856bd8864a99b3..9993a66e4aed22d998d58e10a5d0db0193fcff95 100644 +--- a/common/zhash.h ++++ b/common/zhash.h +@@ -259,7 +259,7 @@ void zhash_iterator_remove(zhash_iterator_t *zit); + * for the key, which the caller should not modify, as the hash table will not be + * re-indexed. The function may be NULL, in which case no action is taken. + */ +-void zhash_map_keys(zhash_t *zh, void (*f)()); ++void zhash_map_keys(zhash_t *zh, void (*f)(void *)); + + /** + * Calls the supplied function with a pointer to every value in the hash table in +@@ -267,7 +267,7 @@ void zhash_map_keys(zhash_t *zh, void (*f)()); + * for the value, which the caller may safely modify. The function may be NULL, + * in which case no action is taken. + */ +-void zhash_map_values(zhash_t *zh, void (*f)()); ++void zhash_map_values(zhash_t *zh, void (*f)(void *)); + + /** + * Calls the supplied function with a copy of every key in the hash table in +@@ -280,7 +280,7 @@ void zhash_map_values(zhash_t *zh, void (*f)()); + * Use with non-pointer keys (i.e. integer, double, etc.) will likely cause a + * segmentation fault. + */ +-void zhash_vmap_keys(zhash_t *vh, void (*f)()); ++void zhash_vmap_keys(zhash_t *vh, void (*f)(void *)); + + /** + * Calls the supplied function with a copy of every value in the hash table in +@@ -293,7 +293,7 @@ void zhash_vmap_keys(zhash_t *vh, void (*f)()); + * Use with non-pointer values (i.e. integer, double, etc.) will likely cause a + * segmentation fault. + */ +-void zhash_vmap_values(zhash_t *vh, void (*f)()); ++void zhash_vmap_values(zhash_t *vh, void (*f)(void *)); + + /** + * Returns an array which contains copies of all of the hash table's keys, in no +diff --git a/common/zmaxheap.c b/common/zmaxheap.c +index ed271420d229a98b56c7027f47d9830a78a97db1..a073ae5a17fdd5f11f4e17d56cc05548113f6a6c 100644 +--- a/common/zmaxheap.c ++++ b/common/zmaxheap.c +@@ -167,7 +167,7 @@ void zmaxheap_add(zmaxheap_t *heap, void *p, float v) + } + } + +-void zmaxheap_vmap(zmaxheap_t *heap, void (*f)()) ++void zmaxheap_vmap(zmaxheap_t *heap, void (*f)(void*)) + { + assert(heap != NULL); + assert(f != NULL); +@@ -358,7 +358,7 @@ void zmaxheap_iterator_finish(zmaxheap_iterator_t *it) + maxheapify(heap, i); + } + +-void zmaxheap_test() ++void zmaxheap_test(void) + { + int cap = 10000; + int sz = 0; +diff --git a/common/zmaxheap.h b/common/zmaxheap.h +index f0020f92e7a46c4814f30d77c189931e0e276d18..af9d1458c518e365828a5dd3bb7050cf57d2ce9b 100644 +--- a/common/zmaxheap.h ++++ b/common/zmaxheap.h +@@ -39,7 +39,7 @@ struct zmaxheap_iterator { + + zmaxheap_t *zmaxheap_create(size_t el_sz); + +-void zmaxheap_vmap(zmaxheap_t *heap, void (*f)()); ++void zmaxheap_vmap(zmaxheap_t *heap, void (*f)(void*)); + + void zmaxheap_destroy(zmaxheap_t *heap); + +diff --git a/tag16h5.c b/tag16h5.c +index 775f33c7e2d91af83e4a62949cadebdce34e3eb9..e38302a1d2ca2a2344a775569aed266d063a8861 100644 +--- a/tag16h5.c ++++ b/tag16h5.c +@@ -60,7 +60,7 @@ static uint64_t codedata[30] = { + 0x000000000000154dUL, + 0x000000000000b57aUL, + }; +-apriltag_family_t *tag16h5_create() ++apriltag_family_t *tag16h5_create(void) + { + apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t)); + tf->name = strdup("tag16h5"); +diff --git a/tag16h5.h b/tag16h5.h +index d868c81839d0f982ed1f7de129efe3a3a7d92c71..6661080b66cae147b2b63be5d6d2d0b8b4f2a85d 100644 +--- a/tag16h5.h ++++ b/tag16h5.h +@@ -34,7 +34,7 @@ either expressed or implied, of the Regents of The University of Michigan. + extern "C" { + #endif + +-apriltag_family_t *tag16h5_create(); ++apriltag_family_t *tag16h5_create(void); + void tag16h5_destroy(apriltag_family_t *tf); + + #ifdef __cplusplus +diff --git a/tag25h9.c b/tag25h9.c +index ddf31a2e45cb64934000f42d15ba62619ee9c68e..2478f4ab17bbbe6e224e14ed2dcd593eb0dbba9f 100644 +--- a/tag25h9.c ++++ b/tag25h9.c +@@ -65,7 +65,7 @@ static uint64_t codedata[35] = { + 0x0000000001bcc0f6UL, + 0x0000000000a64f8dUL, + }; +-apriltag_family_t *tag25h9_create() ++apriltag_family_t *tag25h9_create(void) + { + apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t)); + tf->name = strdup("tag25h9"); +diff --git a/tag25h9.h b/tag25h9.h +index 9197c8b367d4b2047cb7d9882dfc11092f0b3dce..6e0107d28f3c3da6ae148a90050774277147addd 100644 +--- a/tag25h9.h ++++ b/tag25h9.h +@@ -34,7 +34,7 @@ either expressed or implied, of the Regents of The University of Michigan. + extern "C" { + #endif + +-apriltag_family_t *tag25h9_create(); ++apriltag_family_t *tag25h9_create(void); + void tag25h9_destroy(apriltag_family_t *tf); + + #ifdef __cplusplus +diff --git a/tag36h10.c b/tag36h10.c +index 44a129e74b135e803e2324cea179ac3001718e14..843dc4e9dcb3fbde3d86ce0a7d18703f7c358c15 100644 +--- a/tag36h10.c ++++ b/tag36h10.c +@@ -2323,7 +2323,7 @@ static uint64_t codedata[2320] = { + 0x0000000447b9e7acUL, + 0x0000000d9f564f30UL, + }; +-apriltag_family_t *tag36h10_create() ++apriltag_family_t *tag36h10_create(void) + { + apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t)); + tf->name = strdup("tag36h10"); +diff --git a/tag36h10.h b/tag36h10.h +index a60ae6c5ade78804ed2ba406fcbef83a086e32c1..cc8a2805a1836550d5a0530497ae007797fb71b6 100644 +--- a/tag36h10.h ++++ b/tag36h10.h +@@ -34,7 +34,7 @@ either expressed or implied, of the Regents of The University of Michigan. + extern "C" { + #endif + +-apriltag_family_t *tag36h10_create(); ++apriltag_family_t *tag36h10_create(void); + void tag36h10_destroy(apriltag_family_t *tf); + + #ifdef __cplusplus +diff --git a/tag36h11.c b/tag36h11.c +index 94acacaaa473d5b1bc530dee625eb7ea388f4928..f1347a5bc141ff88a831147114c0d78c374d9607 100644 +--- a/tag36h11.c ++++ b/tag36h11.c +@@ -617,7 +617,7 @@ static uint64_t codedata[587] = { + 0x00000002164f73a0UL, + 0x0000000e8b772fe0UL, + }; +-apriltag_family_t *tag36h11_create() ++apriltag_family_t *tag36h11_create(void) + { + apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t)); + tf->name = strdup("tag36h11"); +diff --git a/tag36h11.h b/tag36h11.h +index 620387822dfd1a43fac265dd3609ead848bdeb2e..45f9e5698e2657f10e7887ae1ee67debf0d69de7 100644 +--- a/tag36h11.h ++++ b/tag36h11.h +@@ -34,7 +34,7 @@ either expressed or implied, of the Regents of The University of Michigan. + extern "C" { + #endif + +-apriltag_family_t *tag36h11_create(); ++apriltag_family_t *tag36h11_create(void); + void tag36h11_destroy(apriltag_family_t *tf); + + #ifdef __cplusplus +diff --git a/tagCircle21h7.c b/tagCircle21h7.c +index 8dad3feb11b3e9d9cea9e2863bec087b806bd3d2..9459176e00ad898edc85c3fcb01a0985fc242f5a 100644 +--- a/tagCircle21h7.c ++++ b/tagCircle21h7.c +@@ -68,7 +68,7 @@ static uint64_t codedata[38] = { + 0x0000000000015b39UL, + 0x00000000001ec1e3UL, + }; +-apriltag_family_t *tagCircle21h7_create() ++apriltag_family_t *tagCircle21h7_create(void) + { + apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t)); + tf->name = strdup("tagCircle21h7"); +diff --git a/tagCircle21h7.h b/tagCircle21h7.h +index a051db64f9119983d95e59bd5f2569c78435cd70..f002a51abde4cc27a7113d6822be2d7a71cad5c3 100644 +--- a/tagCircle21h7.h ++++ b/tagCircle21h7.h +@@ -34,7 +34,7 @@ either expressed or implied, of the Regents of The University of Michigan. + extern "C" { + #endif + +-apriltag_family_t *tagCircle21h7_create(); ++apriltag_family_t *tagCircle21h7_create(void); + void tagCircle21h7_destroy(apriltag_family_t *tf); + + #ifdef __cplusplus +diff --git a/tagCircle49h12.c b/tagCircle49h12.c +index 02456fab0ff32fba8d9ed5fcca0a1e09a4017228..c703431e3d7237d22b8231f72b80b63ae3702027 100644 +--- a/tagCircle49h12.c ++++ b/tagCircle49h12.c +@@ -65565,7 +65565,7 @@ static uint64_t codedata[65535] = { + 0x0001719f5eec237fUL, + 0x0001e520c2997b43UL, + }; +-apriltag_family_t *tagCircle49h12_create() ++apriltag_family_t *tagCircle49h12_create(void) + { + apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t)); + tf->name = strdup("tagCircle49h12"); +diff --git a/tagCircle49h12.h b/tagCircle49h12.h +index 4b4c0846b670d94eee7dc8996130b41dd7a1a2ee..c69e57065e55182f81629fdd82c03ea281e3d907 100644 +--- a/tagCircle49h12.h ++++ b/tagCircle49h12.h +@@ -34,7 +34,7 @@ either expressed or implied, of the Regents of The University of Michigan. + extern "C" { + #endif + +-apriltag_family_t *tagCircle49h12_create(); ++apriltag_family_t *tagCircle49h12_create(void); + void tagCircle49h12_destroy(apriltag_family_t *tf); + + #ifdef __cplusplus +diff --git a/tagCustom48h12.c b/tagCustom48h12.c +index cd908e10772663732f8130aa9d100908ce8e3825..7b31f014f69ed34261549a2c178d3d3f504d4637 100644 +--- a/tagCustom48h12.c ++++ b/tagCustom48h12.c +@@ -42241,7 +42241,7 @@ static uint64_t codedata[42211] = { + 0x0000bb8c4e2f373cUL, + 0x000018bccac8b0f5UL, + }; +-apriltag_family_t *tagCustom48h12_create() ++apriltag_family_t *tagCustom48h12_create(void) + { + apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t)); + tf->name = strdup("tagCustom48h12"); +diff --git a/tagCustom48h12.h b/tagCustom48h12.h +index 564a98a6869677e020261ba39a0e0bbe8468e3a5..5fca82d4d67d6eaceee7b1e8640ad233dd3b5284 100644 +--- a/tagCustom48h12.h ++++ b/tagCustom48h12.h +@@ -34,7 +34,7 @@ either expressed or implied, of the Regents of The University of Michigan. + extern "C" { + #endif + +-apriltag_family_t *tagCustom48h12_create(); ++apriltag_family_t *tagCustom48h12_create(void); + void tagCustom48h12_destroy(apriltag_family_t *tf); + + #ifdef __cplusplus +diff --git a/tagStandard41h12.c b/tagStandard41h12.c +index 6d77eb95aa3d3c6235e66ddc67da83c794a4800e..f9c0b896360068074da23f1a36ebb3404dbbb650 100644 +--- a/tagStandard41h12.c ++++ b/tagStandard41h12.c +@@ -2145,7 +2145,7 @@ static uint64_t codedata[2115] = { + 0x0000015cff6a6f12UL, + 0x000001ee40155a64UL, + }; +-apriltag_family_t *tagStandard41h12_create() ++apriltag_family_t *tagStandard41h12_create(void) + { + apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t)); + tf->name = strdup("tagStandard41h12"); +diff --git a/tagStandard41h12.h b/tagStandard41h12.h +index 7f2c33b209faf16d0c27589420baab6300342da3..a764fcb9a3dde966985ffb8abe3f489028861a1c 100644 +--- a/tagStandard41h12.h ++++ b/tagStandard41h12.h +@@ -34,7 +34,7 @@ either expressed or implied, of the Regents of The University of Michigan. + extern "C" { + #endif + +-apriltag_family_t *tagStandard41h12_create(); ++apriltag_family_t *tagStandard41h12_create(void); + void tagStandard41h12_destroy(apriltag_family_t *tf); + + #ifdef __cplusplus +diff --git a/tagStandard52h13.c b/tagStandard52h13.c +index 30e2b62f3121d1ab8c7a846157f1e97d0c2cb353..ce5ea491ebbb4c15536bb0f6ffac8706cdb4ea25 100644 +--- a/tagStandard52h13.c ++++ b/tagStandard52h13.c +@@ -48744,7 +48744,7 @@ static uint64_t codedata[48714] = { + 0x000c0d162da0e26dUL, + 0x000cdc7a163acc66UL, + }; +-apriltag_family_t *tagStandard52h13_create() ++apriltag_family_t *tagStandard52h13_create(void) + { + apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t)); + tf->name = strdup("tagStandard52h13"); +diff --git a/tagStandard52h13.h b/tagStandard52h13.h +index aeeb8e3e1d6b6e76288ce1ce673be9189d5acafd..0f239f3d866e49a66d1dc385438d6f12f755f3a3 100644 +--- a/tagStandard52h13.h ++++ b/tagStandard52h13.h +@@ -34,7 +34,7 @@ either expressed or implied, of the Regents of The University of Michigan. + extern "C" { + #endif + +-apriltag_family_t *tagStandard52h13_create(); ++apriltag_family_t *tagStandard52h13_create(void); + void tagStandard52h13_destroy(apriltag_family_t *tf); + + #ifdef __cplusplus