test/uestc-careye
1
0
Fork 0
mirror of https://gitee.com/cv_team/uestc-careye.git synced 2024-11-29 18:28:42 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Modify:添加AprilTags插件

Browse Source
This commit is contained in:
Pulsar-V 2021-08-26 22:10:11 +08:00
parent a0fc265a85
commit e4894398b2
70 changed files with 178825 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
plugins/libapriltags/CMakeLists.txt Normal file
View File

@ -0,0 +1,39 @@
project(libapriltags)
set(PLUGIN_NAME "AprilTags")
message(STATUS "Plugin:${PLUGIN_NAME}")
file(GLOB APRILTAGS_CXX_SOURCES ${PROJECT_SOURCE_DIR}/src/*.cpp)
foreach (APRILTAGS_CXX_SOURCE ${APRILTAGS_CXX_SOURCES})
message(STATUS "${PLUGIN_NAME}_CXX_SOURCE:${APRILTAGS_CXX_SOURCE}")
endforeach ()
file(GLOB APRILTAGS_C_SOURCES ${PROJECT_SOURCE_DIR}/src/*.c)
foreach (APRILTAGS_C_SOURCE ${APRILTAGS_C_SOURCES})
message(STATUS "${PLUGIN_NAME}_C_SOURCE:${APRILTAGS_C_SOURCE}")
endforeach ()
file(GLOB APRILTAGS_HEADERS ${PROJECT_SOURCE_DIR}/include/libapriltags/*.h)
foreach (APRILTAGS_HEADER ${APRILTAGS_HEADERS})
message(STATUS "${PLUGIN_NAME}_HEADER:${APRILTAGS_HEADER}")
endforeach ()
file(GLOB APRILTAGS_COMMON_HEADERS ${PROJECT_SOURCE_DIR}/include/common/*.h)
foreach (APRILTAGS_COMMON_HEADER ${APRILTAGS_COMMON_HEADERS})
message(STATUS "${PLUGIN_NAME}_COMMON_HEADER:${APRILTAGS_COMMON_HEADER}")
endforeach ()
include_directories(${PROJECT_SOURCE_DIR}/include)
include_directories(${PROJECT_SOURCE_DIR}/../../robot_client/include)
set(MRAA_INSTALL_DIR ${PROJECT_BINARY_DIR}/../../3rdparty/mraa/install/lib/pkgconfig)
find_package(PkgConfig REQUIRED)
set(ENV{PKG_CONFIG_PATH} ${MRAA_INSTALL_DIR}:ENV{PKG_CONFIG_PATH})
pkg_check_modules(MRAA REQUIRED mraa)
include_directories(${MRAA_INCLUDE_DIRS})
message(STATUS "MRAA_INCLUDE_DIRS:${MRAA_INCLUDE_DIRS}")
link_directories(${MRAA_LIBRARY_DIRS})
message(STATUS "MRAA_LIBRARY_DIRS:${MRAA_LIBRARY_DIRS}")
message(STATUS "MRAA_LIBRARIES:${MRAA_LIBRARIES}")
add_library(apriltags
${APRILTAGS_HEADERS}
${APRILTAGS_COMMON_HEADERS}
${APRILTAGS_CXX_SOURCES}
${APRILTAGS_C_SOURCES}
)
target_link_libraries(apriltags rccore)

34
plugins/libapriltags/include/common/doubles.h Normal file
View File

@ -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
#define TNAME double
#include "doubles_floats_impl.h"
#undef TNAME

966
plugins/libapriltags/include/common/doubles_floats_impl.h Normal file
View File

@ -0,0 +1,966 @@
/* 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 <stdio.h>
#include <math.h>
#include <string.h>
#include <float.h>
#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

34
plugins/libapriltags/include/common/floats.h Normal file
View File

@ -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
#define TNAME float
#include "doubles_floats_impl.h"
#undef TNAME

123
plugins/libapriltags/include/common/g2d.h Normal file
View File

@ -0,0 +1,123 @@
/* 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 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

64
plugins/libapriltags/include/common/getopt.h Normal file
View File

@ -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 "zarray.h"
#include "string_util.h"
#ifdef __cplusplus
extern "C" {
#endif
typedef struct getopt getopt_t;
getopt_t *getopt_create();
void getopt_destroy(getopt_t *gopt);
// Parse args. Returns 1 on success
int getopt_parse(getopt_t *gopt, int argc, char *argv[], int showErrors);
void getopt_do_usage(getopt_t *gopt);
// Returns a string containing the usage. Must be freed by caller
char *getopt_get_usage(getopt_t *gopt);
void getopt_add_spacer(getopt_t *gopt, const char *s);
void getopt_add_bool(getopt_t *gopt, char sopt, const char *lname, int def, const char *help);
void getopt_add_int(getopt_t *gopt, char sopt, const char *lname, const char *def, const char *help);
void getopt_add_string(getopt_t *gopt, char sopt, const char *lname, const char *def, const char *help);
void getopt_add_double(getopt_t *gopt, char sopt, const char *lname, const char *def, const char *help);
const char *getopt_get_string(getopt_t *gopt, const char *lname);
int getopt_get_int(getopt_t *getopt, const char *lname);
int getopt_get_bool(getopt_t *getopt, const char *lname);
double getopt_get_double(getopt_t *getopt, const char *lname);
int getopt_was_specified(getopt_t *gopt, const char *lname);
const zarray_t *getopt_get_extra_args(getopt_t *gopt);
#ifdef __cplusplus
}
#endif

182
plugins/libapriltags/include/common/homography.h Normal file
View File

@ -0,0 +1,182 @@
/* 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 expresison 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

79
plugins/libapriltags/include/common/image_types.h Normal file
View File

@ -0,0 +1,79 @@
/* 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 <stdint.h>
// 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]
};

99
plugins/libapriltags/include/common/image_u8.h Normal file
View File

@ -0,0 +1,99 @@
/* 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 <stdint.h>
#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

66
plugins/libapriltags/include/common/image_u8x3.h Normal file
View File

@ -0,0 +1,66 @@
/* 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 <stdint.h>
#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

65
plugins/libapriltags/include/common/image_u8x4.h Normal file
View File

@ -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 <stdint.h>
#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

450
plugins/libapriltags/include/common/matd.h Normal file
View File

@ -0,0 +1,450 @@
/* 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 <assert.h>
#include <stddef.h>
#include <string.h>
#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

194
plugins/libapriltags/include/common/math_util.h Normal file
View File

@ -0,0 +1,194 @@
/* 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 <math.h>
#include <float.h>
#include <stdlib.h>
#include <stdint.h>
#include <assert.h>
#include <string.h> // 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() {
return (float) (rand() / (RAND_MAX + 1.0));
}
static inline float signed_randf() {
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

57
plugins/libapriltags/include/common/pam.h Normal file
View File

@ -0,0 +1,57 @@
/* 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 <stdint.h>
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);

101
plugins/libapriltags/include/common/pjpeg.h Normal file
View File

@ -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 "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

58
plugins/libapriltags/include/common/pnm.h Normal file
View File

@ -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 <stdint.h>
#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

52
plugins/libapriltags/include/common/postscript_utils.h Normal file
View File

@ -0,0 +1,52 @@
/* 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
// write commands in postscript language to render an image in the current
// graphics environment. The image will be rendered in one pixel per unit
// with Y up coordinate axis (e.g. upside down).
static void postscript_image(FILE *f, image_u8_t *im) {
// fprintf(f, "/readstring {\n currentfile exch readhexstring pop\n} bind def\n");
fprintf(f, "/picstr %d string def\n", im->width);
fprintf(f, "%d %d 8 [1 0 0 1 0 0]\n",
im->width, im->height);
fprintf(f, "{currentfile picstr readhexstring pop}\nimage\n");
for (int y = 0; y < im->height; y++) {
for (int x = 0; x < im->width; x++) {
uint8_t v = im->buf[y * im->stride + x];
fprintf(f, "%02x", v);
if ((x % 32) == 31)
fprintf(f, "\n");
}
}
fprintf(f, "\n");
}

462
plugins/libapriltags/include/common/string_util.h Normal file
View File

@ -0,0 +1,462 @@
/* 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 <stdio.h>
#include <stdarg.h>
#include <stdbool.h>
#include <ctype.h>
#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 responsibilty 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();
/**
* 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

34
plugins/libapriltags/include/common/svd22.h Normal file
View File

@ -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);

95
plugins/libapriltags/include/common/time_util.h Normal file
View File

@ -0,0 +1,95 @@
/* 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 <stdbool.h>
#include <stdint.h>
#ifdef _WIN32
#include <windows.h>
typedef long long suseconds_t;
#endif
#ifdef _MSC_VER
inline int gettimeofday(struct timeval* tp, void* tzp)
{
unsigned long t;
t = timeGetTime();
tp->tv_sec = t / 1000;
tp->tv_usec = t % 1000;
return 0;
}
#else
#include <sys/time.h>
#include <unistd.h>
#endif
#include <time.h>
#ifdef __cplusplus
extern "C" {
#endif
typedef struct timeutil_rest timeutil_rest_t;
timeutil_rest_t *timeutil_rest_create();
void timeutil_rest_destroy(timeutil_rest_t *rest);
int64_t utime_now(); // 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

112
plugins/libapriltags/include/common/timeprofile.h Normal file
View File

@ -0,0 +1,112 @@
/* 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 <stdio.h>
#include <string.h>
#include <stdint.h>
#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() {
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

139
plugins/libapriltags/include/common/unionfind.h Normal file
View File

@ -0,0 +1,139 @@
/* 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 <stdint.h>
#include <stdlib.h>
typedef struct unionfind unionfind_t;
struct unionfind {
uint32_t maxid;
struct ufrec *data;
};
struct ufrec {
// the parent of this node. If a node's parent is its own index,
// then it is a root.
uint32_t parent;
// for the root of a connected component, the number of components
// connected to it. For intermediate values, it's not meaningful.
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->data = (struct ufrec *) malloc((maxid + 1) * sizeof(struct ufrec));
for (int i = 0; i <= maxid; i++) {
uf->data[i].size = 1;
uf->data[i].parent = i;
}
return uf;
}
static inline void unionfind_destroy(unionfind_t *uf) {
free(uf->data);
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->data[id].parent == id)
return id;
// otherwise, recurse
uint32_t root = unionfind_get_representative(uf, uf->data[id].parent);
// short circuit the path. [XXX This write prevents tail recursion]
uf->data[id].parent = 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 = id;
// chase down the root
while (uf->data[root].parent != root) {
root = uf->data[root].parent;
}
// go back and collapse the tree.
while (uf->data[id].parent != root) {
uint32_t tmp = uf->data[id].parent;
uf->data[id].parent = 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->data[repid].size;
}
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->data[aroot].size;
uint32_t bsize = uf->data[broot].size;
// 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->data[broot].parent = aroot;
uf->data[aroot].size += bsize;
return aroot;
} else {
uf->data[aroot].parent = broot;
uf->data[broot].size += asize;
return broot;
}
}

50
plugins/libapriltags/include/common/workerpool.h Normal file
View File

@ -0,0 +1,50 @@
/* 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();

441
plugins/libapriltags/include/common/zarray.h Normal file
View File

@ -0,0 +1,441 @@
/* 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 <stddef.h>
#include <assert.h>
#include <stdlib.h>
#include <string.h>
#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)());
/**
* 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 all elements from 'source' into 'dest'. el_size must be the same
* for both lists
**/
static inline void zarray_add_all(zarray_t *dest, const zarray_t *source) {
assert(dest->el_sz == source->el_sz);
// Don't allocate on stack because el_sz could be larger than ~8 MB
// stack size
char *tmp = (char *) calloc(1, dest->el_sz);
for (int i = 0; i < zarray_size(source); i++) {
zarray_get(source, i, tmp);
zarray_add(dest, tmp);
}
free(tmp);
}
#ifdef __cplusplus
}
#endif

424
plugins/libapriltags/include/common/zhash.h Normal file
View File

@ -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.
*/
#pragma once
#include <stdint.h>
#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)());
/**
* 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)());
/**
* 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)());
/**
* 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)());
/**
* 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

76
plugins/libapriltags/include/common/zmaxheap.h Normal file
View File

@ -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 <stdio.h>
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 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);

270
plugins/libapriltags/include/libapriltags/apriltag.h Normal file
View File

@ -0,0 +1,270 @@
/* 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 <stdlib.h>
#include "common/matd.h"
#include "common/image_u8.h"
#include "common/zarray.h"
#include "common/workerpool.h"
#include "common/timeprofile.h"
#include <pthread.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 non-zero, 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 (1).
//
// Very computationally inexpensive. Option is ignored if
// quad_decimate = 1.
int 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 non-zero, write a variety of debugging images to the
// current working directory at various stages through the
// detection process. (Somewhat slow).
int 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();
// 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

98
plugins/libapriltags/include/libapriltags/apriltag_math.h Normal file
View File

@ -0,0 +1,98 @@
/* 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 <math.h>
// 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];
}

75
plugins/libapriltags/include/libapriltags/apriltag_pose.h Normal file
View File

@ -0,0 +1,75 @@
#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;
typedef struct {
matd_t *R;
matd_t *t;
} 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);
/**
* 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

20
plugins/libapriltags/include/libapriltags/libapriltags.h Normal file
View File

@ -0,0 +1,20 @@
//
// Created by Pulsar on 2021/8/26.
//
#ifndef ROBOCAR_HUMANDETECTOR_H
#define ROBOCAR_HUMANDETECTOR_H
#include <opencv2/opencv.hpp>
#include <rc_system/data_struct.h>
#include <rc_message/rc_move_msg.h>
extern "C" {
void *InitCallback();
void *BeforeDetcetCallback(void *args);
void DetectCallback(cv::Mat &frame, void *args, const std::shared_ptr<rccore::message::MoveMessage> &p_move_message);
void *AfterDetcetCallback(void *args);
void OnThreadBreakCallback(void *args);
};
#endif //ROBOCAR_HUMANDETECTOR_H

44
plugins/libapriltags/include/libapriltags/tag16h5.h Normal file
View File

@ -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 tag16h5_destroy(apriltag_family_t *tf);
#ifdef __cplusplus
}
#endif
#endif

44
plugins/libapriltags/include/libapriltags/tag25h9.h Normal file
View File

@ -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 _TAG25H9
#define _TAG25H9
#include "apriltag.h"
#ifdef __cplusplus
extern "C" {
#endif
apriltag_family_t *tag25h9_create();
void tag25h9_destroy(apriltag_family_t *tf);
#ifdef __cplusplus
}
#endif
#endif

44
plugins/libapriltags/include/libapriltags/tag36h11.h Normal file
View File

@ -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 tag36h11_destroy(apriltag_family_t *tf);
#ifdef __cplusplus
}
#endif
#endif

44
plugins/libapriltags/include/libapriltags/tagCircle21h7.h Normal file
View File

@ -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 _TAGCircle21H7
#define _TAGCircle21H7
#include "apriltag.h"
#ifdef __cplusplus
extern "C" {
#endif
apriltag_family_t *tagCircle21h7_create();
void tagCircle21h7_destroy(apriltag_family_t *tf);
#ifdef __cplusplus
}
#endif
#endif

44
plugins/libapriltags/include/libapriltags/tagCircle49h12.h Normal file
View File

@ -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 _TAGCircle49H12
#define _TAGCircle49H12
#include "apriltag.h"
#ifdef __cplusplus
extern "C" {
#endif
apriltag_family_t *tagCircle49h12_create();
void tagCircle49h12_destroy(apriltag_family_t *tf);
#ifdef __cplusplus
}
#endif
#endif

44
plugins/libapriltags/include/libapriltags/tagCustom48h12.h Normal file
View File

@ -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 _TAGCustom48H12
#define _TAGCustom48H12
#include "apriltag.h"
#ifdef __cplusplus
extern "C" {
#endif
apriltag_family_t *tagCustom48h12_create();
void tagCustom48h12_destroy(apriltag_family_t *tf);
#ifdef __cplusplus
}
#endif
#endif

44
plugins/libapriltags/include/libapriltags/tagStandard41h12.h Normal file
View File

@ -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 _TAGStandard41H12
#define _TAGStandard41H12
#include "apriltag.h"
#ifdef __cplusplus
extern "C" {
#endif
apriltag_family_t *tagStandard41h12_create();
void tagStandard41h12_destroy(apriltag_family_t *tf);
#ifdef __cplusplus
}
#endif
#endif

44
plugins/libapriltags/include/libapriltags/tagStandard52h13.h Normal file
View File

@ -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 _TAGStandard52H13
#define _TAGStandard52H13
#include "apriltag.h"
#ifdef __cplusplus
extern "C" {
#endif
apriltag_family_t *tagStandard52h13_create();
void tagStandard52h13_destroy(apriltag_family_t *tf);
#ifdef __cplusplus
}
#endif
#endif

1412
plugins/libapriltags/src/apriltag.c Normal file
View File

File diff suppressed because it is too large Load Diff

548
plugins/libapriltags/src/apriltag_pose.c Normal file
View File

@ -0,0 +1,548 @@
#include <math.h>
#include <stdio.h>
#include "apriltag_pose.h"
#include "apriltag_math.h"
#include "common/homography.h"
#include "common/image_u8x3.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.
*
* @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) {
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);
}
prev_error = error;
free(q);
}
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.
fprintf(stderr, "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 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);
solution2->R = fix_pose_ambiguities(v, p, solution1->t, solution1->R, 4);
if (solution2->R) {
solution2->t = matd_create(3, 1);
*err2 = orthogonal_iteration(v, p, &solution2->t, &solution2->R, 4, nIters);
} else {
*err2 = HUGE_VAL;
}
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;
estimate_tag_pose_orthogonal_iteration(info, &err1, &pose1, &err2, &pose2, 50);
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;
}
}

348
plugins/libapriltags/src/apriltag_pywrap.c Normal file
View File

@ -0,0 +1,348 @@
#define NPY_NO_DEPRECATED_API NPY_API_VERSION
#include <stdbool.h>
#include <Python.h>
#include <structmember.h>
#include <numpy/arrayobject.h>
#include <signal.h>
#include "apriltag.h"
#include "tag36h11.h"
#include "tag25h9.h"
#include "tag16h5.h"
#include "tagCircle21h7.h"
#include "tagCircle49h12.h"
#include "tagCustom48h12.h"
#include "tagStandard41h12.h"
#include "tagStandard52h13.h"
#define SUPPORTED_TAG_FAMILIES(_) \
_(tag36h11) \
_(tag25h9) \
_(tag16h5) \
_(tagCircle21h7) \
_(tagCircle49h12) \
_(tagStandard41h12) \
_(tagStandard52h13) \
_(tagCustom48h12)
#define TAG_CREATE_FAMILY(name) \
else if (0 == strcmp(family, #name)) self->tf = name ## _create();
#define TAG_SET_DESTROY_FUNC(name) \
else if (0 == strcmp(family, #name)) self->destroy_func = name ## _destroy;
#define FAMILY_STRING(name) " " #name "\n"
// Python is silly. There's some nuance about signal handling where it sets a
// SIGINT (ctrl-c) handler to just set a flag, and the python layer then reads
// this flag and does the thing. Here I'm running C code, so SIGINT would set a
// flag, but not quit, so I can't interrupt the solver. Thus I reset the SIGINT
// handler to the default, and put it back to the python-specific version when
// I'm done
#define SET_SIGINT() struct sigaction sigaction_old; \
do { \
if( 0 != sigaction(SIGINT, \
&(struct sigaction){ .sa_handler = SIG_DFL }, \
&sigaction_old) ) \
{ \
PyErr_SetString(PyExc_RuntimeError, "sigaction() failed"); \
goto done; \
} \
} while(0)
#define RESET_SIGINT() do { \
if( 0 != sigaction(SIGINT, \
&sigaction_old, NULL )) \
PyErr_SetString(PyExc_RuntimeError, "sigaction-restore failed"); \
} while(0)
#define PYMETHODDEF_ENTRY(function_prefix, name, args) {#name, \
(PyCFunction)function_prefix ## name, \
args, \
function_prefix ## name ## _docstring}
typedef struct {
PyObject_HEAD
apriltag_family_t
*
tf;
apriltag_detector_t *td;
void (*destroy_func)(apriltag_family_t *tf);
} apriltag_py_t;
static PyObject *
apriltag_new(PyTypeObject *type, PyObject *args, PyObject *kwargs) {
bool success = false;
apriltag_py_t *self = (apriltag_py_t *) type->tp_alloc(type, 0);
if (self == NULL) goto done;
self->tf = NULL;
self->td = NULL;
const char *family = NULL;
int Nthreads = 1;
int maxhamming = 1;
float decimate = 1.0;
float blur = 0.0;
bool refine_edges = true;
bool debug = false;
PyObject *py_refine_edges = NULL;
PyObject *py_debug = NULL;
char *keywords[] = {"family",
"threads",
"maxhamming",
"decimate",
"blur",
"refine-edges",
"debug",
NULL};
if (!PyArg_ParseTupleAndKeywords(args, kwargs, "s|iiffOO",
keywords,
&family,
&Nthreads,
&maxhamming,
&decimate,
&blur,
&py_refine_edges,
&py_debug)) {
goto done;
}
if (py_refine_edges != NULL)
refine_edges = PyObject_IsTrue(py_refine_edges);
if (py_debug != NULL)
debug = PyObject_IsTrue(py_debug);
if (0);
SUPPORTED_TAG_FAMILIES(TAG_SET_DESTROY_FUNC)
else {
PyErr_Format(PyExc_RuntimeError, "Unrecognized tag family name: '%s'. Families I know about:\n%s",
family, SUPPORTED_TAG_FAMILIES(FAMILY_STRING));
goto done;
}
if (0); SUPPORTED_TAG_FAMILIES(TAG_CREATE_FAMILY);
self->td = apriltag_detector_create();
if (self->td == NULL) {
PyErr_SetString(PyExc_RuntimeError, "apriltag_detector_create() failed!");
goto done;
}
apriltag_detector_add_family_bits(self->td, self->tf, maxhamming);
self->td->quad_decimate = decimate;
self->td->quad_sigma = blur;
self->td->nthreads = Nthreads;
self->td->refine_edges = refine_edges;
self->td->debug = debug;
success = true;
done:
if (!success) {
if (self != NULL) {
if (self->td != NULL) {
apriltag_detector_destroy(self->td);
self->td = NULL;
}
if (self->tf != NULL) {
self->destroy_func(self->tf);
self->tf = NULL;
}
Py_DECREF(self);
}
return NULL;
}
return (PyObject *) self;
}
static void apriltag_dealloc(apriltag_py_t *self) {
if (self == NULL)
return;
if (self->td != NULL) {
apriltag_detector_destroy(self->td);
self->td = NULL;
}
if (self->tf != NULL) {
self->destroy_func(self->tf);
self->tf = NULL;
}
Py_TYPE(self)->tp_free((PyObject *) self);
}
static PyObject *apriltag_detect(apriltag_py_t *self,
PyObject *args) {
PyObject *result = NULL;
PyArrayObject *xy_c = NULL;
PyArrayObject *xy_lb_rb_rt_lt = NULL;
PyArrayObject *image = NULL;
PyObject *detections_tuple = NULL;
SET_SIGINT();
if (!PyArg_ParseTuple(args, "O&",
PyArray_Converter, &image))
goto done;
npy_intp *dims = PyArray_DIMS(image);
npy_intp *strides = PyArray_STRIDES(image);
int ndims = PyArray_NDIM(image);
if (ndims != 2) {
PyErr_Format(PyExc_RuntimeError, "The input image array must have exactly 2 dims; got %d",
ndims);
goto done;
}
if (PyArray_TYPE(image) != NPY_UINT8) {
PyErr_SetString(PyExc_RuntimeError, "The input image array must contain 8-bit unsigned data");
goto done;
}
if (strides[ndims - 1] != 1) {
PyErr_SetString(PyExc_RuntimeError, "Image rows must live in contiguous memory");
goto done;
}
image_u8_t im = {.width = dims[1],
.height = dims[0],
.stride = strides[0],
.buf = PyArray_DATA(image)};
zarray_t *detections = apriltag_detector_detect(self->td, &im);
int N = zarray_size(detections);
detections_tuple = PyTuple_New(N);
if (detections_tuple == NULL) {
PyErr_Format(PyExc_RuntimeError, "Error creating output tuple of size %d", N);
goto done;
}
for (int i = 0; i < N; i++) {
xy_c = (PyArrayObject *) PyArray_SimpleNew(1, ((npy_intp[]) {2}), NPY_FLOAT64);
if (xy_c == NULL) {
PyErr_SetString(PyExc_RuntimeError, "Could not allocate xy_c array");
goto done;
}
xy_lb_rb_rt_lt = (PyArrayObject *) PyArray_SimpleNew(2, ((npy_intp[]) {4, 2}), NPY_FLOAT64);
if (xy_lb_rb_rt_lt == NULL) {
PyErr_SetString(PyExc_RuntimeError, "Could not allocate xy_lb_rb_rt_lt array");
goto done;
}
apriltag_detection_t *det;
zarray_get(detections, i, &det);
*(double *) PyArray_GETPTR1(xy_c, 0) = det->c[0];
*(double *) PyArray_GETPTR1(xy_c, 1) = det->c[1];
for (int j = 0; j < 4; j++) {
*(double *) PyArray_GETPTR2(xy_lb_rb_rt_lt, j, 0) = det->p[j][0];
*(double *) PyArray_GETPTR2(xy_lb_rb_rt_lt, j, 1) = det->p[j][1];
}
PyTuple_SET_ITEM(detections_tuple, i,
Py_BuildValue("{s:i,s:f,s:i,s:O,s:O}",
"hamming", det->hamming,
"margin", det->decision_margin,
"id", det->id,
"center", xy_c,
"lb-rb-rt-lt", xy_lb_rb_rt_lt));
xy_c = NULL;
xy_lb_rb_rt_lt = NULL;
}
apriltag_detections_destroy(detections);
result = detections_tuple;
detections_tuple = NULL;
done:
Py_XDECREF(xy_c);
Py_XDECREF(xy_lb_rb_rt_lt);
Py_XDECREF(image);
Py_XDECREF(detections_tuple);
RESET_SIGINT();
return result;
}
static const char apriltag_detect_docstring[] =
#include "apriltag_detect.docstring.h";
static const char apriltag_type_docstring[] =
#include "apriltag_py_type.docstring.h";
static PyMethodDef apriltag_methods[] =
{PYMETHODDEF_ENTRY(apriltag_, detect, METH_VARARGS),
{}
};
static PyTypeObject apriltagType =
{
PyVarObject_HEAD_INIT(NULL, 0)
.tp_name = "apriltag",
.tp_basicsize = sizeof(apriltag_py_t),
.tp_new = apriltag_new,
.tp_dealloc = (destructor) apriltag_dealloc,
.tp_methods = apriltag_methods,
.tp_flags = Py_TPFLAGS_DEFAULT,
.tp_doc = apriltag_type_docstring
};
static PyMethodDef methods[] =
{{}
};
#if PY_MAJOR_VERSION == 2
PyMODINIT_FUNC initapriltag(void)
{
if (PyType_Ready(&apriltagType) < 0)
return;
PyObject* module = Py_InitModule3("apriltag", methods,
"AprilTags visual fiducial system detector");
Py_INCREF(&apriltagType);
PyModule_AddObject(module, "apriltag", (PyObject *)&apriltagType);
import_array();
}
#else
static struct PyModuleDef module_def =
{
PyModuleDef_HEAD_INIT,
"apriltag",
"AprilTags visual fiducial system detector",
-1,
methods
};
PyMODINIT_FUNC PyInit_apriltag(void) {
if (PyType_Ready(&apriltagType) < 0)
return NULL;
PyObject *module =
PyModule_Create(&module_def);
Py_INCREF(&apriltagType);
PyModule_AddObject(module, "apriltag", (PyObject * ) & apriltagType);
import_array();
return module;
}
#endif

1924
plugins/libapriltags/src/apriltag_quad_thresh.c Normal file
View File

File diff suppressed because it is too large Load Diff

893
plugins/libapriltags/src/g2d.c Normal file
View File

@ -0,0 +1,893 @@
/* 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 <assert.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#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() {
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 && v != g2d_polygon_contains_point_ref(poly, q)) {
printf("FAILURE %d %d\n", v, quad_acc);
exit(-1);
}
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 ((c < a && c < b) || (c > a && 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 ((c < a && c < b) || (c > a && 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 ((c < a && c < b) || (c > a && 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

525
plugins/libapriltags/src/getopt.c Normal file
View File

@ -0,0 +1,525 @@
/* 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 <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <string.h>
#include <ctype.h>
#include <errno.h>
#include "zhash.h"
#include "zarray.h"
#include "getopt.h"
#include "common/math_util.h"
#define GOO_BOOL_TYPE 1
#define GOO_STRING_TYPE 2
typedef struct getopt_option getopt_option_t;
struct getopt_option {
char *sname;
char *lname;
char *svalue;
char *help;
int type;
int spacer;
int was_specified;
};
struct getopt {
zhash_t *lopts;
zhash_t *sopts;
zarray_t *extraargs;
zarray_t *options;
};
getopt_t *getopt_create() {
getopt_t *gopt = (getopt_t *) calloc(1, sizeof(getopt_t));
gopt->lopts = zhash_create(sizeof(char *), sizeof(getopt_option_t *), zhash_str_hash, zhash_str_equals);
gopt->sopts = zhash_create(sizeof(char *), sizeof(getopt_option_t *), zhash_str_hash, zhash_str_equals);
gopt->options = zarray_create(sizeof(getopt_option_t *));
gopt->extraargs = zarray_create(sizeof(char *));
return gopt;
}
void getopt_option_destroy(getopt_option_t *goo) {
free(goo->sname);
free(goo->lname);
free(goo->svalue);
free(goo->help);
memset(goo, 0, sizeof(getopt_option_t));
free(goo);
}
void getopt_destroy(getopt_t *gopt) {
// free the extra arguments and container
zarray_vmap(gopt->extraargs, free);
zarray_destroy(gopt->extraargs);
// deep free of the getopt_option structs. Also frees key/values, so
// after this loop, hash tables will no longer work
zarray_vmap(gopt->options, getopt_option_destroy);
zarray_destroy(gopt->options);
// free tables
zhash_destroy(gopt->lopts);
zhash_destroy(gopt->sopts);
memset(gopt, 0, sizeof(getopt_t));
free(gopt);
}
static void getopt_modify_string(char **str, char *newvalue) {
char *old = *str;
*str = newvalue;
if (old != NULL)
free(old);
}
static char *get_arg_assignment(char *arg) {
// not an arg starting with "--"?
if (!str_starts_with(arg, "--")) {
return NULL;
}
int eq_index = str_indexof(arg, "=");
// no assignment?
if (eq_index == -1) {
return NULL;
}
// no quotes allowed before '=' in "--key=value" option specification.
// quotes can be used in value string, or by extra arguments
for (int i = 0; i < eq_index; i++) {
if (arg[i] == '\'' || arg[i] == '"') {
return NULL;
}
}
return &arg[eq_index];
}
// returns 1 if no error
int getopt_parse(getopt_t *gopt, int argc, char *argv[], int showErrors) {
int okay = 1;
zarray_t *toks = zarray_create(sizeof(char *));
// take the input stream and chop it up into tokens
for (int i = 1; i < argc; i++) {
char *arg = strdup(argv[i]);
char *eq = get_arg_assignment(arg);
// no equal sign? Push the whole thing.
if (eq == NULL) {
zarray_add(toks, &arg);
} else {
// there was an equal sign. Push the part
// before and after the equal sign
char *val = strdup(&eq[1]);
eq[0] = 0;
zarray_add(toks, &arg);
// if the part after the equal sign is
// enclosed by quotation marks, strip them.
if (val[0] == '\"') {
size_t last = strlen(val) - 1;
if (val[last] == '\"')
val[last] = 0;
char *valclean = strdup(&val[1]);
zarray_add(toks, &valclean);
free(val);
} else {
zarray_add(toks, &val);
}
}
}
// now loop over the elements and evaluate the arguments
unsigned int i = 0;
char *tok = NULL;
while (i < zarray_size(toks)) {
// rather than free statement throughout this while loop
if (tok != NULL)
free(tok);
zarray_get(toks, i, &tok);
if (!strncmp(tok, "--", 2)) {
char *optname = &tok[2];
getopt_option_t *goo = NULL;
zhash_get(gopt->lopts, &optname, &goo);
if (goo == NULL) {
okay = 0;
if (showErrors)
printf("Unknown option --%s\n", optname);
i++;
continue;
}
goo->was_specified = 1;
if (goo->type == GOO_BOOL_TYPE) {
if ((i + 1) < zarray_size(toks)) {
char *val = NULL;
zarray_get(toks, i + 1, &val);
if (!strcmp(val, "true")) {
i += 2;
getopt_modify_string(&goo->svalue, val);
continue;
}
if (!strcmp(val, "false")) {
i += 2;
getopt_modify_string(&goo->svalue, val);
continue;
}
}
getopt_modify_string(&goo->svalue, strdup("true"));
i++;
continue;
}
if (goo->type == GOO_STRING_TYPE) {
// TODO: check whether next argument is an option, denoting missing argument
if ((i + 1) < zarray_size(toks)) {
char *val = NULL;
zarray_get(toks, i + 1, &val);
i += 2;
getopt_modify_string(&goo->svalue, val);
continue;
}
okay = 0;
if (showErrors)
printf("Option %s requires a string argument.\n", optname);
}
}
if (!strncmp(tok, "-", 1) && strncmp(tok, "--", 2)) {
size_t len = strlen(tok);
int pos;
for (pos = 1; pos < len; pos++) {
char sopt[2];
sopt[0] = tok[pos];
sopt[1] = 0;
char *sopt_ptr = (char *) &sopt;
getopt_option_t *goo = NULL;
zhash_get(gopt->sopts, &sopt_ptr, &goo);
if (goo == NULL) {
// is the argument a numerical literal that happens to be negative?
if (pos == 1 && isdigit(tok[pos])) {
zarray_add(gopt->extraargs, &tok);
tok = NULL;
break;
} else {
okay = 0;
if (showErrors)
printf("Unknown option -%c\n", tok[pos]);
i++;
continue;
}
}
goo->was_specified = 1;
if (goo->type == GOO_BOOL_TYPE) {
getopt_modify_string(&goo->svalue, strdup("true"));
continue;
}
if (goo->type == GOO_STRING_TYPE) {
if ((i + 1) < zarray_size(toks)) {
char *val = NULL;
zarray_get(toks, i + 1, &val);
// TODO: allow negative numerical values for short-name options ?
if (val[0] == '-') {
okay = 0;
if (showErrors)
printf("Ran out of arguments for option block %s\n", tok);
}
i++;
getopt_modify_string(&goo->svalue, val);
continue;
}
okay = 0;
if (showErrors)
printf("Option -%c requires a string argument.\n", tok[pos]);
}
}
i++;
continue;
}
// it's not an option-- it's an argument.
zarray_add(gopt->extraargs, &tok);
tok = NULL;
i++;
}
if (tok != NULL)
free(tok);
zarray_destroy(toks);
return okay;
}
void getopt_add_spacer(getopt_t *gopt, const char *s) {
getopt_option_t *goo = (getopt_option_t *) calloc(1, sizeof(getopt_option_t));
goo->spacer = 1;
goo->help = strdup(s);
zarray_add(gopt->options, &goo);
}
void getopt_add_bool(getopt_t *gopt, char sopt, const char *lname, int def, const char *help) {
char sname[2];
sname[0] = sopt;
sname[1] = 0;
char *sname_ptr = (char *) &sname;
if (strlen(lname) < 1) { // must have long name
fprintf(stderr, "getopt_add_bool(): must supply option name\n");
exit(EXIT_FAILURE);
}
if (sopt == '-') { // short name cannot be '-' (no way to reference)
fprintf(stderr, "getopt_add_bool(): invalid option character: '%c'\n", sopt);
exit(EXIT_FAILURE);
}
if (zhash_contains(gopt->lopts, &lname)) {
fprintf(stderr, "getopt_add_bool(): duplicate option name: --%s\n", lname);
exit(EXIT_FAILURE);
}
if (sopt != '\0' && zhash_contains(gopt->sopts, &sname_ptr)) {
fprintf(stderr, "getopt_add_bool(): duplicate option: -%s ('%s')\n", sname, lname);
exit(EXIT_FAILURE);
}
getopt_option_t *goo = (getopt_option_t *) calloc(1, sizeof(getopt_option_t));
goo->sname = strdup(sname);
goo->lname = strdup(lname);
goo->svalue = strdup(def ? "true" : "false");
goo->type = GOO_BOOL_TYPE;
goo->help = strdup(help);
zhash_put(gopt->lopts, &goo->lname, &goo, NULL, NULL);
zhash_put(gopt->sopts, &goo->sname, &goo, NULL, NULL);
zarray_add(gopt->options, &goo);
}
void getopt_add_int(getopt_t *gopt, char sopt, const char *lname, const char *def, const char *help) {
getopt_add_string(gopt, sopt, lname, def, help);
}
void
getopt_add_double(getopt_t *gopt, char sopt, const char *lname, const char *def, const char *help) {
getopt_add_string(gopt, sopt, lname, def, help);
}
void getopt_add_string(getopt_t *gopt, char sopt, const char *lname, const char *def, const char *help) {
char sname[2];
sname[0] = sopt;
sname[1] = 0;
char *sname_ptr = (char *) &sname;
if (strlen(lname) < 1) { // must have long name
fprintf(stderr, "getopt_add_string(): must supply option name\n");
exit(EXIT_FAILURE);
}
if (sopt == '-') { // short name cannot be '-' (no way to reference)
fprintf(stderr, "getopt_add_string(): invalid option character: '%c'\n", sopt);
exit(EXIT_FAILURE);
}
if (zhash_contains(gopt->lopts, &lname)) {
fprintf(stderr, "getopt_add_string(): duplicate option name: --%s\n", lname);
exit(EXIT_FAILURE);
}
if (sopt != '\0' && zhash_contains(gopt->sopts, &sname_ptr)) {
fprintf(stderr, "getopt_add_string(): duplicate option: -%s ('%s')\n", sname, lname);
exit(EXIT_FAILURE);
}
getopt_option_t *goo = (getopt_option_t *) calloc(1, sizeof(getopt_option_t));
goo->sname = strdup(sname);
goo->lname = strdup(lname);
goo->svalue = strdup(def);
goo->type = GOO_STRING_TYPE;
goo->help = strdup(help);
zhash_put(gopt->lopts, &goo->lname, &goo, NULL, NULL);
zhash_put(gopt->sopts, &goo->sname, &goo, NULL, NULL);
zarray_add(gopt->options, &goo);
}
const char *getopt_get_string(getopt_t *gopt, const char *lname) {
getopt_option_t *goo = NULL;
zhash_get(gopt->lopts, &lname, &goo);
// could return null, but this would be the only
// method that doesn't assert on a missing key
assert (goo != NULL);
return goo->svalue;
}
int getopt_get_int(getopt_t *getopt, const char *lname) {
const char *v = getopt_get_string(getopt, lname);
assert(v != NULL);
errno = 0;
char *endptr = (char *) v;
long val = strtol(v, &endptr, 10);
if (errno != 0) {
fprintf(stderr, "--%s argument: strtol failed: %s\n", lname, strerror(errno));
exit(EXIT_FAILURE);
}
if (endptr == v) {
fprintf(stderr, "--%s argument cannot be parsed as an int\n", lname);
exit(EXIT_FAILURE);
}
return (int) val;
}
int getopt_get_bool(getopt_t *getopt, const char *lname) {
const char *v = getopt_get_string(getopt, lname);
assert (v != NULL);
int val = !strcmp(v, "true");
return val;
}
double getopt_get_double(getopt_t *getopt, const char *lname) {
const char *v = getopt_get_string(getopt, lname);
assert (v != NULL);
errno = 0;
char *endptr = (char *) v;
double d = strtod(v, &endptr);
if (errno != 0) {
fprintf(stderr, "--%s argument: strtod failed: %s\n", lname, strerror(errno));
exit(EXIT_FAILURE);
}
if (endptr == v) {
fprintf(stderr, "--%s argument cannot be parsed as a double\n", lname);
exit(EXIT_FAILURE);
}
return d;
}
int getopt_was_specified(getopt_t *getopt, const char *lname) {
getopt_option_t *goo = NULL;
zhash_get(getopt->lopts, &lname, &goo);
if (goo == NULL)
return 0;
return goo->was_specified;
}
const zarray_t *getopt_get_extra_args(getopt_t *gopt) {
return gopt->extraargs;
}
void getopt_do_usage(getopt_t *gopt) {
char *usage = getopt_get_usage(gopt);
printf("%s", usage);
free(usage);
}
char *getopt_get_usage(getopt_t *gopt) {
string_buffer_t *sb = string_buffer_create();
int leftmargin = 2;
int longwidth = 12;
int valuewidth = 10;
for (unsigned int i = 0; i < zarray_size(gopt->options); i++) {
getopt_option_t *goo = NULL;
zarray_get(gopt->options, i, &goo);
if (goo->spacer)
continue;
longwidth = max(longwidth, (int) strlen(goo->lname));
if (goo->type == GOO_STRING_TYPE)
valuewidth = max(valuewidth, (int) strlen(goo->svalue));
}
for (unsigned int i = 0; i < zarray_size(gopt->options); i++) {
getopt_option_t *goo = NULL;
zarray_get(gopt->options, i, &goo);
if (goo->spacer) {
if (goo->help == NULL || strlen(goo->help) == 0)
string_buffer_appendf(sb, "\n");
else
string_buffer_appendf(sb, "\n%*s%s\n\n", leftmargin, "", goo->help);
continue;
}
string_buffer_appendf(sb, "%*s", leftmargin, "");
if (goo->sname[0] == 0)
string_buffer_appendf(sb, " ");
else
string_buffer_appendf(sb, "-%c | ", goo->sname[0]);
string_buffer_appendf(sb, "--%*s ", -longwidth, goo->lname);
string_buffer_appendf(sb, " [ %s ]", goo->svalue); // XXX: displays current value rather than default value
string_buffer_appendf(sb, "%*s", (int) (valuewidth - strlen(goo->svalue)), "");
string_buffer_appendf(sb, " %s ", goo->help);
string_buffer_appendf(sb, "\n");
}
char *usage = string_buffer_to_string(sb);
string_buffer_destroy(sb);
return usage;
}

476
plugins/libapriltags/src/homography.c Normal file
View File

@ -0,0 +1,476 @@
/* 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 <math.h>
#include <stdio.h>
#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
// expresison 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;
}

541
plugins/libapriltags/src/image_u8.c Normal file
View File

@ -0,0 +1,541 @@
/* 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 <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#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;
}
}
}

265
plugins/libapriltags/src/image_u8x3.c Normal file
View File

@ -0,0 +1,265 @@
/* 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 <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#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);
}

225
plugins/libapriltags/src/image_u8x4.c Normal file
View File

@ -0,0 +1,225 @@
/* 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 <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#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);
}

93
plugins/libapriltags/src/libapriltags.cpp Normal file
View File

@ -0,0 +1,93 @@
//
// Created by Pulsar on 2021/8/26.
//
#include <libapriltags/libapriltags.h>
#include <rc_system/functions.h>
#include <vector>
#include <libapriltags/apriltag.h>
#include <libapriltags/apriltag_pose.h>
#include <libapriltags/tag16h5.h>
#include <libapriltags/tag25h9.h>
#include <libapriltags/tag36h11.h>
#include <libapriltags/tagCircle21h7.h>
#include <libapriltags/tagCircle49h12.h>
#include <libapriltags/tagCustom48h12.h>
#include <libapriltags/tagStandard41h12.h>
#include <libapriltags/tagStandard52h13.h>
#include <utility>
#include <rc_log/slog.hpp>
void *InitCallback() {
}
void *BeforeDetcetCallback(void *args) {
}
void DetectCallback(cv::Mat &frame, void *args, const std::shared_ptr<rccore::message::MoveMessage> &p_move_message) {
apriltag_family_t *tf = tag36h11_create();
apriltag_detector_t *td = apriltag_detector_create();
apriltag_detector_add_family(td, tf);
td->quad_decimate = 1.0;
WHEEL_DATA wheelData = {0.0, 0.0, 0.0, 0.0};
p_move_message->push_message(wheelData);
cv::Mat gray;
cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
image_u8_t im = {
.width = gray.cols, .height = gray.rows, .stride = gray.cols, .buf = gray.data};
zarray_t *detections = apriltag_detector_detect(td, &im);
size_t tag_number = zarray_size(detections);
apriltag_detection_t min_det;
for (size_t i = 0; i < tag_number; i++) {
apriltag_detection_t *det;
zarray_get(detections, i, &det);
line(
frame, cv::Point(det->c[0], det->c[1]), cv::Point(frame.cols / 2, frame.rows / 2),
cv::Scalar(255, 255, 0), 3);
line(
frame, cv::Point(det->p[0][0], det->p[0][1]), cv::Point(det->p[1][0], det->p[1][1]),
cv::Scalar(0, 0, 0xff), 2);
line(
frame, cv::Point(det->p[0][0], det->p[0][1]), cv::Point(det->p[3][0], det->p[3][1]),
cv::Scalar(0, 0xff, 0), 2);
line(
frame, cv::Point(det->p[1][0], det->p[1][1]), cv::Point(det->p[2][0], det->p[2][1]),
cv::Scalar(0xff, 0, 0), 2);
line(
frame, cv::Point(det->p[2][0], det->p[2][1]), cv::Point(det->p[3][0], det->p[3][1]),
cv::Scalar(0xff, 0, 0), 2);
std::stringstream tag_id;
tag_id << det->id;
cv::String text = tag_id.str();
int baseline;
cv::Size text_size = cv::getTextSize(text, cv::FONT_HERSHEY_SIMPLEX, 1.0, 2, &baseline);
putText(
frame, text,
cv::Point(det->c[0] - text_size.width / 2, det->c[1] + text_size.height / 2),
cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(0xff, 0x99, 0), 2);
}
slog::info << frame.size() << slog::endl;
cv::imshow("frame", frame);
cv::waitKey(1);
apriltag_detector_destroy(td);
}
void *AfterDetcetCallback(void *args) {
}
void OnThreadBreakCallback(void *args) {
}

1970
plugins/libapriltags/src/matd.c Normal file
View File

File diff suppressed because it is too large Load Diff

251
plugins/libapriltags/src/pam.c Normal file
View File

@ -0,0 +1,251 @@
/* 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 <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <assert.h>
#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 (int 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;
}

388
plugins/libapriltags/src/pjpeg-idct.c Normal file
View File

@ -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 <math.h>
#include <stdint.h>
#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);
}

868
plugins/libapriltags/src/pjpeg.c Normal file
View File

@ -0,0 +1,868 @@
/* 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 <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <stdint.h>
#include <string.h>
#include "pjpeg.h"
#include "image_u8.h"
#include "image_u8x3.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 coefficents
// 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 += ((-1) << 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 += ((-1) << 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, "r");
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);
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);
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;
}

153
plugins/libapriltags/src/pnm.c Normal file
View File

@ -0,0 +1,153 @@
/* 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 <assert.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#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);
}

728
plugins/libapriltags/src/string_util.c Normal file
View File

@ -0,0 +1,728 @@
/* 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 <assert.h>
#include <ctype.h>
#include <errno.h>
#include <string.h>
#include <stdarg.h>
#include <stdlib.h>
#include <stdio.h>
#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() {
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);
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);
int 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;
}

261
plugins/libapriltags/src/svd22.c Normal file
View File

@ -0,0 +1,261 @@
/* 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 <math.h>
#include <string.h>
#include <assert.h>
#include <stdio.h>
#include "common/doubles.h"
/** 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);
}

114
plugins/libapriltags/src/tag16h5.c Normal file
View File

@ -0,0 +1,114 @@
/* 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 <stdlib.h>
#include "tag16h5.h"
apriltag_family_t *tag16h5_create() {
apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t));
tf->name = strdup("tag16h5");
tf->h = 5;
tf->ncodes = 30;
tf->codes = calloc(30, sizeof(uint64_t));
tf->codes[0] = 0x00000000000027c8UL;
tf->codes[1] = 0x00000000000031b6UL;
tf->codes[2] = 0x0000000000003859UL;
tf->codes[3] = 0x000000000000569cUL;
tf->codes[4] = 0x0000000000006c76UL;
tf->codes[5] = 0x0000000000007ddbUL;
tf->codes[6] = 0x000000000000af09UL;
tf->codes[7] = 0x000000000000f5a1UL;
tf->codes[8] = 0x000000000000fb8bUL;
tf->codes[9] = 0x0000000000001cb9UL;
tf->codes[10] = 0x00000000000028caUL;
tf->codes[11] = 0x000000000000e8dcUL;
tf->codes[12] = 0x0000000000001426UL;
tf->codes[13] = 0x0000000000005770UL;
tf->codes[14] = 0x0000000000009253UL;
tf->codes[15] = 0x000000000000b702UL;
tf->codes[16] = 0x000000000000063aUL;
tf->codes[17] = 0x0000000000008f34UL;
tf->codes[18] = 0x000000000000b4c0UL;
tf->codes[19] = 0x00000000000051ecUL;
tf->codes[20] = 0x000000000000e6f0UL;
tf->codes[21] = 0x0000000000005fa4UL;
tf->codes[22] = 0x000000000000dd43UL;
tf->codes[23] = 0x0000000000001aaaUL;
tf->codes[24] = 0x000000000000e62fUL;
tf->codes[25] = 0x0000000000006dbcUL;
tf->codes[26] = 0x000000000000b6ebUL;
tf->codes[27] = 0x000000000000de10UL;
tf->codes[28] = 0x000000000000154dUL;
tf->codes[29] = 0x000000000000b57aUL;
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->codes);
free(tf->bit_x);
free(tf->bit_y);
free(tf->name);
free(tf);
}

137
plugins/libapriltags/src/tag25h9.c Normal file
View File

@ -0,0 +1,137 @@
/* 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 <stdlib.h>
#include "tag25h9.h"
apriltag_family_t *tag25h9_create() {
apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t));
tf->name = strdup("tag25h9");
tf->h = 9;
tf->ncodes = 35;
tf->codes = calloc(35, sizeof(uint64_t));
tf->codes[0] = 0x000000000156f1f4UL;
tf->codes[1] = 0x0000000001f28cd5UL;
tf->codes[2] = 0x00000000016ce32cUL;
tf->codes[3] = 0x0000000001ea379cUL;
tf->codes[4] = 0x0000000001390f89UL;
tf->codes[5] = 0x000000000034fad0UL;
tf->codes[6] = 0x00000000007dcdb5UL;
tf->codes[7] = 0x000000000119ba95UL;
tf->codes[8] = 0x0000000001ae9daaUL;
tf->codes[9] = 0x0000000000df02aaUL;
tf->codes[10] = 0x000000000082fc15UL;
tf->codes[11] = 0x0000000000465123UL;
tf->codes[12] = 0x0000000000ceee98UL;
tf->codes[13] = 0x0000000001f17260UL;
tf->codes[14] = 0x00000000014429cdUL;
tf->codes[15] = 0x00000000017248a8UL;
tf->codes[16] = 0x00000000016ad452UL;
tf->codes[17] = 0x00000000009670adUL;
tf->codes[18] = 0x00000000016f65b2UL;
tf->codes[19] = 0x0000000000b8322bUL;
tf->codes[20] = 0x00000000005d715bUL;
tf->codes[21] = 0x0000000001a1c7e7UL;
tf->codes[22] = 0x0000000000d7890dUL;
tf->codes[23] = 0x0000000001813522UL;
tf->codes[24] = 0x0000000001c9c611UL;
tf->codes[25] = 0x000000000099e4a4UL;
tf->codes[26] = 0x0000000000855234UL;
tf->codes[27] = 0x00000000017b81c0UL;
tf->codes[28] = 0x0000000000c294bbUL;
tf->codes[29] = 0x000000000089fae3UL;
tf->codes[30] = 0x000000000044df5fUL;
tf->codes[31] = 0x0000000001360159UL;
tf->codes[32] = 0x0000000000ec31e8UL;
tf->codes[33] = 0x0000000001bcc0f6UL;
tf->codes[34] = 0x0000000000a64f8dUL;
tf->nbits = 25;
tf->bit_x = calloc(25, sizeof(uint32_t));
tf->bit_y = calloc(25, 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] = 2;
tf->bit_y[4] = 2;
tf->bit_x[5] = 3;
tf->bit_y[5] = 2;
tf->bit_x[6] = 5;
tf->bit_y[6] = 1;
tf->bit_x[7] = 5;
tf->bit_y[7] = 2;
tf->bit_x[8] = 5;
tf->bit_y[8] = 3;
tf->bit_x[9] = 5;
tf->bit_y[9] = 4;
tf->bit_x[10] = 4;
tf->bit_y[10] = 2;
tf->bit_x[11] = 4;
tf->bit_y[11] = 3;
tf->bit_x[12] = 5;
tf->bit_y[12] = 5;
tf->bit_x[13] = 4;
tf->bit_y[13] = 5;
tf->bit_x[14] = 3;
tf->bit_y[14] = 5;
tf->bit_x[15] = 2;
tf->bit_y[15] = 5;
tf->bit_x[16] = 4;
tf->bit_y[16] = 4;
tf->bit_x[17] = 3;
tf->bit_y[17] = 4;
tf->bit_x[18] = 1;
tf->bit_y[18] = 5;
tf->bit_x[19] = 1;
tf->bit_y[19] = 4;
tf->bit_x[20] = 1;
tf->bit_y[20] = 3;
tf->bit_x[21] = 1;
tf->bit_y[21] = 2;
tf->bit_x[22] = 2;
tf->bit_y[22] = 4;
tf->bit_x[23] = 2;
tf->bit_y[23] = 3;
tf->bit_x[24] = 3;
tf->bit_y[24] = 3;
tf->width_at_border = 7;
tf->total_width = 9;
tf->reversed_border = false;
return tf;
}
void tag25h9_destroy(apriltag_family_t *tf) {
free(tf->codes);
free(tf->bit_x);
free(tf->bit_y);
free(tf->name);
free(tf);
}

711
plugins/libapriltags/src/tag36h11.c Normal file
View File

@ -0,0 +1,711 @@
/* 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 <stdlib.h>
#include "tag36h11.h"
apriltag_family_t *tag36h11_create() {
apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t));
tf->name = strdup("tag36h11");
tf->h = 11;
tf->ncodes = 587;
tf->codes = calloc(587, sizeof(uint64_t));
tf->codes[0] = 0x0000000d7e00984bUL;
tf->codes[1] = 0x0000000dda664ca7UL;
tf->codes[2] = 0x0000000dc4a1c821UL;
tf->codes[3] = 0x0000000e17b470e9UL;
tf->codes[4] = 0x0000000ef91d01b1UL;
tf->codes[5] = 0x0000000f429cdd73UL;
tf->codes[6] = 0x000000005da29225UL;
tf->codes[7] = 0x00000001106cba43UL;
tf->codes[8] = 0x0000000223bed79dUL;
tf->codes[9] = 0x000000021f51213cUL;
tf->codes[10] = 0x000000033eb19ca6UL;
tf->codes[11] = 0x00000003f76eb0f8UL;
tf->codes[12] = 0x0000000469a97414UL;
tf->codes[13] = 0x000000045dcfe0b0UL;
tf->codes[14] = 0x00000004a6465f72UL;
tf->codes[15] = 0x000000051801db96UL;
tf->codes[16] = 0x00000005eb946b4eUL;
tf->codes[17] = 0x000000068a7cc2ecUL;
tf->codes[18] = 0x00000006f0ba2652UL;
tf->codes[19] = 0x000000078765559dUL;
tf->codes[20] = 0x000000087b83d129UL;
tf->codes[21] = 0x000000086cc4a5c5UL;
tf->codes[22] = 0x00000008b64df90fUL;
tf->codes[23] = 0x00000009c577b611UL;
tf->codes[24] = 0x0000000a3810f2f5UL;
tf->codes[25] = 0x0000000af4d75b83UL;
tf->codes[26] = 0x0000000b59a03fefUL;
tf->codes[27] = 0x0000000bb1096f85UL;
tf->codes[28] = 0x0000000d1b92fc76UL;
tf->codes[29] = 0x0000000d0dd509d2UL;
tf->codes[30] = 0x0000000e2cfda160UL;
tf->codes[31] = 0x00000002ff497c63UL;
tf->codes[32] = 0x000000047240671bUL;
tf->codes[33] = 0x00000005047a2e55UL;
tf->codes[34] = 0x0000000635ca87c7UL;
tf->codes[35] = 0x0000000691254166UL;
tf->codes[36] = 0x000000068f43d94aUL;
tf->codes[37] = 0x00000006ef24bdb6UL;
tf->codes[38] = 0x00000008cdd8f886UL;
tf->codes[39] = 0x00000009de96b718UL;
tf->codes[40] = 0x0000000aff6e5a8aUL;
tf->codes[41] = 0x0000000bae46f029UL;
tf->codes[42] = 0x0000000d225b6d59UL;
tf->codes[43] = 0x0000000df8ba8c01UL;
tf->codes[44] = 0x0000000e3744a22fUL;
tf->codes[45] = 0x0000000fbb59375dUL;
tf->codes[46] = 0x000000018a916828UL;
tf->codes[47] = 0x000000022f29c1baUL;
tf->codes[48] = 0x0000000286887d58UL;
tf->codes[49] = 0x000000041392322eUL;
tf->codes[50] = 0x000000075d18ecd1UL;
tf->codes[51] = 0x000000087c302743UL;
tf->codes[52] = 0x00000008c6317ba9UL;
tf->codes[53] = 0x00000009e40f36d7UL;
tf->codes[54] = 0x0000000c0e5a806aUL;
tf->codes[55] = 0x0000000cc78cb87cUL;
tf->codes[56] = 0x000000012d2f2d01UL;
tf->codes[57] = 0x0000000379f36a21UL;
tf->codes[58] = 0x00000006973f59acUL;
tf->codes[59] = 0x00000007789ea9f4UL;
tf->codes[60] = 0x00000008f1c73e84UL;
tf->codes[61] = 0x00000008dd287a20UL;
tf->codes[62] = 0x000000094a4eee4cUL;
tf->codes[63] = 0x0000000a455379b5UL;
tf->codes[64] = 0x0000000a9e92987dUL;
tf->codes[65] = 0x0000000bd25cb40bUL;
tf->codes[66] = 0x0000000be98d3582UL;
tf->codes[67] = 0x0000000d3d5972b2UL;
tf->codes[68] = 0x000000014c53d7c7UL;
tf->codes[69] = 0x00000004f1796936UL;
tf->codes[70] = 0x00000004e71fed1aUL;
tf->codes[71] = 0x000000066d46fae0UL;
tf->codes[72] = 0x0000000a55abb933UL;
tf->codes[73] = 0x0000000ebee1accaUL;
tf->codes[74] = 0x00000001ad4ba6a4UL;
tf->codes[75] = 0x0000000305b17571UL;
tf->codes[76] = 0x0000000553611351UL;
tf->codes[77] = 0x000000059ca62775UL;
tf->codes[78] = 0x00000007819cb6a1UL;
tf->codes[79] = 0x0000000edb7bc9ebUL;
tf->codes[80] = 0x00000005b2694212UL;
tf->codes[81] = 0x000000072e12d185UL;
tf->codes[82] = 0x0000000ed6152e2cUL;
tf->codes[83] = 0x00000005bcdadbf3UL;
tf->codes[84] = 0x000000078e0aa0c6UL;
tf->codes[85] = 0x0000000c60a0b909UL;
tf->codes[86] = 0x0000000ef9a34b0dUL;
tf->codes[87] = 0x0000000398a6621aUL;
tf->codes[88] = 0x0000000a8a27c944UL;
tf->codes[89] = 0x00000004b564304eUL;
tf->codes[90] = 0x000000052902b4e2UL;
tf->codes[91] = 0x0000000857280b56UL;
tf->codes[92] = 0x0000000a91b2c84bUL;
tf->codes[93] = 0x0000000e91df939bUL;
tf->codes[94] = 0x00000001fa405f28UL;
tf->codes[95] = 0x000000023793ab86UL;
tf->codes[96] = 0x000000068c17729fUL;
tf->codes[97] = 0x00000009fbf3b840UL;
tf->codes[98] = 0x000000036922413cUL;
tf->codes[99] = 0x00000004eb5f946eUL;
tf->codes[100] = 0x0000000533fe2404UL;
tf->codes[101] = 0x000000063de7d35eUL;
tf->codes[102] = 0x0000000925eddc72UL;
tf->codes[103] = 0x000000099b8b3896UL;
tf->codes[104] = 0x0000000aace4c708UL;
tf->codes[105] = 0x0000000c22994af0UL;
tf->codes[106] = 0x00000008f1eae41bUL;
tf->codes[107] = 0x0000000d95fb486cUL;
tf->codes[108] = 0x000000013fb77857UL;
tf->codes[109] = 0x00000004fe0983a3UL;
tf->codes[110] = 0x0000000d559bf8a9UL;
tf->codes[111] = 0x0000000e1855d78dUL;
tf->codes[112] = 0x0000000fec8daaadUL;
tf->codes[113] = 0x000000071ecb6d95UL;
tf->codes[114] = 0x0000000dc9e50e4cUL;
tf->codes[115] = 0x0000000ca3a4c259UL;
tf->codes[116] = 0x0000000740d12bbfUL;
tf->codes[117] = 0x0000000aeedd18e0UL;
tf->codes[118] = 0x0000000b509b9c8eUL;
tf->codes[119] = 0x00000005232fea1cUL;
tf->codes[120] = 0x000000019282d18bUL;
tf->codes[121] = 0x000000076c22d67bUL;
tf->codes[122] = 0x0000000936beb34bUL;
tf->codes[123] = 0x000000008a5ea8ddUL;
tf->codes[124] = 0x0000000679eadc28UL;
tf->codes[125] = 0x0000000a08e119c5UL;
tf->codes[126] = 0x000000020a6e3e24UL;
tf->codes[127] = 0x00000007eab9c239UL;
tf->codes[128] = 0x000000096632c32eUL;
tf->codes[129] = 0x0000000470d06e44UL;
tf->codes[130] = 0x00000008a70212fbUL;
tf->codes[131] = 0x00000000a7e4251bUL;
tf->codes[132] = 0x00000009ec762cc0UL;
tf->codes[133] = 0x0000000d8a3a1f48UL;
tf->codes[134] = 0x0000000db680f346UL;
tf->codes[135] = 0x00000004a1e93a9dUL;
tf->codes[136] = 0x0000000638ddc04fUL;
tf->codes[137] = 0x00000004c2fcc993UL;
tf->codes[138] = 0x000000001ef28c95UL;
tf->codes[139] = 0x0000000bf0d9792dUL;
tf->codes[140] = 0x00000006d27557c3UL;
tf->codes[141] = 0x0000000623f977f4UL;
tf->codes[142] = 0x000000035b43be57UL;
tf->codes[143] = 0x0000000bb0c428d5UL;
tf->codes[144] = 0x0000000a6f01474dUL;
tf->codes[145] = 0x00000005a70c9749UL;
tf->codes[146] = 0x000000020ddabc3bUL;
tf->codes[147] = 0x00000002eabd78cfUL;
tf->codes[148] = 0x000000090aa18f88UL;
tf->codes[149] = 0x0000000a9ea89350UL;
tf->codes[150] = 0x00000003cdb39b22UL;
tf->codes[151] = 0x0000000839a08f34UL;
tf->codes[152] = 0x0000000169bb814eUL;
tf->codes[153] = 0x00000001a575ab08UL;
tf->codes[154] = 0x0000000a04d3d5a2UL;
tf->codes[155] = 0x0000000bf7902f2bUL;
tf->codes[156] = 0x0000000095a5e65cUL;
tf->codes[157] = 0x000000092e8fce94UL;
tf->codes[158] = 0x000000067ef48d12UL;
tf->codes[159] = 0x00000006400dbcacUL;
tf->codes[160] = 0x0000000b12d8fb9fUL;
tf->codes[161] = 0x00000000347f45d3UL;
tf->codes[162] = 0x0000000b35826f56UL;
tf->codes[163] = 0x0000000c546ac6e4UL;
tf->codes[164] = 0x000000081cc35b66UL;
tf->codes[165] = 0x000000041d14bd57UL;
tf->codes[166] = 0x00000000c052b168UL;
tf->codes[167] = 0x00000007d6ce5018UL;
tf->codes[168] = 0x0000000ab4ed5edeUL;
tf->codes[169] = 0x00000005af817119UL;
tf->codes[170] = 0x0000000d1454b182UL;
tf->codes[171] = 0x00000002badb090bUL;
tf->codes[172] = 0x000000003fcb4c0cUL;
tf->codes[173] = 0x00000002f1c28fd8UL;
tf->codes[174] = 0x000000093608c6f7UL;
tf->codes[175] = 0x00000004c93ba2b5UL;
tf->codes[176] = 0x000000007d950a5dUL;
tf->codes[177] = 0x0000000e54b3d3fcUL;
tf->codes[178] = 0x000000015560cf9dUL;
tf->codes[179] = 0x0000000189e4958aUL;
tf->codes[180] = 0x000000062140e9d2UL;
tf->codes[181] = 0x0000000723bc1cdbUL;
tf->codes[182] = 0x00000002063f26faUL;
tf->codes[183] = 0x0000000fa08ab19fUL;
tf->codes[184] = 0x00000007955641dbUL;
tf->codes[185] = 0x0000000646b01daaUL;
tf->codes[186] = 0x000000071cd427ccUL;
tf->codes[187] = 0x000000009a42f7d4UL;
tf->codes[188] = 0x0000000717edc643UL;
tf->codes[189] = 0x000000015eb94367UL;
tf->codes[190] = 0x00000008392e6bb2UL;
tf->codes[191] = 0x0000000832408542UL;
tf->codes[192] = 0x00000002b9b874beUL;
tf->codes[193] = 0x0000000b21f4730dUL;
tf->codes[194] = 0x0000000b5d8f24c9UL;
tf->codes[195] = 0x00000007dbaf6931UL;
tf->codes[196] = 0x00000001b4e33629UL;
tf->codes[197] = 0x000000013452e710UL;
tf->codes[198] = 0x0000000e974af612UL;
tf->codes[199] = 0x00000001df61d29aUL;
tf->codes[200] = 0x000000099f2532adUL;
tf->codes[201] = 0x0000000e50ec71b4UL;
tf->codes[202] = 0x00000005df0a36e8UL;
tf->codes[203] = 0x00000004934e4ceaUL;
tf->codes[204] = 0x0000000e34a0b4bdUL;
tf->codes[205] = 0x0000000b7b26b588UL;
tf->codes[206] = 0x00000000f255118dUL;
tf->codes[207] = 0x0000000d0c8fa31eUL;
tf->codes[208] = 0x000000006a50c94fUL;
tf->codes[209] = 0x0000000f28aa9f06UL;
tf->codes[210] = 0x0000000131d194d8UL;
tf->codes[211] = 0x0000000622e3da79UL;
tf->codes[212] = 0x0000000ac7478303UL;
tf->codes[213] = 0x0000000c8f2521d7UL;
tf->codes[214] = 0x00000006c9c881f5UL;
tf->codes[215] = 0x000000049e38b60aUL;
tf->codes[216] = 0x0000000513d8df65UL;
tf->codes[217] = 0x0000000d7c2b0785UL;
tf->codes[218] = 0x00000009f6f9d75aUL;
tf->codes[219] = 0x00000009f6966020UL;
tf->codes[220] = 0x00000001e1a54e33UL;
tf->codes[221] = 0x0000000c04d63419UL;
tf->codes[222] = 0x0000000946e04cd7UL;
tf->codes[223] = 0x00000001bdac5902UL;
tf->codes[224] = 0x000000056469b830UL;
tf->codes[225] = 0x0000000ffad59569UL;
tf->codes[226] = 0x000000086970e7d8UL;
tf->codes[227] = 0x00000008a4b41e12UL;
tf->codes[228] = 0x0000000ad4688e3bUL;
tf->codes[229] = 0x000000085f8f5df4UL;
tf->codes[230] = 0x0000000d833a0893UL;
tf->codes[231] = 0x00000002a36fdd7cUL;
tf->codes[232] = 0x0000000d6a857cf2UL;
tf->codes[233] = 0x00000008829bc35cUL;
tf->codes[234] = 0x00000005e50d79bcUL;
tf->codes[235] = 0x0000000fbb8035e4UL;
tf->codes[236] = 0x0000000c1a95bebfUL;
tf->codes[237] = 0x0000000036b0baf8UL;
tf->codes[238] = 0x0000000e0da964eaUL;
tf->codes[239] = 0x0000000b6483689bUL;
tf->codes[240] = 0x00000007c8e2f4c1UL;
tf->codes[241] = 0x00000005b856a23bUL;
tf->codes[242] = 0x00000002fc183995UL;
tf->codes[243] = 0x0000000e914b6d70UL;
tf->codes[244] = 0x0000000b31041969UL;
tf->codes[245] = 0x00000001bb478493UL;
tf->codes[246] = 0x0000000063e2b456UL;
tf->codes[247] = 0x0000000f2a082b9cUL;
tf->codes[248] = 0x00000008e5e646eaUL;
tf->codes[249] = 0x000000008172f8f6UL;
tf->codes[250] = 0x00000000dacd923eUL;
tf->codes[251] = 0x0000000e5dcf0e2eUL;
tf->codes[252] = 0x0000000bf9446baeUL;
tf->codes[253] = 0x00000004822d50d1UL;
tf->codes[254] = 0x000000026e710bf5UL;
tf->codes[255] = 0x0000000b90ba2a24UL;
tf->codes[256] = 0x0000000f3b25aa73UL;
tf->codes[257] = 0x0000000809ad589bUL;
tf->codes[258] = 0x000000094cc1e254UL;
tf->codes[259] = 0x00000005334a3adbUL;
tf->codes[260] = 0x0000000592886b2fUL;
tf->codes[261] = 0x0000000bf64704aaUL;
tf->codes[262] = 0x0000000566dbf24cUL;
tf->codes[263] = 0x000000072203e692UL;
tf->codes[264] = 0x000000064e61e809UL;
tf->codes[265] = 0x0000000d7259aad6UL;
tf->codes[266] = 0x00000007b924aedcUL;
tf->codes[267] = 0x00000002df2184e8UL;
tf->codes[268] = 0x0000000353d1eca7UL;
tf->codes[269] = 0x0000000fce30d7ceUL;
tf->codes[270] = 0x0000000f7b0f436eUL;
tf->codes[271] = 0x000000057e8d8f68UL;
tf->codes[272] = 0x00000008c79e60dbUL;
tf->codes[273] = 0x00000009c8362b2bUL;
tf->codes[274] = 0x000000063a5804f2UL;
tf->codes[275] = 0x00000009298353dcUL;
tf->codes[276] = 0x00000006f98a71c8UL;
tf->codes[277] = 0x0000000a5731f693UL;
tf->codes[278] = 0x000000021ca5c870UL;
tf->codes[279] = 0x00000001c2107fd3UL;
tf->codes[280] = 0x00000006181f6c39UL;
tf->codes[281] = 0x000000019e574304UL;
tf->codes[282] = 0x0000000329937606UL;
tf->codes[283] = 0x0000000043d5c70dUL;
tf->codes[284] = 0x00000009b18ff162UL;
tf->codes[285] = 0x00000008e2ccfebfUL;
tf->codes[286] = 0x000000072b7b9b54UL;
tf->codes[287] = 0x00000009b71f4f3cUL;
tf->codes[288] = 0x0000000935d7393eUL;
tf->codes[289] = 0x000000065938881aUL;
tf->codes[290] = 0x00000006a5bd6f2dUL;
tf->codes[291] = 0x0000000a19783306UL;
tf->codes[292] = 0x0000000e6472f4d7UL;
tf->codes[293] = 0x000000081163df5aUL;
tf->codes[294] = 0x0000000a838e1cbdUL;
tf->codes[295] = 0x0000000982748477UL;
tf->codes[296] = 0x0000000050c54febUL;
tf->codes[297] = 0x00000000d82fbb58UL;
tf->codes[298] = 0x00000002c4c72799UL;
tf->codes[299] = 0x000000097d259ad6UL;
tf->codes[300] = 0x000000022d9a43edUL;
tf->codes[301] = 0x0000000fdb162a9fUL;
tf->codes[302] = 0x00000000cb4a727dUL;
tf->codes[303] = 0x00000004fae2e371UL;
tf->codes[304] = 0x0000000535b5be8bUL;
tf->codes[305] = 0x000000048795908aUL;
tf->codes[306] = 0x0000000ce7c18962UL;
tf->codes[307] = 0x00000004ea154d80UL;
tf->codes[308] = 0x000000050c064889UL;
tf->codes[309] = 0x00000008d97fc75dUL;
tf->codes[310] = 0x0000000c8bd9ec61UL;
tf->codes[311] = 0x000000083ee8e8bbUL;
tf->codes[312] = 0x0000000c8431419aUL;
tf->codes[313] = 0x00000001aa78079dUL;
tf->codes[314] = 0x00000008111aa4a5UL;
tf->codes[315] = 0x0000000dfa3a69feUL;
tf->codes[316] = 0x000000051630d83fUL;
tf->codes[317] = 0x00000002d930fb3fUL;
tf->codes[318] = 0x00000002133116e5UL;
tf->codes[319] = 0x0000000ae5395522UL;
tf->codes[320] = 0x0000000bc07a4e8aUL;
tf->codes[321] = 0x000000057bf08ba0UL;
tf->codes[322] = 0x00000006cb18036aUL;
tf->codes[323] = 0x0000000f0e2e4b75UL;
tf->codes[324] = 0x00000003eb692b6fUL;
tf->codes[325] = 0x0000000d8178a3faUL;
tf->codes[326] = 0x0000000238cce6a6UL;
tf->codes[327] = 0x0000000e97d5cdd7UL;
tf->codes[328] = 0x0000000fe10d8d5eUL;
tf->codes[329] = 0x0000000b39584a1dUL;
tf->codes[330] = 0x0000000ca03536fdUL;
tf->codes[331] = 0x0000000aa61f3998UL;
tf->codes[332] = 0x000000072ff23ec2UL;
tf->codes[333] = 0x000000015aa7d770UL;
tf->codes[334] = 0x000000057a3a1282UL;
tf->codes[335] = 0x0000000d1f3902dcUL;
tf->codes[336] = 0x00000006554c9388UL;
tf->codes[337] = 0x0000000fd01283c7UL;
tf->codes[338] = 0x0000000e8baa42c5UL;
tf->codes[339] = 0x000000072cee6adfUL;
tf->codes[340] = 0x0000000f6614b3faUL;
tf->codes[341] = 0x000000095c3778a2UL;
tf->codes[342] = 0x00000007da4cea7aUL;
tf->codes[343] = 0x0000000d18a5912cUL;
tf->codes[344] = 0x0000000d116426e5UL;
tf->codes[345] = 0x000000027c17bc1cUL;
tf->codes[346] = 0x0000000b95b53bc1UL;
tf->codes[347] = 0x0000000c8f937a05UL;
tf->codes[348] = 0x0000000ed220c9bdUL;
tf->codes[349] = 0x00000000c97d72abUL;
tf->codes[350] = 0x00000008fb1217aeUL;
tf->codes[351] = 0x000000025ca8a5a1UL;
tf->codes[352] = 0x0000000b261b871bUL;
tf->codes[353] = 0x00000001bef0a056UL;
tf->codes[354] = 0x0000000806a51179UL;
tf->codes[355] = 0x0000000eed249145UL;
tf->codes[356] = 0x00000003f82aecebUL;
tf->codes[357] = 0x0000000cc56e9acfUL;
tf->codes[358] = 0x00000002e78d01ebUL;
tf->codes[359] = 0x0000000102cee17fUL;
tf->codes[360] = 0x000000037caad3d5UL;
tf->codes[361] = 0x000000016ac5b1eeUL;
tf->codes[362] = 0x00000002af164eceUL;
tf->codes[363] = 0x0000000d4cd81dc9UL;
tf->codes[364] = 0x000000012263a7e7UL;
tf->codes[365] = 0x000000057ac7d117UL;
tf->codes[366] = 0x00000009391d9740UL;
tf->codes[367] = 0x00000007aedaa77fUL;
tf->codes[368] = 0x00000009675a3c72UL;
tf->codes[369] = 0x0000000277f25191UL;
tf->codes[370] = 0x0000000ebb6e64b9UL;
tf->codes[371] = 0x00000007ad3ef747UL;
tf->codes[372] = 0x000000012759b181UL;
tf->codes[373] = 0x0000000948257d4dUL;
tf->codes[374] = 0x0000000b63a850f6UL;
tf->codes[375] = 0x00000003a52a8f75UL;
tf->codes[376] = 0x00000004a019532cUL;
tf->codes[377] = 0x0000000a021a7529UL;
tf->codes[378] = 0x0000000cc661876dUL;
tf->codes[379] = 0x00000004085afd05UL;
tf->codes[380] = 0x0000000e7048e089UL;
tf->codes[381] = 0x00000003f979cdc6UL;
tf->codes[382] = 0x0000000d9da9071bUL;
tf->codes[383] = 0x0000000ed2fc5b68UL;
tf->codes[384] = 0x000000079d64c3a1UL;
tf->codes[385] = 0x0000000fd44e2361UL;
tf->codes[386] = 0x00000008eea46a74UL;
tf->codes[387] = 0x000000042233b9c2UL;
tf->codes[388] = 0x0000000ae4d1765dUL;
tf->codes[389] = 0x00000007303a094cUL;
tf->codes[390] = 0x00000002d7033abeUL;
tf->codes[391] = 0x00000003dcc2b0b4UL;
tf->codes[392] = 0x00000000f0967d09UL;
tf->codes[393] = 0x000000006f0cd7deUL;
tf->codes[394] = 0x000000009807aca0UL;
tf->codes[395] = 0x00000003a295cad3UL;
tf->codes[396] = 0x00000002b106b202UL;
tf->codes[397] = 0x00000003f38a828eUL;
tf->codes[398] = 0x000000078af46596UL;
tf->codes[399] = 0x0000000bda2dc713UL;
tf->codes[400] = 0x00000009a8c8c9d9UL;
tf->codes[401] = 0x00000006a0f2ddceUL;
tf->codes[402] = 0x0000000a76af6fe2UL;
tf->codes[403] = 0x0000000086f66fa4UL;
tf->codes[404] = 0x0000000d52d63f8dUL;
tf->codes[405] = 0x000000089f7a6e73UL;
tf->codes[406] = 0x0000000cc6b23362UL;
tf->codes[407] = 0x0000000b4ebf3c39UL;
tf->codes[408] = 0x0000000564f300faUL;
tf->codes[409] = 0x0000000e8de3a706UL;
tf->codes[410] = 0x000000079a033b61UL;
tf->codes[411] = 0x0000000765e160c5UL;
tf->codes[412] = 0x0000000a266a4f85UL;
tf->codes[413] = 0x0000000a68c38c24UL;
tf->codes[414] = 0x0000000dca0711fbUL;
tf->codes[415] = 0x000000085fba85baUL;
tf->codes[416] = 0x000000037a207b46UL;
tf->codes[417] = 0x0000000158fcc4d0UL;
tf->codes[418] = 0x00000000569d79b3UL;
tf->codes[419] = 0x00000007b1a25555UL;
tf->codes[420] = 0x0000000a8ae22468UL;
tf->codes[421] = 0x00000007c592bdfdUL;
tf->codes[422] = 0x00000000c59a5f66UL;
tf->codes[423] = 0x0000000b1115daa3UL;
tf->codes[424] = 0x0000000f17c87177UL;
tf->codes[425] = 0x00000006769d766bUL;
tf->codes[426] = 0x00000002b637356dUL;
tf->codes[427] = 0x000000013d8685acUL;
tf->codes[428] = 0x0000000f24cb6ec0UL;
tf->codes[429] = 0x00000000bd0b56d1UL;
tf->codes[430] = 0x000000042ff0e26dUL;
tf->codes[431] = 0x0000000b41609267UL;
tf->codes[432] = 0x000000096f9518afUL;
tf->codes[433] = 0x0000000c56f96636UL;
tf->codes[434] = 0x00000004a8e10349UL;
tf->codes[435] = 0x0000000863512171UL;
tf->codes[436] = 0x0000000ea455d86cUL;
tf->codes[437] = 0x0000000bd0e25279UL;
tf->codes[438] = 0x0000000e65e3f761UL;
tf->codes[439] = 0x000000036c84a922UL;
tf->codes[440] = 0x000000085fd1b38fUL;
tf->codes[441] = 0x0000000657c91539UL;
tf->codes[442] = 0x000000015033fe04UL;
tf->codes[443] = 0x000000009051c921UL;
tf->codes[444] = 0x0000000ab27d80d8UL;
tf->codes[445] = 0x0000000f92f7d0a1UL;
tf->codes[446] = 0x00000008eb6bb737UL;
tf->codes[447] = 0x000000010b5b0f63UL;
tf->codes[448] = 0x00000006c9c7ad63UL;
tf->codes[449] = 0x0000000f66fe70aeUL;
tf->codes[450] = 0x0000000ca579bd92UL;
tf->codes[451] = 0x0000000956198e4dUL;
tf->codes[452] = 0x000000029e4405e5UL;
tf->codes[453] = 0x0000000e44eb885cUL;
tf->codes[454] = 0x000000041612456cUL;
tf->codes[455] = 0x0000000ea45e0abfUL;
tf->codes[456] = 0x0000000d326529bdUL;
tf->codes[457] = 0x00000007b2c33cefUL;
tf->codes[458] = 0x000000080bc9b558UL;
tf->codes[459] = 0x00000007169b9740UL;
tf->codes[460] = 0x0000000c37f99209UL;
tf->codes[461] = 0x000000031ff6dab9UL;
tf->codes[462] = 0x0000000c795190edUL;
tf->codes[463] = 0x0000000a7636e95fUL;
tf->codes[464] = 0x00000009df075841UL;
tf->codes[465] = 0x000000055a083932UL;
tf->codes[466] = 0x0000000a7cbdf630UL;
tf->codes[467] = 0x0000000409ea4ef0UL;
tf->codes[468] = 0x000000092a1991b6UL;
tf->codes[469] = 0x00000004b078dee9UL;
tf->codes[470] = 0x0000000ae18ce9e4UL;
tf->codes[471] = 0x00000005a6e1ef35UL;
tf->codes[472] = 0x00000001a403bd59UL;
tf->codes[473] = 0x000000031ea70a83UL;
tf->codes[474] = 0x00000002bc3c4f3aUL;
tf->codes[475] = 0x00000005c921b3cbUL;
tf->codes[476] = 0x0000000042da05c5UL;
tf->codes[477] = 0x00000001f667d16bUL;
tf->codes[478] = 0x0000000416a368cfUL;
tf->codes[479] = 0x0000000fbc0a7a3bUL;
tf->codes[480] = 0x00000009419f0c7cUL;
tf->codes[481] = 0x000000081be2fa03UL;
tf->codes[482] = 0x000000034e2c172fUL;
tf->codes[483] = 0x000000028648d8aeUL;
tf->codes[484] = 0x0000000c7acbb885UL;
tf->codes[485] = 0x000000045f31eb6aUL;
tf->codes[486] = 0x0000000d1cfc0a7bUL;
tf->codes[487] = 0x000000042c4d260dUL;
tf->codes[488] = 0x0000000cf6584097UL;
tf->codes[489] = 0x000000094b132b14UL;
tf->codes[490] = 0x00000003c5c5df75UL;
tf->codes[491] = 0x00000008ae596fefUL;
tf->codes[492] = 0x0000000aea8054ebUL;
tf->codes[493] = 0x00000000ae9cc573UL;
tf->codes[494] = 0x0000000496fb731bUL;
tf->codes[495] = 0x0000000ebf105662UL;
tf->codes[496] = 0x0000000af9c83a37UL;
tf->codes[497] = 0x0000000c0d64cd6bUL;
tf->codes[498] = 0x00000007b608159aUL;
tf->codes[499] = 0x0000000e74431642UL;
tf->codes[500] = 0x0000000d6fb9d900UL;
tf->codes[501] = 0x0000000291e99de0UL;
tf->codes[502] = 0x000000010500ba9aUL;
tf->codes[503] = 0x00000005cd05d037UL;
tf->codes[504] = 0x0000000a87254fb2UL;
tf->codes[505] = 0x00000009d7824a37UL;
tf->codes[506] = 0x00000008b2c7b47cUL;
tf->codes[507] = 0x000000030c788145UL;
tf->codes[508] = 0x00000002f4e5a8beUL;
tf->codes[509] = 0x0000000badb884daUL;
tf->codes[510] = 0x0000000026e0d5c9UL;
tf->codes[511] = 0x00000006fdbaa32eUL;
tf->codes[512] = 0x000000034758eb31UL;
tf->codes[513] = 0x0000000565cd1b4fUL;
tf->codes[514] = 0x00000002bfd90fb0UL;
tf->codes[515] = 0x0000000093052a6bUL;
tf->codes[516] = 0x0000000d3c13c4b9UL;
tf->codes[517] = 0x00000002daea43bfUL;
tf->codes[518] = 0x0000000a279762bcUL;
tf->codes[519] = 0x0000000f1bd9f22cUL;
tf->codes[520] = 0x00000004b7fec94fUL;
tf->codes[521] = 0x0000000545761d5aUL;
tf->codes[522] = 0x00000007327df411UL;
tf->codes[523] = 0x00000001b52a442eUL;
tf->codes[524] = 0x000000049b0ce108UL;
tf->codes[525] = 0x000000024c764bc8UL;
tf->codes[526] = 0x0000000374563045UL;
tf->codes[527] = 0x0000000a3e8f91c6UL;
tf->codes[528] = 0x00000000e6bd2241UL;
tf->codes[529] = 0x0000000e0e52ee3cUL;
tf->codes[530] = 0x000000007e8e3caaUL;
tf->codes[531] = 0x000000096c2b7372UL;
tf->codes[532] = 0x000000033acbdfdaUL;
tf->codes[533] = 0x0000000b15d91e54UL;
tf->codes[534] = 0x0000000464759ac1UL;
tf->codes[535] = 0x00000006886a1998UL;
tf->codes[536] = 0x000000057f5d3958UL;
tf->codes[537] = 0x00000005a1f5c1f5UL;
tf->codes[538] = 0x00000000b58158adUL;
tf->codes[539] = 0x0000000e712053fbUL;
tf->codes[540] = 0x00000005352ddb25UL;
tf->codes[541] = 0x0000000414b98ea0UL;
tf->codes[542] = 0x000000074f89f546UL;
tf->codes[543] = 0x000000038a56b3c3UL;
tf->codes[544] = 0x000000038db0dc17UL;
tf->codes[545] = 0x0000000aa016a755UL;
tf->codes[546] = 0x0000000dc72366f5UL;
tf->codes[547] = 0x00000000cee93d75UL;
tf->codes[548] = 0x0000000b2fe7a56bUL;
tf->codes[549] = 0x0000000a847ed390UL;
tf->codes[550] = 0x00000008713ef88cUL;
tf->codes[551] = 0x0000000a217cc861UL;
tf->codes[552] = 0x00000008bca25d7bUL;
tf->codes[553] = 0x0000000455526818UL;
tf->codes[554] = 0x0000000ea3a7a180UL;
tf->codes[555] = 0x0000000a9536e5e0UL;
tf->codes[556] = 0x00000009b64a1975UL;
tf->codes[557] = 0x00000005bfc756bcUL;
tf->codes[558] = 0x0000000046aa169bUL;
tf->codes[559] = 0x000000053a17f76fUL;
tf->codes[560] = 0x00000004d6815274UL;
tf->codes[561] = 0x0000000cca9cf3f6UL;
tf->codes[562] = 0x00000004013fcb8bUL;
tf->codes[563] = 0x00000003d26cdfa5UL;
tf->codes[564] = 0x00000005786231f7UL;
tf->codes[565] = 0x00000007d4ab09abUL;
tf->codes[566] = 0x0000000960b5ffbcUL;
tf->codes[567] = 0x00000008914df0d4UL;
tf->codes[568] = 0x00000002fc6f2213UL;
tf->codes[569] = 0x0000000ac235637eUL;
tf->codes[570] = 0x0000000151b28ed3UL;
tf->codes[571] = 0x000000046f79b6dbUL;
tf->codes[572] = 0x00000001382e0c9fUL;
tf->codes[573] = 0x000000053abf983aUL;
tf->codes[574] = 0x0000000383c47adeUL;
tf->codes[575] = 0x00000003fcf88978UL;
tf->codes[576] = 0x0000000eb9079df7UL;
tf->codes[577] = 0x000000009af0714dUL;
tf->codes[578] = 0x0000000da19d1bb7UL;
tf->codes[579] = 0x00000009a02749f8UL;
tf->codes[580] = 0x00000001c62dab9bUL;
tf->codes[581] = 0x00000001a137e44bUL;
tf->codes[582] = 0x00000002867718c7UL;
tf->codes[583] = 0x000000035815525bUL;
tf->codes[584] = 0x00000007cd35c550UL;
tf->codes[585] = 0x00000002164f73a0UL;
tf->codes[586] = 0x0000000e8b772fe0UL;
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->codes);
free(tf->bit_x);
free(tf->bit_y);
free(tf->name);
free(tf);
}

132
plugins/libapriltags/src/tagCircle21h7.c Normal file
View File

@ -0,0 +1,132 @@
/* 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 <stdlib.h>
#include "tagCircle21h7.h"
apriltag_family_t *tagCircle21h7_create() {
apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t));
tf->name = strdup("tagCircle21h7");
tf->h = 7;
tf->ncodes = 38;
tf->codes = calloc(38, sizeof(uint64_t));
tf->codes[0] = 0x0000000000157863UL;
tf->codes[1] = 0x0000000000047e28UL;
tf->codes[2] = 0x00000000001383edUL;
tf->codes[3] = 0x000000000000953cUL;
tf->codes[4] = 0x00000000000da68bUL;
tf->codes[5] = 0x00000000001cac50UL;
tf->codes[6] = 0x00000000000bb215UL;
tf->codes[7] = 0x000000000016ceeeUL;
tf->codes[8] = 0x000000000005d4b3UL;
tf->codes[9] = 0x00000000001ff751UL;
tf->codes[10] = 0x00000000000efd16UL;
tf->codes[11] = 0x0000000000072b3eUL;
tf->codes[12] = 0x0000000000163103UL;
tf->codes[13] = 0x0000000000106e56UL;
tf->codes[14] = 0x00000000001996b9UL;
tf->codes[15] = 0x00000000000c0234UL;
tf->codes[16] = 0x00000000000624d2UL;
tf->codes[17] = 0x00000000001fa985UL;
tf->codes[18] = 0x00000000000344a5UL;
tf->codes[19] = 0x00000000000762fbUL;
tf->codes[20] = 0x000000000019e92bUL;
tf->codes[21] = 0x0000000000043755UL;
tf->codes[22] = 0x000000000001a4f4UL;
tf->codes[23] = 0x000000000010fad8UL;
tf->codes[24] = 0x0000000000001b52UL;
tf->codes[25] = 0x000000000017e59fUL;
tf->codes[26] = 0x00000000000e6f70UL;
tf->codes[27] = 0x00000000000ed47aUL;
tf->codes[28] = 0x00000000000c9931UL;
tf->codes[29] = 0x0000000000014df2UL;
tf->codes[30] = 0x00000000000a06f1UL;
tf->codes[31] = 0x00000000000e5041UL;
tf->codes[32] = 0x000000000012ec03UL;
tf->codes[33] = 0x000000000016724eUL;
tf->codes[34] = 0x00000000000af1a5UL;
tf->codes[35] = 0x000000000008a8acUL;
tf->codes[36] = 0x0000000000015b39UL;
tf->codes[37] = 0x00000000001ec1e3UL;
tf->nbits = 21;
tf->bit_x = calloc(21, sizeof(uint32_t));
tf->bit_y = calloc(21, sizeof(uint32_t));
tf->bit_x[0] = 1;
tf->bit_y[0] = -2;
tf->bit_x[1] = 2;
tf->bit_y[1] = -2;
tf->bit_x[2] = 3;
tf->bit_y[2] = -2;
tf->bit_x[3] = 1;
tf->bit_y[3] = 1;
tf->bit_x[4] = 2;
tf->bit_y[4] = 1;
tf->bit_x[5] = 6;
tf->bit_y[5] = 1;
tf->bit_x[6] = 6;
tf->bit_y[6] = 2;
tf->bit_x[7] = 6;
tf->bit_y[7] = 3;
tf->bit_x[8] = 3;
tf->bit_y[8] = 1;
tf->bit_x[9] = 3;
tf->bit_y[9] = 2;
tf->bit_x[10] = 3;
tf->bit_y[10] = 6;
tf->bit_x[11] = 2;
tf->bit_y[11] = 6;
tf->bit_x[12] = 1;
tf->bit_y[12] = 6;
tf->bit_x[13] = 3;
tf->bit_y[13] = 3;
tf->bit_x[14] = 2;
tf->bit_y[14] = 3;
tf->bit_x[15] = -2;
tf->bit_y[15] = 3;
tf->bit_x[16] = -2;
tf->bit_y[16] = 2;
tf->bit_x[17] = -2;
tf->bit_y[17] = 1;
tf->bit_x[18] = 1;
tf->bit_y[18] = 3;
tf->bit_x[19] = 1;
tf->bit_y[19] = 2;
tf->bit_x[20] = 2;
tf->bit_y[20] = 2;
tf->width_at_border = 5;
tf->total_width = 9;
tf->reversed_border = true;
return tf;
}
void tagCircle21h7_destroy(apriltag_family_t *tf) {
free(tf->codes);
free(tf->bit_x);
free(tf->bit_y);
free(tf->name);
free(tf);
}

65687
plugins/libapriltags/src/tagCircle49h12.c Normal file
View File

File diff suppressed because it is too large Load Diff

42363
plugins/libapriltags/src/tagCustom48h12.c Normal file
View File

File diff suppressed because it is too large Load Diff

2249
plugins/libapriltags/src/tagStandard41h12.c Normal file
View File

File diff suppressed because it is too large Load Diff

48874
plugins/libapriltags/src/tagStandard52h13.c Normal file
View File

File diff suppressed because it is too large Load Diff

146
plugins/libapriltags/src/time_util.c Normal file
View File

@ -0,0 +1,146 @@
/* 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 <stdlib.h>
#include <math.h>
#include "time_util.h"
struct timeutil_rest {
int64_t acc_time;
int64_t start_time;
};
timeutil_rest_t *timeutil_rest_create() {
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() // 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;
}

30
plugins/libapriltags/src/unionfind.c Normal file
View File

@ -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 <stdlib.h>
#include <assert.h>

212
plugins/libapriltags/src/workerpool.c Normal file
View File

@ -0,0 +1,212 @@
/* 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.
*/
#define __USE_GNU
#include <pthread.h>
#include <sched.h>
#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <inttypes.h>
#ifdef _WIN32
#include <windows.h>
#else
#include <unistd.h>
#endif
#include "workerpool.h"
#include "timeprofile.h"
#include "math_util.h"
#include "string_util.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) {
perror("pthread_create");
exit(-1);
}
}
}
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() {
#ifdef WIN32
SYSTEM_INFO sysinfo;
GetSystemInfo(&sysinfo);
return sysinfo.dwNumberOfProcessors;
#else
return sysconf(_SC_NPROCESSORS_ONLN);
#endif
}

55
plugins/libapriltags/src/zarray.c Normal file
View File

@ -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 <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#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)()) {
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);
}
}

529
plugins/libapriltags/src/zhash.c Normal file
View File

@ -0,0 +1,529 @@
/* 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 <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#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);
zh->nentries = nentries;
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)()) {
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)()) {
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)()) {
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)()) {
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;
int32_t hash = 0;
while (*a != 0) {
hash = (hash << 7) + (hash >> 23);
hash += *a;
a++;
}
return (uint32_t) 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);
}
}

406
plugins/libapriltags/src/zmaxheap.c Normal file
View File

@ -0,0 +1,406 @@
/* 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 <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <assert.h>
#include <stdint.h>
#include "zmaxheap.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)()) {
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) {
printf("Warning: zmaxheap_vmap item %d is NULL\n", idx);
fflush(stdout);
}
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);
return maxheapify(heap, betterchild);
}
}
#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() {
int cap = 10000;
int sz = 0;
int32_t *vals = calloc(sizeof(int32_t), cap);
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) {
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);
}