├── .gitignore ├── CMakeLists.txt ├── README.md ├── images ├── clipboard │ ├── 1-debug_0_0.png │ ├── 10-debug_0_0.png │ ├── 2-debug_0_0.png │ ├── 3-debug_0_0.png │ ├── 4-debug_0_0.png │ ├── 5-debug_0_0.png │ ├── 6-debug_0_0.png │ ├── 7-debug_0_0.png │ ├── 8-debug_0_0.png │ └── 9-debug_0_0.png └── iphonecam │ ├── IMG_0612.jpg │ ├── IMG_0613.jpg │ ├── IMG_0614.jpg │ ├── IMG_0615.jpg │ └── IMG_0617.jpg └── src ├── AprilTypes.h ├── CMakeLists.txt ├── CameraUtil.cpp ├── CameraUtil.h ├── DebugImage.cpp ├── DebugImage.h ├── Geometry.cpp ├── Geometry.h ├── GrayModel.cpp ├── GrayModel.h ├── MathUtil.cpp ├── MathUtil.h ├── Profiler.h ├── Refine.cpp ├── Refine.h ├── TagDetection.h ├── TagDetector.cpp ├── TagDetector.h ├── TagFamilies.cpp ├── TagFamily.cpp ├── TagFamily.h ├── UnionFindSimple.cpp ├── UnionFindSimple.h ├── camtest.cpp ├── gltest.cpp ├── maketags.cpp ├── quadtest.cpp └── tagtest.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | .DS_Store 3 | *~ 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # swatbotics/apriltags-cpp/CMakeLists.txt 2 | 3 | cmake_minimum_required(VERSION 2.6) 4 | 5 | project(APRILTAGS) 6 | 7 | set(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}) 8 | set(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}) 9 | 10 | if(APPLE) 11 | include_directories(/opt/local/include) # MacPorts 12 | link_directories(/opt/local/lib) 13 | find_library(OPENGL_LIBRARY OpenGL) 14 | else() 15 | find_library(OPENGL_LIBRARY GL) 16 | find_library(GLU_LIBRARY GLU) 17 | set(OPENGL_LIBRARY ${OPENGL_LIBRARY} ${GLU_LIBRARY}) 18 | endif() 19 | 20 | find_library(GLUT_LIBRARY glut) 21 | 22 | include(FindPkgConfig) 23 | 24 | pkg_search_module(OPENCV REQUIRED opencv>=2.3 opencv-2.3.1) 25 | include_directories(${OPENCV_INCLUDE_DIRS}) 26 | 27 | pkg_check_modules(CAIRO cairo) 28 | 29 | if (${CAIRO_FOUND}) 30 | add_definitions(-DMZ_HAVE_CAIRO) 31 | endif () 32 | 33 | find_package( CGAL QUIET COMPONENTS ) 34 | 35 | if (CGAL_FOUND) 36 | include( ${CGAL_USE_FILE} ) 37 | add_definitions(-DHAVE_CGAL) 38 | find_package( Boost REQUIRED ) 39 | else() 40 | message("CGAL not found, can not use new quad detector") 41 | endif() 42 | 43 | set(CMAKE_C_FLAGS "-Wall -g") 44 | set(CMAKE_CXX_FLAGS "-Wall -g") 45 | 46 | if (APPLE OR UNIX) 47 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wsign-compare -frounding-math") 48 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wsign-compare -frounding-math") 49 | endif() 50 | 51 | set(CMAKE_C_FLAGS_DEBUG "-O") 52 | set(CMAKE_CXX_FLAGS_DEBUG "-O") 53 | 54 | set(CMAKE_C_FLAGS_RELEASE "-O2") 55 | set(CMAKE_CXX_FLAGS_RELEASE "-O2") 56 | 57 | add_subdirectory(src) 58 | 59 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | apriltags-cpp 2 | ============= 3 | 4 | C++ port of the APRIL tags library, using OpenCV (and optionally, CGAL). 5 | 6 | Requirements 7 | ============ 8 | 9 | Currently, apriltags-cpp requires the following to build: 10 | 11 | * OpenCV >= 2.3 12 | * GLUT or freeglut (optional, used for test program) 13 | * Cairo (optional, used to print tags) 14 | * CGAL (optional, used for new quad detection algorithm) 15 | 16 | You must have cmake installed to build the software as well. 17 | 18 | Building 19 | ======== 20 | 21 | To compile the code, 22 | 23 | cd /path/to/apriltags-cpp 24 | mkdir build 25 | cd build 26 | cmake .. -DCMAKE_BUILD_TYPE=Release 27 | make 28 | 29 | Demo/utility programs 30 | ===================== 31 | 32 | The APRIL tags library is intended to be used as a library, from C++, 33 | but there are also five demo/utility programs included in this 34 | distribution: 35 | 36 | * `tagtest` - Demonstrate tag recognition and time profiling. 37 | 38 | * `camtest` - Demonstrate 3D tag locations using OpenCV to 39 | visualize, with an attached camera. 40 | 41 | * `gltest` - Demonstrate 3D tag locations using OpenGL to 42 | visualize, with an attached camera. 43 | 44 | * `quadtest` - Demonstrate/test tag position refinement using 45 | a template tracking approach. 46 | 47 | * `maketags` - Create PDF files for printing tags. 48 | 49 | There are some test images in the `images` directory, that may be 50 | useful to use with the `tagtest` program. 51 | -------------------------------------------------------------------------------- /images/clipboard/1-debug_0_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swatbotics/apriltags-cpp/34b0166e01f16a85f095c15ab54e94447f1fa003/images/clipboard/1-debug_0_0.png -------------------------------------------------------------------------------- /images/clipboard/10-debug_0_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swatbotics/apriltags-cpp/34b0166e01f16a85f095c15ab54e94447f1fa003/images/clipboard/10-debug_0_0.png -------------------------------------------------------------------------------- /images/clipboard/2-debug_0_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swatbotics/apriltags-cpp/34b0166e01f16a85f095c15ab54e94447f1fa003/images/clipboard/2-debug_0_0.png -------------------------------------------------------------------------------- /images/clipboard/3-debug_0_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swatbotics/apriltags-cpp/34b0166e01f16a85f095c15ab54e94447f1fa003/images/clipboard/3-debug_0_0.png -------------------------------------------------------------------------------- /images/clipboard/4-debug_0_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swatbotics/apriltags-cpp/34b0166e01f16a85f095c15ab54e94447f1fa003/images/clipboard/4-debug_0_0.png -------------------------------------------------------------------------------- /images/clipboard/5-debug_0_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swatbotics/apriltags-cpp/34b0166e01f16a85f095c15ab54e94447f1fa003/images/clipboard/5-debug_0_0.png -------------------------------------------------------------------------------- /images/clipboard/6-debug_0_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swatbotics/apriltags-cpp/34b0166e01f16a85f095c15ab54e94447f1fa003/images/clipboard/6-debug_0_0.png -------------------------------------------------------------------------------- /images/clipboard/7-debug_0_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swatbotics/apriltags-cpp/34b0166e01f16a85f095c15ab54e94447f1fa003/images/clipboard/7-debug_0_0.png -------------------------------------------------------------------------------- /images/clipboard/8-debug_0_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swatbotics/apriltags-cpp/34b0166e01f16a85f095c15ab54e94447f1fa003/images/clipboard/8-debug_0_0.png -------------------------------------------------------------------------------- /images/clipboard/9-debug_0_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swatbotics/apriltags-cpp/34b0166e01f16a85f095c15ab54e94447f1fa003/images/clipboard/9-debug_0_0.png -------------------------------------------------------------------------------- /images/iphonecam/IMG_0612.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swatbotics/apriltags-cpp/34b0166e01f16a85f095c15ab54e94447f1fa003/images/iphonecam/IMG_0612.jpg -------------------------------------------------------------------------------- /images/iphonecam/IMG_0613.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swatbotics/apriltags-cpp/34b0166e01f16a85f095c15ab54e94447f1fa003/images/iphonecam/IMG_0613.jpg -------------------------------------------------------------------------------- /images/iphonecam/IMG_0614.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swatbotics/apriltags-cpp/34b0166e01f16a85f095c15ab54e94447f1fa003/images/iphonecam/IMG_0614.jpg -------------------------------------------------------------------------------- /images/iphonecam/IMG_0615.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swatbotics/apriltags-cpp/34b0166e01f16a85f095c15ab54e94447f1fa003/images/iphonecam/IMG_0615.jpg -------------------------------------------------------------------------------- /images/iphonecam/IMG_0617.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swatbotics/apriltags-cpp/34b0166e01f16a85f095c15ab54e94447f1fa003/images/iphonecam/IMG_0617.jpg -------------------------------------------------------------------------------- /src/AprilTypes.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * This file is distributed as part of the C++ port of the APRIL tags 3 | * library. The code is licensed under GPLv2. 4 | * 5 | * Original author: Edwin Olson 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #ifndef _APRILTYPES_H_ 10 | #define _APRILTYPES_H_ 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | // Namespace to hold data type definitions for apriltags library. 17 | namespace at { 18 | 19 | typedef uint64_t code_t; 20 | typedef unsigned int uint; 21 | 22 | #if 1 // Change this to zero to make everything double-precision 23 | 24 | typedef float real; 25 | enum { REAL_IMAGE_TYPE = CV_32F }; 26 | #define AT_REAL_MAX FLT_MAX 27 | #define AT_EPSILON 0.0000001; 28 | 29 | #else 30 | 31 | typedef double real; 32 | enum { REAL_IMAGE_TYPE = CV_64F }; 33 | #define AT_REAL_MAX DBL_MAX 34 | #define AT_EPSILON 0.000000001 35 | 36 | #endif 37 | 38 | typedef cv::Point_ Point; 39 | typedef cv::Point3_ Point3; 40 | typedef cv::Vec Vec3; 41 | typedef cv::Mat_ Mat; 42 | 43 | }; 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(apriltags 2 | CameraUtil.cpp 3 | DebugImage.cpp 4 | Geometry.cpp 5 | GrayModel.cpp 6 | MathUtil.cpp 7 | Refine.cpp 8 | TagDetector.cpp 9 | TagFamily.cpp 10 | TagFamilies.cpp 11 | UnionFindSimple.cpp 12 | ) 13 | 14 | set(AT_LIBS apriltags ${OPENCV_LDFLAGS}) 15 | 16 | target_link_libraries(apriltags ${AT_LIBS}) 17 | 18 | add_executable(camtest camtest.cpp) 19 | target_link_libraries(camtest ${AT_LIBS}) 20 | 21 | add_executable(tagtest tagtest.cpp) 22 | target_link_libraries(tagtest ${AT_LIBS}) 23 | 24 | add_executable(quadtest quadtest.cpp) 25 | target_link_libraries(quadtest ${AT_LIBS}) 26 | 27 | if (GLUT_LIBRARY) 28 | 29 | add_executable(gltest gltest.cpp) 30 | target_link_libraries(gltest ${GLUT_LIBRARY} ${OPENGL_LIBRARY} ${AT_LIBS}) 31 | 32 | endif() 33 | 34 | if (CAIRO_FOUND) 35 | 36 | add_executable(maketags maketags.cpp) 37 | target_link_libraries(maketags ${CAIRO_LIBRARIES} ${AT_LIBS} ${CAIRO_LIBS}) 38 | 39 | endif() 40 | -------------------------------------------------------------------------------- /src/CameraUtil.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * This file is distributed as part of the C++ port of the APRIL tags 3 | * library. The code is licensed under GPLv2. 4 | * 5 | * Original author: Edwin Olson 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #include "CameraUtil.h" 10 | #include 11 | 12 | namespace CameraUtil { 13 | 14 | static at::real sq(at::real x) { return x*x; } 15 | 16 | /** Given a 3x3 homography matrix and the focal lengths of the 17 | * camera, compute the pose of the tag. The focal lengths should 18 | * be given in pixels. For example, if the camera's focal length 19 | * is twice the width of the sensor, and the sensor is 600 pixels 20 | * across, the focal length in pixels is 2*600. Note that the 21 | * focal lengths in the fx and fy direction will be approximately 22 | * equal for most lenses, and is not a function of aspect ratio. 23 | * 24 | * Theory: The homography matrix is the product of the camera 25 | * projection matrix and the tag's pose matrix (the matrix that 26 | * projects points from the tag's local coordinate system to the 27 | * camera's coordinate frame). 28 | * 29 | * [ h00 h01 h02 h03] = [ 1/fx 0 0 0 ] [ R00 R01 R02 TX ] 30 | * [ h10 h11 h12 h13] = [ 0 1/fy 0 0 ] [ R10 R11 R12 TY ] 31 | * [ h20 h21 h22 h23] = [ 0 0 1 0 ] [ R20 R21 R22 TZ ] 32 | * [ 0 0 0 1 ] 33 | * 34 | * When observing a tag, the points we project in world space all 35 | * have z=0, so we can form a 3x3 matrix by eliminating the 3rd 36 | * column of the pose matrix. 37 | * 38 | * [ h00 h01 h02 ] = [ 1/fx 0 0 0 ] [ R00 R01 TX ] 39 | * [ h10 h11 h12 ] = [ 0 1/fy 0 0 ] [ R10 R11 TY ] 40 | * [ h20 h21 h22 ] = [ 0 0 1 0 ] [ R20 R21 TZ ] 41 | * [ 0 0 1 ] 42 | * 43 | * (note that these h's are different from the ones above.) 44 | * 45 | * We can multiply the right-hand side to yield a set of equations 46 | * relating the values of h to the values of the pose matrix. 47 | * 48 | * There are two wrinkles. The first is that the homography matrix 49 | * is known only up to scale. We recover the unknown scale by 50 | * constraining the magnitude of the first two columns of the pose 51 | * matrix to be 1. We use the geometric average scale. The sign of 52 | * the scale factor is recovered by constraining the observed tag 53 | * to be in front of the camera. Once scaled, we recover the first 54 | * two colmuns of the rotation matrix. The third column is the 55 | * cross product of these. 56 | * 57 | * The second wrinkle is that the computed rotation matrix might 58 | * not be exactly orthogonal, so we perform a polar decomposition 59 | * to find a good pure rotation approximation. 60 | * 61 | * Tagsize is the size of the tag in your desired units. I.e., if 62 | * your tag measures 0.25m along the side, your tag size is 63 | * 0.25. (The homography is computed in terms of *half* the tag 64 | * size, i.e., that a tag is 2 units wide as it spans from -1 to 65 | * +1, but this code makes the appropriate adjustment.) 66 | **/ 67 | 68 | at::Mat homographyToPose(at::real fx, at::real fy, 69 | at::real tagSize, 70 | const at::Mat& horig, 71 | bool openGLStyle) { 72 | 73 | // flip the homography along the Y axis to align the 74 | // conventional image coordinate system (y=0 at the top) with 75 | // the conventional camera coordinate system (y=0 at the 76 | // bottom). 77 | 78 | at::Mat h = horig; 79 | 80 | at::Mat F = at::Mat::eye(3,3); 81 | F(1,1) = F(2,2) = -1; 82 | h = F * horig; 83 | 84 | at::Mat M(4,4); 85 | M(0,0) = h(0,0) / fx; 86 | M(0,1) = h(0,1) / fx; 87 | M(0,3) = h(0,2) / fx; 88 | M(1,0) = h(1,0) / fy; 89 | M(1,1) = h(1,1) / fy; 90 | M(1,3) = h(1,2) / fy; 91 | M(2,0) = h(2,0); 92 | M(2,1) = h(2,1); 93 | M(2,3) = h(2,2); 94 | 95 | // Compute the scale. The columns of M should be made to be 96 | // unit vectors. This is over-determined, so we take the 97 | // geometric average. 98 | at::real scale0 = sqrt(sq(M(0,0)) + sq(M(1,0)) + sq(M(2,0))); 99 | at::real scale1 = sqrt(sq(M(0,1)) + sq(M(1,1)) + sq(M(2,1))); 100 | at::real scale = sqrt(scale0*scale1); 101 | 102 | M *= 1.0/scale; 103 | 104 | // recover sign of scale factor by noting that observations must 105 | // occur in front of the camera. 106 | if (M(2,3) > 0) { 107 | M *= -1; 108 | } 109 | 110 | // The bottom row should always be [0 0 0 1]. 111 | M(3,0) = 0; 112 | M(3,1) = 0; 113 | M(3,2) = 0; 114 | M(3,3) = 1; 115 | 116 | // recover third rotation vector by crossproduct of the other two 117 | // rotation vectors 118 | at::Vec3 a( M(0,0), M(1,0), M(2,0) ); 119 | at::Vec3 b( M(0,1), M(1,1), M(2,1) ); 120 | at::Vec3 ab = a.cross(b); 121 | 122 | M(0,2) = ab[0]; 123 | M(1,2) = ab[1]; 124 | M(2,2) = ab[2]; 125 | 126 | // pull out just the rotation component so we can normalize it. 127 | 128 | at::Mat R(3,3); 129 | for (int i=0; i<3; ++i) { 130 | for (int j=0; j<3; ++j) { 131 | R(i,j) = M(i,j); 132 | } 133 | } 134 | 135 | // polar decomposition, R = (UV')(VSV') 136 | 137 | cv::SVD svd(R); 138 | at::Mat MR = svd.u * svd.vt; 139 | 140 | if (!openGLStyle) { MR = F * MR; } 141 | 142 | for (int i=0; i<3; ++i) { 143 | for (int j=0; j<3; ++j) { 144 | M(i,j) = MR(i,j); 145 | } 146 | } 147 | 148 | // Scale the results based on the scale in the homography. The 149 | // homography assumes that tags span from -1 to +1, i.e., that 150 | // they are two units wide (and tall). 151 | for (int i = 0; i < 3; i++) { 152 | at::real scl = openGLStyle ? 1 : F(i,i); 153 | M(i,3) *= scl * tagSize / 2; 154 | } 155 | 156 | return M; 157 | 158 | 159 | } 160 | 161 | at::Point project(const at::Mat& H, 162 | at::real x, at::real y) { 163 | 164 | at::real z = H[2][0]*x + H[2][1]*y + H[2][2]; 165 | 166 | return at::Point( (H[0][0]*x + H[0][1]*y + H[0][2])/z, 167 | (H[1][0]*x + H[1][1]*y + H[1][2])/z ); 168 | 169 | 170 | } 171 | 172 | void homographyToPoseCV(at::real fx, at::real fy, 173 | at::real tagSize, 174 | const at::Mat& horig, 175 | cv::Mat& rvec, 176 | cv::Mat& tvec) { 177 | 178 | at::Point3 opoints[4] = { 179 | at::Point3(-1, -1, 0), 180 | at::Point3( 1, -1, 0), 181 | at::Point3( 1, 1, 0), 182 | at::Point3(-1, 1, 0) 183 | }; 184 | 185 | at::Point ipoints[4]; 186 | 187 | at::real s = 0.5*tagSize; 188 | 189 | for (int i=0; i<4; ++i) { 190 | ipoints[i] = project(horig, opoints[i].x, opoints[i].y); 191 | opoints[i] *= s; 192 | } 193 | 194 | at::real Kdata[9] = { 195 | fx, 0, 0, 196 | 0, fy, 0, 197 | 0, 0, 1 198 | }; 199 | 200 | at::Mat Kmat(3, 3, Kdata); 201 | 202 | at::Mat dcoeffs = at::Mat::zeros(4, 1); 203 | 204 | cv::Mat_ omat(4, 1, opoints); 205 | cv::Mat_ imat(4, 1, ipoints); 206 | 207 | cv::Mat r, t; 208 | 209 | cv::solvePnP(omat, imat, Kmat, dcoeffs, r, t); 210 | 211 | if (rvec.type() == CV_32F) { 212 | r.convertTo(rvec, rvec.type()); 213 | } else { 214 | rvec = r; 215 | } 216 | 217 | if (tvec.type() == CV_32F) { 218 | t.convertTo(tvec, tvec.type()); 219 | } else { 220 | tvec = t; 221 | } 222 | 223 | } 224 | 225 | 226 | } 227 | 228 | 229 | -------------------------------------------------------------------------------- /src/CameraUtil.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * This file is distributed as part of the C++ port of the APRIL tags 3 | * library. The code is licensed under GPLv2. 4 | * 5 | * Original author: Edwin Olson 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #ifndef _CAMERAUTIL_H_ 10 | #define _CAMERAUTIL_H_ 11 | 12 | #include "AprilTypes.h" 13 | 14 | namespace CameraUtil { 15 | 16 | /** Convert a 3x3 homography matrix to a 4x4 rigid body 17 | * transformation, given focal distances in X/Y, and tag size in m. 18 | * This function uses the original APRIL tags algorithm, but the 19 | * OpenCV-based algorithm may be more accurate. 20 | */ 21 | at::Mat homographyToPose(at::real fx, at::real fy, 22 | at::real tagSize, 23 | const at::Mat& horig, 24 | bool openGLStyle=false); 25 | 26 | /** Convert a 3x3 homography matrix to a 4x4 rigid body 27 | * transformation, given focal distances in X/Y, and tag size in m. 28 | * This function calls the OpenCV solvePnP function under the hood 29 | * to compute a rotation vector and translation vector. 30 | */ 31 | void homographyToPoseCV(at::real fx, at::real fy, 32 | at::real tagSize, 33 | const at::Mat& horig, 34 | cv::Mat& rvec, 35 | cv::Mat& tvec); 36 | 37 | }; 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /src/DebugImage.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * This file is distributed as part of the C++ port of the APRIL tags 3 | * library. The code is licensed under GPLv2. 4 | * 5 | * Original author: Edwin Olson 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #include "DebugImage.h" 10 | #include "AprilTypes.h" 11 | 12 | #include 13 | #include 14 | 15 | cv::Mat rescaleImageIntensity(const cv::Mat& img, ScaleType type) { 16 | 17 | cv::Mat rval; 18 | 19 | if (type == ScaleNone) { 20 | 21 | if (img.depth() == CV_8U) { 22 | rval = img.clone(); 23 | } else { 24 | double fmin, fmax; 25 | cv::minMaxLoc(img, &fmin, &fmax); 26 | if (fmax - fmin <= 1) { 27 | rval = cv::Mat(img.rows, img.cols, CV_8U); 28 | cv::convertScaleAbs(img, rval, 255); 29 | } else { 30 | img.convertTo(rval, CV_8U); 31 | } 32 | } 33 | 34 | } else { 35 | 36 | cv::Mat fsrc; 37 | 38 | if (img.depth() != at::REAL_IMAGE_TYPE) { 39 | img.convertTo(fsrc, at::REAL_IMAGE_TYPE); 40 | } else { 41 | fsrc = img; 42 | } 43 | 44 | cv::Mat tmp; 45 | 46 | if (type == ScaleMinMax) { 47 | double fmin, fmax; 48 | cv::minMaxLoc(fsrc, &fmin, &fmax); 49 | tmp = 255*((fsrc-fmin)/(fmax-fmin)); 50 | } else { 51 | at::real fmag = cv::norm(fsrc, cv::NORM_INF); 52 | tmp = 127 + 127*fsrc/fmag; 53 | } 54 | 55 | tmp.convertTo(rval, CV_8U); 56 | 57 | } 58 | 59 | return rval; 60 | 61 | } 62 | 63 | void labelImage(cv::Mat& img, const std::string& text) { 64 | 65 | cv::putText(img, text, cv::Point(4, 20), 66 | cv::FONT_HERSHEY_SIMPLEX, 67 | 0.6, CV_RGB(0,0,0), 3, CV_AA); 68 | 69 | cv::putText(img, text, cv::Point(4, 20), 70 | cv::FONT_HERSHEY_SIMPLEX, 71 | 0.6, CV_RGB(255,255,255), 1, CV_AA); 72 | 73 | } 74 | 75 | int resizeToDisplay(const cv::Mat& src, cv::Mat& dst, 76 | int width, int height) { 77 | 78 | int fw = width / src.cols; 79 | int fh = height / src.rows; 80 | 81 | int f = std::min(fw,fh); 82 | 83 | if (f > 1) { 84 | 85 | cv::Mat tmp; 86 | cv::resize(src, tmp, cv::Size(0,0), f, f, CV_INTER_NN); 87 | dst = tmp; 88 | return f; 89 | 90 | } else { 91 | 92 | dst = src.clone(); 93 | return 1; 94 | 95 | } 96 | 97 | 98 | } 99 | 100 | void labelAndWaitForKey(const std::string& window, 101 | const std::string& text, 102 | const cv::Mat& img, 103 | ScaleType type, 104 | bool resize, 105 | int w, int h) { 106 | 107 | cv::Mat rescaled = rescaleImageIntensity(img, type); 108 | 109 | if (resize) { resizeToDisplay(rescaled, rescaled, w, h); } 110 | 111 | labelImage(rescaled, text); 112 | 113 | cv::imshow(window, rescaled); 114 | cv::waitKey(); 115 | 116 | 117 | } 118 | -------------------------------------------------------------------------------- /src/DebugImage.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * This file is distributed as part of the C++ port of the APRIL tags 3 | * library. The code is licensed under GPLv2. 4 | * 5 | * Original author: Edwin Olson 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #ifndef _IMAGEUTIL_H_ 10 | #define _IMAGEUTIL_H_ 11 | 12 | #include 13 | 14 | enum ScaleType { 15 | ScaleNone, 16 | ScaleMinMax, 17 | ScaleAbs 18 | }; 19 | 20 | 21 | /** Rescales the image to normalize the brightness. */ 22 | cv::Mat rescaleImageIntensity(const cv::Mat& img, ScaleType type); 23 | 24 | enum { 25 | DEFAULT_SCREEN_W = 1280, 26 | DEFAULT_SCREEN_H = 760 27 | }; 28 | 29 | /** Multiplies the image by the largest integer scale factor that will 30 | * make it no bigger than the screen dimensions given, and returns 31 | * the scale factor. 32 | */ 33 | int resizeToDisplay(const cv::Mat& src, cv::Mat& dst, 34 | int w=DEFAULT_SCREEN_W, int h=DEFAULT_SCREEN_H); 35 | 36 | /** Places a black and white label on an image. */ 37 | void labelImage(cv::Mat& img, const std::string& text); 38 | 39 | /** Convenience function to call the above three functions and place 40 | * the result on the screen. */ 41 | void labelAndWaitForKey(const std::string& window, 42 | const std::string& text, 43 | const cv::Mat& img, 44 | ScaleType type, 45 | bool resize, 46 | int w=DEFAULT_SCREEN_W, 47 | int h=DEFAULT_SCREEN_H); 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /src/Geometry.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * This file is distributed as part of the C++ port of the APRIL tags 3 | * library. The code is licensed under GPLv2. 4 | * 5 | * Original author: Edwin Olson 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #include "Geometry.h" 10 | #include "MathUtil.h" 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | XYW::XYW() {} 17 | XYW::XYW(at::real xx, at::real yy, at::real ww): x(xx), y(yy), w(ww) {} 18 | at::Point XYW::point() const { return at::Point(x,y); } 19 | 20 | Gridder::Gridder(at::real x0, at::real y0, at::real x1, at::real y1, at::real metersPerCell) { 21 | 22 | this->x0 = x0; 23 | this->y0 = y0; 24 | this->metersPerCell = metersPerCell; 25 | 26 | width = (int) ((x1 - x0)/metersPerCell + 1); 27 | height = (int) ((y1 - y0)/metersPerCell + 1); 28 | 29 | this->x1 = x0 + metersPerCell*width; 30 | this->y1 = y0 + metersPerCell*height; 31 | 32 | cells.clear(); 33 | cells.resize(width*height); 34 | 35 | } 36 | 37 | int Gridder::sub2ind(int x, int y) const { 38 | return y*width + x; 39 | } 40 | 41 | void Gridder::add(at::real x, at::real y, Segment* s) { 42 | 43 | int ix = (int) ((x - x0)/metersPerCell); 44 | int iy = (int) ((y - y0)/metersPerCell); 45 | 46 | if (ix >=0 && iy >=0 && ix < width && iy < height) { 47 | size_t idx = sub2ind(ix,iy); 48 | s->nextGrid = cells[idx]; 49 | cells[idx] = s; 50 | } 51 | 52 | } 53 | 54 | void Gridder::find(at::real x, at::real y, at::real range, SegmentArray& results) const { 55 | 56 | results.clear(); 57 | 58 | int ix0 = (int) ((x - range - x0)/metersPerCell); 59 | int iy0 = (int) ((y - range - y0)/metersPerCell); 60 | 61 | int ix1 = (int) ((x + range - x0)/metersPerCell); 62 | int iy1 = (int) ((y + range - y0)/metersPerCell); 63 | 64 | for (int iy=iy0; iy<=iy1; ++iy) { 65 | for (int ix=ix0; ix<=ix1; ++ix) { 66 | 67 | if (ix >=0 && iy >=0 && ix < width && iy < height) { 68 | 69 | for (Segment* s = cells[sub2ind(ix,iy)]; s; s = s->nextGrid) { 70 | results.push_back(s); 71 | } 72 | 73 | } 74 | 75 | } 76 | } 77 | 78 | } 79 | 80 | bool intersect(const GLineSegment2D& g1, 81 | const GLineSegment2D& g2, 82 | at::Point& pinter) { 83 | 84 | Segment s1, s2; 85 | 86 | s1.x0 = g1.p1.x; 87 | s1.x1 = g1.p2.x; 88 | 89 | s1.y0 = g1.p1.y; 90 | s1.y1 = g1.p2.y; 91 | 92 | s2.x0 = g2.p1.x; 93 | s2.x1 = g2.p2.x; 94 | 95 | s2.y0 = g2.p1.y; 96 | s2.y1 = g2.p2.y; 97 | 98 | return intersect(&s1, &s2, pinter); 99 | 100 | } 101 | 102 | bool intersect(const Segment* s1, 103 | const Segment* s2, 104 | at::Point& pinter) { 105 | 106 | at::real m00 = s1->x1-s1->x0; 107 | at::real m01 = s2->x0-s2->x1; 108 | 109 | at::real m10 = s1->y1-s1->y0; 110 | at::real m11 = s2->y0-s2->y1; 111 | 112 | at::real det = m00*m11 - m01*m10; 113 | 114 | if (MathUtil::fabs(det) < 0.0000000001) { 115 | return false; 116 | } 117 | 118 | at::real i00 = m11/det; 119 | //at::real i11 = m00/det; 120 | at::real i01 = -m01/det; 121 | //at::real i10 = -m10/det; 122 | 123 | at::real b00 = s2->x0 - s1->x0; 124 | at::real b10 = s2->y0 - s1->y0; 125 | 126 | at::real x00 = i00*b00 + i01*b10; 127 | pinter.x = m00*x00 + s1->x0; 128 | pinter.y = m10*x00 + s1->y0; 129 | 130 | return true; 131 | 132 | } 133 | 134 | at::real pdist(const at::Point& p1, const at::Point& p2) { 135 | at::Point pd = p2-p1; 136 | return sqrt(pd.dot(pd)); 137 | } 138 | 139 | at::real pdist(const at::Point& p, int x, int y) { 140 | return pdist(p, at::Point(x,y)); 141 | } 142 | 143 | GLineSegment2D::GLineSegment2D() {} 144 | 145 | GLineSegment2D::GLineSegment2D(const at::Point& pp1, const at::Point& pp2): 146 | p1(pp1), p2(pp2) {} 147 | 148 | at::real GLineSegment2D::length() const { 149 | return pdist(p1, p2); 150 | } 151 | 152 | static at::real square(at::real x) { return x*x; } 153 | 154 | GLineSegment2D lsqFitXYW(const XYWArray& points) { 155 | 156 | at::real Cxx=0, Cyy=0, Cxy=0, Ex=0, Ey=0, mXX=0, mYY=0, mXY=0, mX=0, mY=0; 157 | at::real n=0; 158 | 159 | int idx = 0; 160 | for (size_t i=0; i srcmat(4, 1, src); 293 | cv::Mat_ dstmat(4, 1, dst); 294 | 295 | H = cv::findHomography(srcmat, dstmat); 296 | 297 | } 298 | 299 | at::Point Quad::interpolate(const at::Point& p) const { 300 | 301 | return interpolate(p.x, p.y); 302 | 303 | } 304 | 305 | at::Point Quad::interpolate(at::real x, at::real y) const { 306 | 307 | at::real z = H[2][0]*x + H[2][1]*y + H[2][2]; 308 | 309 | return at::Point( (H[0][0]*x + H[0][1]*y + H[0][2])/z + opticalCenter.x, 310 | (H[1][0]*x + H[1][1]*y + H[1][2])/z + opticalCenter.y ); 311 | 312 | } 313 | 314 | at::Point Quad::interpolate01(const at::Point& p) const { 315 | return interpolate01(p.x, p.y); 316 | } 317 | 318 | at::Point Quad::interpolate01(at::real x, at::real y) const { 319 | return interpolate(2*x-1, 2*y-1); 320 | } 321 | 322 | -------------------------------------------------------------------------------- /src/Geometry.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * This file is distributed as part of the C++ port of the APRIL tags 3 | * library. The code is licensed under GPLv2. 4 | * 5 | * Original author: Edwin Olson 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #ifndef _GEOMETRY_H_ 10 | #define _GEOMETRY_H_ 11 | 12 | // Note: Geometry.h is a bit of a misnomer, as this holds a bunch of 13 | // classes all used by the TagDetector class. 14 | 15 | #include "AprilTypes.h" 16 | #include 17 | ////////////////////////////////////////////////////////////////////// 18 | // Struct for a line segment, which holds start/end points, length, 19 | // angle. A segment may also have a number of children, as well as a 20 | // sibling in the grid structure (see Gridder below). Used by 21 | // TagDetector to find quads. 22 | 23 | struct Segment; 24 | typedef std::vector SegmentArray; 25 | 26 | struct Segment { 27 | at::real x0, y0, x1, y1, length, theta; 28 | SegmentArray children; 29 | Segment* nextGrid; 30 | }; 31 | 32 | bool intersect(const Segment* s1, const Segment* s2, at::Point& pinter); 33 | 34 | ////////////////////////////////////////////////////////////////////// 35 | // Class to grid up a bunch of line segments to support region-based 36 | // search of line segments. Used by TagDetector to find quads. 37 | 38 | class Gridder { 39 | public: 40 | 41 | SegmentArray cells; 42 | at::real x0, y0, x1, y1; 43 | int width, height; 44 | at::real metersPerCell; 45 | 46 | Gridder(at::real x0, at::real y0, 47 | at::real x1, at::real y1, 48 | at::real metersPerCell); 49 | 50 | int sub2ind(int x, int y) const; 51 | 52 | void add(at::real x, at::real y, Segment* s); 53 | 54 | void find(at::real x, at::real y, at::real range, 55 | SegmentArray& results) const; 56 | 57 | }; 58 | 59 | ////////////////////////////////////////////////////////////////////// 60 | // Convenience functions to use Green's theorem to compute the area of 61 | // a polygonal region using either floating-point or integer 62 | // coordinates. 63 | 64 | at::real area(const at::Point* p, size_t n); 65 | at::real area(const cv::Point* p, size_t n); 66 | 67 | ////////////////////////////////////////////////////////////////////// 68 | // Class to represent a quadrilateral inside an image, along with a 69 | // homography mapping to/from. 70 | 71 | class Quad { 72 | public: 73 | 74 | at::Point p[4]; 75 | 76 | at::Point opticalCenter; 77 | at::Mat H; 78 | 79 | at::real observedPerimeter; 80 | 81 | at::real area() const; 82 | 83 | Quad(); 84 | 85 | Quad(const at::Point p[4], 86 | const at::Point& opticalCenter, 87 | at::real observedPerimeter); 88 | 89 | void recomputeHomography(); 90 | 91 | at::Point interpolate(const at::Point& p) const; 92 | at::Point interpolate(at::real x, at::real y) const; 93 | 94 | 95 | at::Point interpolate01(const at::Point& p) const; 96 | at::Point interpolate01(at::real x, at::real y) const; 97 | 98 | }; 99 | 100 | typedef std::vector QuadArray; 101 | 102 | ////////////////////////////////////////////////////////////////////// 103 | // Utility functions to compute distances between points 104 | 105 | at::real pdist(const at::Point& p1, const at::Point& p2); 106 | at::real pdist(const at::Point& p, int x, int y); 107 | 108 | ////////////////////////////////////////////////////////////////////// 109 | // Lightweight line segment class, used by TagDetector to find quad 110 | // edges after quad detection. 111 | 112 | class GLineSegment2D { 113 | public: 114 | 115 | at::Point p1, p2; 116 | 117 | GLineSegment2D(); 118 | 119 | GLineSegment2D(const at::Point& p1, const at::Point& p2); 120 | 121 | at::real length() const; 122 | 123 | }; 124 | 125 | bool intersect(const GLineSegment2D& s1, 126 | const GLineSegment2D& s2, 127 | at::Point& pinter); 128 | 129 | ////////////////////////////////////////////////////////////////////// 130 | // Class for a weighted (x,y) point, used for line segment fitting by 131 | // TagDetector. 132 | 133 | class XYW { 134 | public: 135 | at::real x, y, w; 136 | XYW(); 137 | XYW(at::real x, at::real y, at::real w); 138 | at::Point point() const; 139 | }; 140 | 141 | typedef std::vector XYWArray; 142 | typedef std::map< int, XYWArray > ClusterLookup; 143 | 144 | GLineSegment2D lsqFitXYW(const XYWArray& points); 145 | 146 | #endif 147 | -------------------------------------------------------------------------------- /src/GrayModel.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * This file is distributed as part of the C++ port of the APRIL tags 3 | * library. The code is licensed under GPLv2. 4 | * 5 | * Original author: Edwin Olson 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #include "GrayModel.h" 10 | #include 11 | 12 | GrayModel::GrayModel(): nobs(0), have_solution(false) { 13 | 14 | memset(A, 0, sizeof(A)); 15 | memset(b, 0, sizeof(b)); 16 | memset(X, 0, sizeof(X)); 17 | 18 | } 19 | 20 | 21 | void GrayModel::addObservation(at::real x, at::real y, at::real gray) { 22 | 23 | at::real xy = x*y; 24 | 25 | // update only upper-right elements. A'A is symmetric, 26 | // we'll fill the other elements in later. 27 | A[0][0] += x*x; 28 | A[0][1] += x*y; 29 | A[0][2] += x*xy; 30 | A[0][3] += x; 31 | A[1][1] += y*y; 32 | A[1][2] += y*xy; 33 | A[1][3] += y; 34 | A[2][2] += xy*xy; 35 | A[2][3] += xy; 36 | A[3][3] += 1; 37 | 38 | b[0] += x*gray; 39 | b[1] += y*gray; 40 | b[2] += xy*gray; 41 | b[3] += gray; 42 | 43 | nobs++; 44 | have_solution = false; 45 | 46 | } 47 | 48 | int GrayModel::getNumObservations() { 49 | return nobs; 50 | } 51 | 52 | void GrayModel::compute() { 53 | 54 | if (have_solution) { return; } 55 | 56 | if (nobs >= 6) { 57 | // we really only need 4 linearly independent 58 | // observations to fit our answer, but we'll be very 59 | // sensitive to noise if we don't have an 60 | // over-determined system. Thus, require at least 6 61 | // observations (or we'll use a constant model below). 62 | 63 | // make symmetric 64 | for (int i = 0; i < 4; i++) 65 | for (int j = i+1; j < 4; j++) 66 | A[j][i] = A[i][j]; 67 | 68 | at::Mat Amat(4, 4, &(A[0][0])); 69 | at::Mat bvec(4, 1, b); 70 | at::Mat xvec(4, 1, X); 71 | 72 | have_solution = cv::solve(Amat, bvec, xvec); 73 | 74 | } 75 | 76 | if (!have_solution) { 77 | // not enough samples to fit a good model. Use a flat model. 78 | X[0] = X[1] = X[2] = 0; 79 | X[3] = b[3] / nobs; 80 | } 81 | 82 | } 83 | 84 | at::real GrayModel::interpolate(at::real x, at::real y) { 85 | compute(); 86 | return X[0]*x + X[1]*y + X[2]*x*y + X[3]; 87 | } 88 | 89 | 90 | /* 91 | void testGrayModel() { 92 | 93 | at::real cx = 5; 94 | at::real cy = -3; 95 | at::real cxy = -0.1; 96 | at::real cb = 0.5; 97 | 98 | GrayModel gmodel; 99 | 100 | for (int y=-5; y<=5; ++y) { 101 | for (int x=-5; x<=5; ++x) { 102 | at::real g = cx*x + cy*y + cxy*x*y + cb; 103 | gmodel.addObservation(x, y, g); 104 | } 105 | } 106 | 107 | gmodel.compute(); 108 | 109 | for (int i=0; i<4; ++i) { 110 | std::cout << "gmodel.X[" << i << "] = " << gmodel.X[i] << "\n"; 111 | } 112 | 113 | } 114 | */ 115 | -------------------------------------------------------------------------------- /src/GrayModel.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * This file is distributed as part of the C++ port of the APRIL tags 3 | * library. The code is licensed under GPLv2. 4 | * 5 | * Original author: Edwin Olson 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #ifndef _GRAYMODEL_H_ 10 | #define _GRAYMODEL_H_ 11 | 12 | #include "AprilTypes.h" 13 | 14 | /** Fits a grayscale model over an area of the form: 15 | * 16 | * Ax + By + Cxy + D = value 17 | * 18 | * We use this model to compute spatially-varying thresholds for 19 | * reading bits. */ 20 | 21 | class GrayModel { 22 | public: 23 | 24 | // we're solving Ax = b. For each observation, we add a row to 25 | // A of the form [x y xy 1] and to be of the form [gray]. x is 26 | // the vector [A B C D]. 27 | // 28 | // The least-squares solution to the system is x = inv(A'A)A'b 29 | 30 | at::real A[4][4]; 31 | at::real b[4]; 32 | at::real X[4]; 33 | 34 | int nobs; // how many observations? 35 | bool have_solution; 36 | 37 | GrayModel(); 38 | 39 | void addObservation(at::real x, at::real y, at::real gray); 40 | 41 | int getNumObservations(); 42 | 43 | void compute(); 44 | 45 | at::real interpolate(at::real x, at::real y); 46 | 47 | }; 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /src/MathUtil.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * This file is distributed as part of the C++ port of the APRIL tags 3 | * library. The code is licensed under GPLv2. 4 | * 5 | * Original author: Edwin Olson 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #include "MathUtil.h" 10 | #include 11 | 12 | const at::real MathUtil::epsilon = AT_EPSILON; 13 | const at::real MathUtil::twopi_inv = 0.5/M_PI; 14 | const at::real MathUtil::twopi = 2.0*M_PI; 15 | 16 | at::real MathUtil::fabs(at::real f) { 17 | return std::fabs(f); 18 | } 19 | 20 | at::real MathUtil::mod2pi_pos(at::real vin) { 21 | 22 | at::real q = vin * twopi_inv + at::real(0.5); 23 | int qi = (int) q; 24 | 25 | return vin - qi*twopi; 26 | 27 | } 28 | 29 | at::real MathUtil::mod2pi(at::real vin) { 30 | 31 | at::real v; 32 | 33 | if (vin < 0) { 34 | v = -mod2pi_pos(-vin); 35 | } else { 36 | v = mod2pi_pos(vin); 37 | } 38 | 39 | // Validation test: 40 | // if (v < -Math.PI || v > Math.PI) 41 | // System.out.printf("%10.3f -> %10.3f\n", vin, v); 42 | 43 | return v; 44 | } 45 | 46 | at::real MathUtil::mod2pi(at::real ref, at::real v) { 47 | return ref + mod2pi(v-ref); 48 | } 49 | 50 | // /** Returns a value of v wrapped such that ref and v differ by no 51 | // * more +/-PI 52 | // **/ 53 | // static public at::real mod2pi(at::real ref, at::real v) 54 | // { 55 | // return ref + mod2pi(v-ref); 56 | // } 57 | 58 | // /** Returns true if the two at::reals are within a small epsilon of 59 | // * each other. 60 | // **/ 61 | // static public boolean at::realEquals(at::real a, at::real b) 62 | // { 63 | // return Math.abs(a-b)max) 71 | // v=max; 72 | // return v; 73 | // } 74 | 75 | // static public at::real clamp(at::real v, at::real min, at::real max) 76 | // { 77 | // if (vmax) 80 | // v=max; 81 | // return v; 82 | // } 83 | 84 | // public static final at::real square(at::real x) 85 | // { 86 | // return x*x; 87 | // } 88 | 89 | // public static final int sign(at::real v) 90 | // { 91 | // if (v >= 0) 92 | // return 1; 93 | // return -1; 94 | // } 95 | 96 | // /** Quickly compute e^x for all x. 97 | 98 | // Accuracy for x>0: 99 | // x<0.5, absolute error < .0099 100 | // x>0.5, relative error 0.36% 101 | 102 | // For x<0, we internally compute the reciprocal form; error is 103 | // magnified. 104 | 105 | // This approximation is also monotonic. 106 | 107 | // **/ 108 | // public static final at::real exp(at::real xin) 109 | // { 110 | // if (xin>=0) 111 | // return exp_pos(xin); 112 | 113 | // return 1/(exp_pos(-xin)); 114 | // } 115 | 116 | // /** Quickly compute e^x for positive x. 117 | // **/ 118 | // protected static final at::real exp_pos(at::real xin) 119 | // { 120 | // // our algorithm: compute 2^(x/log(2)) by breaking exponent 121 | // // into integer and fractional parts. The integer part can be 122 | // // done with a bit shift operation. The fractional part, which 123 | // // has bounded magnitude, can be computed with a polynomial 124 | // // approximation. We then multiply together the two parts. 125 | 126 | // // prevent deep recursion that would just return INF anyway... 127 | // // e^709 > At::Real.MAX_VALUE; 128 | // if (xin>709) 129 | // return At::Real.MAX_VALUE; 130 | 131 | // if (xin>43) // recursively handle values which would otherwise blow up. 132 | // { 133 | // // the value 43 was determined emperically 134 | // return 4727839468229346561.4744575*exp_pos(xin-43); 135 | // } 136 | 137 | // at::real x = 1.44269504088896*xin; // now we compute 2^x 138 | // int wx = (int) x; // integer part 139 | // at::real rx = x-wx; // fractional part 140 | 141 | // rx*=0.69314718055995; // scale fractional part by log(2) 142 | 143 | // at::real b = 1L<=0) { 160 | if (x>=0) { 161 | return atn; 162 | } 163 | return M_PI+atn; 164 | } 165 | if (x>=0) { 166 | return atn; 167 | } 168 | return -at::real(M_PI)+atn; 169 | 170 | } 171 | 172 | /** returns [-PI/2, PI/2] 173 | accurate within 0.014 degrees 174 | **/ 175 | at::real MathUtil::atan(at::real x) { 176 | 177 | if (fabs(x) <= 1) { 178 | return atan_mag1(x); 179 | } 180 | if (x < 0) { 181 | return -M_PI/2-atan_mag1(1/x); 182 | } else { 183 | return M_PI/2-atan_mag1(1/x); 184 | } 185 | 186 | } 187 | 188 | // returns reasonable answers for |x|<=1. 189 | at::real MathUtil::atan_mag1(at::real x) { 190 | 191 | // accuracy = 0.26814 degrees 192 | // return x/(1+0.28087207802773*x*x); 193 | 194 | if (true) { 195 | 196 | static const at::real p0 = -0.000158023363661; 197 | static const at::real p1 = 1.003839939589617; 198 | static const at::real p2 = -0.016224975245612; 199 | static const at::real p3 = -0.343317496147292; 200 | static const at::real p4 = 0.141501628812858; 201 | 202 | at::real a = fabs(x); 203 | at::real a2 = a*a; 204 | 205 | at::real y = p0 + p1*a + p2*a2 + p3*(a2*a) + p4*(a2*a2); 206 | 207 | if (x < 0) { 208 | return -y; 209 | } 210 | return y; 211 | 212 | } else { 213 | at::real xx = x*x; 214 | 215 | // accuracy = 0.10550 degrees (according to matlab) 216 | return (at::real(0.00182789418543) + 217 | at::real(0.97687229491851)*x + 218 | at::real(0.00087659977713)*xx)/ 219 | (at::real(0.99499024627366) + 220 | at::real(0.00228262896304)*x + 221 | at::real(0.25288677429562)*xx); 222 | } 223 | } 224 | 225 | 226 | // strictfp public static void main(String args[]) 227 | // { 228 | // Random r = new Random(); 229 | 230 | // at::real err = 0; 231 | // at::real M = 100; 232 | 233 | // System.out.println("Max_at::real: "+At::Real.MAX_VALUE); 234 | 235 | // System.out.println("Checking atan"); 236 | 237 | // for (int i=0;i<10000000;i++) 238 | // { 239 | // at::real x = M*r.nextAt::Real()-M/2; 240 | // at::real y = M*r.nextAt::Real()-M/2; 241 | 242 | // if (r.nextInt(100)==0) 243 | // x=0; 244 | // else if (r.nextInt(100)==0) 245 | // y=0; 246 | 247 | // at::real v1 = Math.atan2(y,x); 248 | // at::real v2 = atan2(y,x); 249 | 250 | // // System.out.println(x+" "+y); 251 | // at::real thiserr = Math.abs(v1-v2); 252 | // if (thiserr>.1) 253 | // System.out.println(x+"\t"+y+"\t"+v1+"\t"+v2); 254 | // if (thiserr>err) 255 | // err=thiserr; 256 | // } 257 | // System.out.println("err: "+err); 258 | // System.out.println("err deg: "+Math.toDegrees(err)); 259 | 260 | // err = 0; 261 | // M=500; 262 | // System.out.println("Checking exp"); 263 | // for (int i=0;i<10000000;i++) 264 | // { 265 | // at::real x = r.nextAt::Real()*M-M/2; 266 | // at::real v1 = Math.exp(x); 267 | // at::real v2 = exp(x); 268 | 269 | // at::real abserr = Math.abs(v1-v2); 270 | // at::real relerr = Math.abs((v2-v1)/v1); 271 | 272 | // if ((x<.5 && abserr>0.01) || (x>.5 && relerr>0.004)) 273 | // System.out.println(x+"\t"+v1+"\t"+v2); 274 | // } 275 | 276 | // System.out.println("Benchmarking exp"); 277 | // // benchexp(); 278 | 279 | // /////////////////////////////////////////////// 280 | // System.out.println("Benchmarking atan"); 281 | 282 | // if (true) { 283 | // at::real d[] = new at::real[10000]; 284 | 285 | // for (int i = 0; i < d.length; i++) { 286 | // d[i] = r.nextAt::Real(); 287 | // } 288 | 289 | // if (true) { 290 | // Tic tic = new Tic(); 291 | // for (int i = 0; i < d.length; i++) { 292 | // for (int j = 0; j < d.length; j++) { 293 | // at::real v = Math.atan2(d[i],d[j]); 294 | // } 295 | // } 296 | // System.out.printf("native: %15f\n", tic.toc()); 297 | // } 298 | 299 | // if (true) { 300 | // Tic tic = new Tic(); 301 | // for (int i = 0; i < d.length; i++) { 302 | // for (int j = 0; j < d.length; j++) { 303 | // at::real v = atan2(d[i],d[j]); 304 | // } 305 | // } 306 | // System.out.printf("our version: %15f\n", tic.toc()); 307 | // } 308 | // } 309 | 310 | // } 311 | 312 | // public static void benchexp() 313 | // { 314 | // Random r = new Random(); 315 | // long startTime, endTime; 316 | // at::real elapsedTime; 317 | // int iter = 100000000; 318 | // at::real v; 319 | 320 | // startTime = System.currentTimeMillis(); 321 | // v=0; 322 | // for (int i=0;i 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #ifndef _MATHUTIL_H_ 10 | #define _MATHUTIL_H_ 11 | 12 | #include 13 | #include "AprilTypes.h" 14 | 15 | // Fast math routines for april tags. TODO: profile to see if these 16 | // are still necessary after portint to C++. 17 | 18 | class MathUtil { 19 | private: 20 | 21 | 22 | // only good for positive numbers. 23 | static at::real mod2pi_pos(at::real vin); 24 | 25 | public: 26 | 27 | static const at::real epsilon; 28 | static const at::real twopi_inv; 29 | static const at::real twopi; 30 | 31 | static at::real fabs(at::real f); 32 | 33 | /** Ensure that v is [-PI, PI] **/ 34 | static at::real mod2pi(at::real vin); 35 | 36 | static at::real mod2pi(at::real ref, at::real v); 37 | 38 | static at::real atan2(at::real y, at::real x); 39 | 40 | static at::real atan(at::real x); 41 | 42 | static at::real atan_mag1(at::real x); 43 | 44 | }; 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /src/Profiler.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * This file is distributed as part of the C++ port of the APRIL tags 3 | * library. The code is licensed under GPLv2. 4 | * 5 | * Original author: Edwin Olson 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #ifndef _PROFILER_H_ 10 | #define _PROFILER_H_ 11 | 12 | #include 13 | #include 14 | 15 | // Lightweight classes/structs used for profiling AprilTags. 16 | 17 | ////////////////////////////////////////////////////////////////////// 18 | // Encapsulate a step/substep of an algorithm. 19 | 20 | class AlgorithmStep { 21 | public: 22 | 23 | int step; 24 | int substep; 25 | 26 | AlgorithmStep(): step(0), substep(0) {} 27 | AlgorithmStep(int v): step(v), substep(0) {} 28 | AlgorithmStep(int v, int i): step(v), substep(i) {} 29 | 30 | }; 31 | 32 | // We need to compare and sort these. 33 | 34 | inline bool operator==(const AlgorithmStep& s1, const AlgorithmStep& s2) { 35 | return s1.step == s2.step && s1.substep == s2.substep; 36 | } 37 | 38 | inline bool operator<(const AlgorithmStep& s1, const AlgorithmStep& s2) { 39 | return ( (s1.step < s2.step) || 40 | (s1.step == s2.step && s1.substep < s2.substep) ); 41 | } 42 | 43 | ////////////////////////////////////////////////////////////////////// 44 | // Structure used for profiling. 45 | 46 | struct TimingInfo { 47 | 48 | double start; 49 | double run; 50 | const char* desc; 51 | 52 | TimingInfo(): run(0) {} 53 | 54 | }; 55 | 56 | typedef std::map TimingLookup; 57 | 58 | ////////////////////////////////////////////////////////////////////// 59 | // Class for profiling. You may need to replace getTimeAsDouble on 60 | // platforms where gettimeofday is not available. 61 | 62 | class Profiler { 63 | public: 64 | 65 | TimingLookup timers; 66 | 67 | int num_iterations; 68 | int num_detections; 69 | 70 | Profiler(): num_iterations(0), num_detections(0) {} 71 | 72 | static double getTimeAsDouble() { 73 | struct timeval tp; 74 | gettimeofday(&tp, 0); 75 | return double(tp.tv_sec) + double(tp.tv_usec) * 1e-6; 76 | } 77 | 78 | void start(int step, int substep, const char* desc) { 79 | 80 | std::pair info = 81 | timers.insert(std::make_pair(AlgorithmStep(step, substep), TimingInfo())); 82 | 83 | TimingLookup::iterator i = info.first; 84 | 85 | i->second.start = getTimeAsDouble(); 86 | i->second.desc = desc; 87 | 88 | } 89 | 90 | void end(int step, int substep) { 91 | TimingLookup::iterator i = timers.find(AlgorithmStep(step, substep)); 92 | i->second.run += getTimeAsDouble() - i->second.start; 93 | if (i->first == AlgorithmStep(0,0)) { ++num_iterations; } 94 | } 95 | 96 | }; 97 | 98 | #endif 99 | -------------------------------------------------------------------------------- /src/Refine.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * This file is distributed as part of the C++ port of the APRIL tags 3 | * library. The code is licensed under GPLv2. 4 | * 5 | * Original author: Edwin Olson 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #include "Refine.h" 10 | #include 11 | #include 12 | #include 13 | 14 | at::Point interpolateH(const at::Mat& H, const at::Point& uv, at::Mat* pdxydh) { 15 | 16 | at::real hx = H[0][0]*uv.x + H[0][1]*uv.y + H[0][2]; 17 | at::real hy = H[1][0]*uv.x + H[1][1]*uv.y + H[1][2]; 18 | at::real hw = H[2][0]*uv.x + H[2][1]*uv.y + H[2][2]; 19 | 20 | if (pdxydh) { 21 | 22 | at::Mat& dxydh = *pdxydh; 23 | if (dxydh.rows != 2 || dxydh.cols != 8) { dxydh = at::Mat(2,8); } 24 | 25 | dxydh[0][0] = uv.x/hw; 26 | dxydh[0][1] = uv.y/hw; 27 | dxydh[0][2] = 1/hw; 28 | dxydh[0][3] = 0; 29 | dxydh[0][4] = 0; 30 | dxydh[0][5] = 0; 31 | at::real tmp0 = hw*hw; 32 | dxydh[0][6] = -(hx*uv.x)/tmp0; 33 | dxydh[0][7] = -(hx*uv.y)/tmp0; 34 | dxydh[1][0] = 0; 35 | dxydh[1][1] = 0; 36 | dxydh[1][2] = 0; 37 | dxydh[1][3] = uv.x/hw; 38 | dxydh[1][4] = uv.y/hw; 39 | dxydh[1][5] = 1/hw; 40 | dxydh[1][6] = -(hy*uv.x)/tmp0; 41 | dxydh[1][7] = -(hy*uv.y)/tmp0; 42 | 43 | 44 | } 45 | 46 | return at::Point(hx/hw, hy/hw); 47 | 48 | 49 | } 50 | 51 | // MZ notes: Pretty sure this code is autogenerated. Too bad I might 52 | // have lost the program that did this :) 53 | void computeH(const at::Point p[4], at::Mat& H, at::Mat* pdhdp) { 54 | 55 | if (H.rows != 3 || H.cols != 3) { H = at::Mat(3,3); } 56 | 57 | at::real tmp0 = p[0].x*p[2].x; 58 | at::real tmp1 = tmp0*p[1].y; 59 | at::real tmp2 = p[1].x*p[2].x; 60 | at::real tmp3 = p[0].x*p[3].x; 61 | at::real tmp4 = p[1].x*p[3].x; 62 | at::real tmp5 = tmp4*p[0].y; 63 | at::real tmp6 = tmp0*p[3].y; 64 | at::real tmp7 = tmp4*p[2].y; 65 | at::real tmp8 = tmp1 - tmp2*p[0].y - tmp3*p[1].y + tmp5 - tmp6 + tmp3*p[2].y + tmp2*p[3].y - tmp7; 66 | at::real tmp9 = p[1].x*p[2].y; 67 | at::real tmp10 = p[2].x*p[1].y; 68 | at::real tmp11 = tmp9 - tmp10; 69 | at::real tmp12 = p[1].x*p[3].y; 70 | at::real tmp13 = tmp11 - tmp12; 71 | at::real tmp14 = p[3].x*p[1].y; 72 | at::real tmp15 = p[2].x*p[3].y; 73 | at::real tmp16 = p[3].x*p[2].y; 74 | at::real tmp17 = tmp13 + tmp14 + tmp15 - tmp16; 75 | H[0][0] = tmp8/tmp17; 76 | at::real tmp18 = p[0].x*p[1].x; 77 | at::real tmp19 = p[2].x*p[3].x; 78 | at::real tmp20 = tmp18*p[2].y - tmp1 - tmp18*p[3].y + tmp5 + tmp6 - tmp19*p[0].y - tmp7 + tmp19*p[1].y; 79 | H[0][1] = -tmp20/tmp17; 80 | H[0][2] = p[0].x; 81 | at::real tmp21 = p[0].x*p[1].y; 82 | at::real tmp22 = p[1].x*p[0].y; 83 | at::real tmp23 = tmp22*p[2].y; 84 | at::real tmp24 = tmp21*p[3].y; 85 | at::real tmp25 = p[2].x*p[0].y; 86 | at::real tmp26 = p[3].x*p[0].y; 87 | at::real tmp27 = tmp26*p[2].y; 88 | at::real tmp28 = tmp10*p[3].y; 89 | at::real tmp29 = tmp21*p[2].y - tmp23 - tmp24 + tmp22*p[3].y - tmp25*p[3].y + tmp27 + tmp28 - tmp14*p[2].y; 90 | H[1][0] = tmp29/tmp17; 91 | at::real tmp30 = p[0].x*p[2].y; 92 | at::real tmp31 = tmp23 - tmp25*p[1].y - tmp24 + tmp26*p[1].y + tmp30*p[3].y - tmp27 - tmp9*p[3].y + tmp28; 93 | H[1][1] = -tmp31/tmp17; 94 | H[1][2] = p[0].y; 95 | at::real tmp32 = p[0].x*p[3].y; 96 | at::real tmp33 = tmp30 - tmp25 - tmp32 - tmp9 + tmp10 + tmp26 + tmp12 - tmp14; 97 | H[2][0] = tmp33/tmp17; 98 | at::real tmp34 = tmp21 - tmp22; 99 | at::real tmp35 = tmp34 - tmp30 + tmp25 + tmp12 - tmp14 - tmp15 + tmp16; 100 | H[2][1] = tmp35/tmp17; 101 | H[2][2] = 1; 102 | 103 | 104 | if (pdhdp) { 105 | 106 | at::Mat& dhdp = *pdhdp; 107 | if (dhdp.rows != 8 || dhdp.cols != 8) { dhdp = at::Mat(8,8); } 108 | 109 | dhdp[0][0] = (tmp10 - tmp14 - tmp15 + tmp16)/tmp17; 110 | dhdp[0][1] = (-tmp2 + tmp4)/tmp17; 111 | at::real tmp36 = p[2].y - p[3].y; 112 | at::real tmp37 = tmp17*tmp17; 113 | dhdp[0][2] = ((-tmp25 + tmp26 + tmp15 - tmp16)*tmp17 - tmp8*tmp36)/tmp37; 114 | at::real tmp38 = -p[2].x + p[3].x; 115 | dhdp[0][3] = ((tmp0 - tmp3)*tmp17 - tmp8*tmp38)/tmp37; 116 | at::real tmp39 = -p[1].y + p[3].y; 117 | dhdp[0][4] = ((tmp34 - tmp32 + tmp12)*tmp17 - tmp8*tmp39)/tmp37; 118 | at::real tmp40 = p[1].x - p[3].x; 119 | dhdp[0][5] = ((tmp3 - tmp4)*tmp17 - tmp8*tmp40)/tmp37; 120 | at::real tmp41 = -tmp21 + tmp22; 121 | at::real tmp42 = p[1].y - p[2].y; 122 | dhdp[0][6] = ((tmp41 + tmp30 - tmp9)*tmp17 - tmp8*tmp42)/tmp37; 123 | at::real tmp43 = -p[1].x + p[2].x; 124 | dhdp[0][7] = ((-tmp0 + tmp2)*tmp17 - tmp8*tmp43)/tmp37; 125 | dhdp[1][0] = -(tmp13 + tmp15)/tmp17; 126 | dhdp[1][1] = -(tmp4 - tmp19)/tmp17; 127 | at::real tmp44 = tmp30 - tmp32; 128 | dhdp[1][2] = -((tmp44 + tmp26 - tmp16)*tmp17 - tmp20*tmp36)/tmp37; 129 | dhdp[1][3] = -((-tmp0 + tmp19)*tmp17 - tmp20*tmp38)/tmp37; 130 | dhdp[1][4] = -((-tmp21 + tmp32 - tmp26 + tmp14)*tmp17 - tmp20*tmp39)/tmp37; 131 | dhdp[1][5] = -((tmp18 - tmp4)*tmp17 - tmp20*tmp40)/tmp37; 132 | dhdp[1][6] = -((tmp22 - tmp25 - tmp9 + tmp10)*tmp17 - tmp20*tmp42)/tmp37; 133 | dhdp[1][7] = -((-tmp18 + tmp0)*tmp17 - tmp20*tmp43)/tmp37; 134 | dhdp[2][0] = 1; 135 | dhdp[2][1] = 0; 136 | dhdp[2][2] = 0; 137 | dhdp[2][3] = 0; 138 | dhdp[2][4] = 0; 139 | dhdp[2][5] = 0; 140 | dhdp[2][6] = 0; 141 | dhdp[2][7] = 0; 142 | at::real tmp45 = p[1].y*p[2].y; 143 | at::real tmp46 = p[1].y*p[3].y; 144 | dhdp[3][0] = (tmp45 - tmp46)/tmp17; 145 | dhdp[3][1] = (-tmp9 + tmp12 - tmp15 + tmp16)/tmp17; 146 | at::real tmp47 = p[0].y*p[2].y; 147 | at::real tmp48 = p[0].y*p[3].y; 148 | dhdp[3][2] = ((-tmp47 + tmp48)*tmp17 - tmp29*tmp36)/tmp37; 149 | dhdp[3][3] = ((tmp44 + tmp15 - tmp16)*tmp17 - tmp29*tmp38)/tmp37; 150 | dhdp[3][4] = ((-tmp48 + tmp46)*tmp17 - tmp29*tmp39)/tmp37; 151 | dhdp[3][5] = ((tmp34 + tmp26 - tmp14)*tmp17 - tmp29*tmp40)/tmp37; 152 | dhdp[3][6] = ((tmp47 - tmp45)*tmp17 - tmp29*tmp42)/tmp37; 153 | dhdp[3][7] = ((tmp41 - tmp25 + tmp10)*tmp17 - tmp29*tmp43)/tmp37; 154 | at::real tmp49 = p[2].y*p[3].y; 155 | dhdp[4][0] = -(-tmp46 + tmp49)/tmp17; 156 | dhdp[4][1] = -(tmp11 + tmp14 - tmp16)/tmp17; 157 | dhdp[4][2] = -((tmp47 - tmp49)*tmp17 - tmp31*tmp36)/tmp37; 158 | dhdp[4][3] = -((-tmp25 - tmp32 + tmp26 + tmp15)*tmp17 - tmp31*tmp38)/tmp37; 159 | at::real tmp50 = p[0].y*p[1].y; 160 | dhdp[4][4] = -((-tmp50 + tmp46)*tmp17 - tmp31*tmp39)/tmp37; 161 | dhdp[4][5] = -((tmp22 + tmp32 - tmp26 - tmp12)*tmp17 - tmp31*tmp40)/tmp37; 162 | dhdp[4][6] = -((tmp50 - tmp47)*tmp17 - tmp31*tmp42)/tmp37; 163 | dhdp[4][7] = -((-tmp21 + tmp30 - tmp9 + tmp10)*tmp17 - tmp31*tmp43)/tmp37; 164 | dhdp[5][0] = 0; 165 | dhdp[5][1] = 1; 166 | dhdp[5][2] = 0; 167 | dhdp[5][3] = 0; 168 | dhdp[5][4] = 0; 169 | dhdp[5][5] = 0; 170 | dhdp[5][6] = 0; 171 | dhdp[5][7] = 0; 172 | dhdp[6][0] = tmp36/tmp17; 173 | dhdp[6][1] = tmp38/tmp17; 174 | dhdp[6][2] = ((-p[2].y + p[3].y)*tmp17 - tmp33*tmp36)/tmp37; 175 | dhdp[6][3] = ((p[2].x - p[3].x)*tmp17 - tmp33*tmp38)/tmp37; 176 | dhdp[6][4] = ((-p[0].y + p[1].y)*tmp17 - tmp33*tmp39)/tmp37; 177 | dhdp[6][5] = ((p[0].x - p[1].x)*tmp17 - tmp33*tmp40)/tmp37; 178 | dhdp[6][6] = ((p[0].y - p[1].y)*tmp17 - tmp33*tmp42)/tmp37; 179 | dhdp[6][7] = ((-p[0].x + p[1].x)*tmp17 - tmp33*tmp43)/tmp37; 180 | dhdp[7][0] = tmp42/tmp17; 181 | dhdp[7][1] = tmp43/tmp17; 182 | dhdp[7][2] = ((-p[0].y + p[3].y)*tmp17 - tmp35*tmp36)/tmp37; 183 | dhdp[7][3] = ((p[0].x - p[3].x)*tmp17 - tmp35*tmp38)/tmp37; 184 | dhdp[7][4] = ((p[0].y - p[3].y)*tmp17 - tmp35*tmp39)/tmp37; 185 | dhdp[7][5] = ((-p[0].x + p[3].x)*tmp17 - tmp35*tmp40)/tmp37; 186 | dhdp[7][6] = ((-p[1].y + p[2].y)*tmp17 - tmp35*tmp42)/tmp37; 187 | dhdp[7][7] = ((p[1].x - p[2].x)*tmp17 - tmp35*tmp43)/tmp37; 188 | 189 | } 190 | 191 | } 192 | 193 | // MZ notes: Pretty sure this code is autogenerated. Too bad I might 194 | // have lost the program that did this :) 195 | 196 | at::Point interpolate(const at::Point p[4], const at::Point& uv, at::Mat* pJ) { 197 | 198 | at::real tmp0 = p[1].x*p[2].y; 199 | at::real tmp1 = p[2].x*p[1].y; 200 | at::real tmp2 = tmp0 - tmp1; 201 | at::real tmp3 = p[1].x*p[3].y; 202 | at::real tmp4 = tmp2 - tmp3; 203 | at::real tmp5 = p[3].x*p[1].y; 204 | at::real tmp6 = p[2].x*p[3].y; 205 | at::real tmp7 = p[3].x*p[2].y; 206 | at::real tmp8 = tmp4 + tmp5 + tmp6 - tmp7; 207 | at::real tmp9 = p[0].x*p[2].x; 208 | at::real tmp10 = tmp9*p[1].y; 209 | at::real tmp11 = p[1].x*p[2].x; 210 | at::real tmp12 = p[0].x*p[3].x; 211 | at::real tmp13 = p[1].x*p[3].x; 212 | at::real tmp14 = tmp13*p[0].y; 213 | at::real tmp15 = tmp9*p[3].y; 214 | at::real tmp16 = tmp13*p[2].y; 215 | at::real tmp17 = tmp10 - tmp11*p[0].y - tmp12*p[1].y + tmp14 - tmp15 + tmp12*p[2].y + tmp11*p[3].y - tmp16; 216 | at::real tmp18 = p[0].x*p[1].x; 217 | at::real tmp19 = p[2].x*p[3].x; 218 | at::real tmp20 = tmp18*p[2].y - tmp10 - tmp18*p[3].y + tmp14 + tmp15 - tmp19*p[0].y - tmp16 + tmp19*p[1].y; 219 | at::real tmp21 = p[0].x*p[1].y; 220 | at::real tmp22 = p[1].x*p[0].y; 221 | at::real tmp23 = tmp22*p[2].y; 222 | at::real tmp24 = tmp21*p[3].y; 223 | at::real tmp25 = p[2].x*p[0].y; 224 | at::real tmp26 = p[3].x*p[0].y; 225 | at::real tmp27 = tmp26*p[2].y; 226 | at::real tmp28 = tmp1*p[3].y; 227 | at::real tmp29 = tmp21*p[2].y - tmp23 - tmp24 + tmp22*p[3].y - tmp25*p[3].y + tmp27 + tmp28 - tmp5*p[2].y; 228 | at::real tmp30 = p[0].x*p[2].y; 229 | at::real tmp31 = tmp23 - tmp25*p[1].y - tmp24 + tmp26*p[1].y + tmp30*p[3].y - tmp27 - tmp0*p[3].y + tmp28; 230 | at::real tmp32 = p[0].x*p[3].y; 231 | at::real tmp33 = tmp30 - tmp25 - tmp32 - tmp0 + tmp1 + tmp26 + tmp3 - tmp5; 232 | at::real tmp34 = tmp21 - tmp22; 233 | at::real tmp35 = tmp34 - tmp30 + tmp25 + tmp3 - tmp5 - tmp6 + tmp7; 234 | at::real hx = (tmp17/tmp8)*uv.x - (tmp20/tmp8)*uv.y + p[0].x; 235 | at::real hy = (tmp29/tmp8)*uv.x - (tmp31/tmp8)*uv.y + p[0].y; 236 | at::real hw = (tmp33/tmp8)*uv.x + (tmp35/tmp8)*uv.y + 1; 237 | 238 | if (pJ) { 239 | 240 | at::Mat& J = *pJ; 241 | if (J.rows != 2 || J.cols != 8) { J = at::Mat(2,8); } 242 | 243 | at::real tmp36 = tmp8*tmp8; 244 | at::real tmp37 = p[2].y - p[3].y; 245 | at::real tmp38 = p[1].y - p[2].y; 246 | at::real tmp39 = ((tmp37*tmp8)/tmp36)*uv.x + ((tmp38*tmp8)/tmp36)*uv.y; 247 | at::real tmp40 = hw*hw; 248 | J[0][0] = (((((tmp1 - tmp5 - tmp6 + tmp7)*tmp8)/tmp36)*uv.x - (((tmp4 + tmp6)*tmp8)/tmp36)*uv.y + 1)*hw - hx*tmp39)/tmp40; 249 | at::real tmp41 = -p[2].x + p[3].x; 250 | at::real tmp42 = -p[1].x + p[2].x; 251 | at::real tmp43 = ((tmp41*tmp8)/tmp36)*uv.x + ((tmp42*tmp8)/tmp36)*uv.y; 252 | J[0][1] = (((((-tmp11 + tmp13)*tmp8)/tmp36)*uv.x - (((tmp13 - tmp19)*tmp8)/tmp36)*uv.y)*hw - hx*tmp43)/tmp40; 253 | at::real tmp44 = tmp30 - tmp32; 254 | at::real tmp45 = (((-p[2].y + p[3].y)*tmp8 - tmp33*tmp37)/tmp36)*uv.x + (((-p[0].y + p[3].y)*tmp8 - tmp35*tmp37)/tmp36)*uv.y; 255 | J[0][2] = (((((-tmp25 + tmp26 + tmp6 - tmp7)*tmp8 - tmp17*tmp37)/tmp36)*uv.x - (((tmp44 + tmp26 - tmp7)*tmp8 - tmp20*tmp37)/tmp36)*uv.y)*hw - hx*tmp45)/tmp40; 256 | at::real tmp46 = (((p[2].x - p[3].x)*tmp8 - tmp33*tmp41)/tmp36)*uv.x + (((p[0].x - p[3].x)*tmp8 - tmp35*tmp41)/tmp36)*uv.y; 257 | J[0][3] = (((((tmp9 - tmp12)*tmp8 - tmp17*tmp41)/tmp36)*uv.x - (((-tmp9 + tmp19)*tmp8 - tmp20*tmp41)/tmp36)*uv.y)*hw - hx*tmp46)/tmp40; 258 | at::real tmp47 = -p[1].y + p[3].y; 259 | at::real tmp48 = (((-p[0].y + p[1].y)*tmp8 - tmp33*tmp47)/tmp36)*uv.x + (((p[0].y - p[3].y)*tmp8 - tmp35*tmp47)/tmp36)*uv.y; 260 | J[0][4] = (((((tmp34 - tmp32 + tmp3)*tmp8 - tmp17*tmp47)/tmp36)*uv.x - (((-tmp21 + tmp32 - tmp26 + tmp5)*tmp8 - tmp20*tmp47)/tmp36)*uv.y)*hw - hx*tmp48)/tmp40; 261 | at::real tmp49 = p[1].x - p[3].x; 262 | at::real tmp50 = (((p[0].x - p[1].x)*tmp8 - tmp33*tmp49)/tmp36)*uv.x + (((-p[0].x + p[3].x)*tmp8 - tmp35*tmp49)/tmp36)*uv.y; 263 | J[0][5] = (((((tmp12 - tmp13)*tmp8 - tmp17*tmp49)/tmp36)*uv.x - (((tmp18 - tmp13)*tmp8 - tmp20*tmp49)/tmp36)*uv.y)*hw - hx*tmp50)/tmp40; 264 | at::real tmp51 = -tmp21 + tmp22; 265 | at::real tmp52 = (((p[0].y - p[1].y)*tmp8 - tmp33*tmp38)/tmp36)*uv.x + (((-p[1].y + p[2].y)*tmp8 - tmp35*tmp38)/tmp36)*uv.y; 266 | J[0][6] = (((((tmp51 + tmp30 - tmp0)*tmp8 - tmp17*tmp38)/tmp36)*uv.x - (((tmp22 - tmp25 - tmp0 + tmp1)*tmp8 - tmp20*tmp38)/tmp36)*uv.y)*hw - hx*tmp52)/tmp40; 267 | at::real tmp53 = p[1].y*p[2].y; 268 | at::real tmp54 = (((-p[0].x + p[1].x)*tmp8 - tmp33*tmp42)/tmp36)*uv.x + (((p[1].x - p[2].x)*tmp8 - tmp35*tmp42)/tmp36)*uv.y; 269 | J[0][7] = (((((-tmp9 + tmp11)*tmp8 - tmp17*tmp42)/tmp36)*uv.x - (((-tmp18 + tmp9)*tmp8 - tmp20*tmp42)/tmp36)*uv.y)*hw - hx*tmp54)/tmp40; 270 | at::real tmp55 = p[1].y*p[3].y; 271 | at::real tmp56 = p[2].y*p[3].y; 272 | J[1][0] = (((((tmp53 - tmp55)*tmp8)/tmp36)*uv.x - (((-tmp55 + tmp56)*tmp8)/tmp36)*uv.y)*hw - hy*tmp39)/tmp40; 273 | J[1][1] = (((((-tmp0 + tmp3 - tmp6 + tmp7)*tmp8)/tmp36)*uv.x - (((tmp2 + tmp5 - tmp7)*tmp8)/tmp36)*uv.y + 1)*hw - hy*tmp43)/tmp40; 274 | at::real tmp57 = p[0].y*p[2].y; 275 | at::real tmp58 = p[0].y*p[3].y; 276 | J[1][2] = (((((-tmp57 + tmp58)*tmp8 - tmp29*tmp37)/tmp36)*uv.x - (((tmp57 - tmp56)*tmp8 - tmp31*tmp37)/tmp36)*uv.y)*hw - hy*tmp45)/tmp40; 277 | J[1][3] = (((((tmp44 + tmp6 - tmp7)*tmp8 - tmp29*tmp41)/tmp36)*uv.x - (((-tmp25 - tmp32 + tmp26 + tmp6)*tmp8 - tmp31*tmp41)/tmp36)*uv.y)*hw - hy*tmp46)/tmp40; 278 | at::real tmp59 = p[0].y*p[1].y; 279 | J[1][4] = (((((-tmp58 + tmp55)*tmp8 - tmp29*tmp47)/tmp36)*uv.x - (((-tmp59 + tmp55)*tmp8 - tmp31*tmp47)/tmp36)*uv.y)*hw - hy*tmp48)/tmp40; 280 | J[1][5] = (((((tmp34 + tmp26 - tmp5)*tmp8 - tmp29*tmp49)/tmp36)*uv.x - (((tmp22 + tmp32 - tmp26 - tmp3)*tmp8 - tmp31*tmp49)/tmp36)*uv.y)*hw - hy*tmp50)/tmp40; 281 | J[1][6] = (((((tmp57 - tmp53)*tmp8 - tmp29*tmp38)/tmp36)*uv.x - (((tmp59 - tmp57)*tmp8 - tmp31*tmp38)/tmp36)*uv.y)*hw - hy*tmp52)/tmp40; 282 | J[1][7] = (((((tmp51 - tmp25 + tmp1)*tmp8 - tmp29*tmp42)/tmp36)*uv.x - (((-tmp21 + tmp30 - tmp0 + tmp1)*tmp8 - tmp31*tmp42)/tmp36)*uv.y)*hw - hy*tmp54)/tmp40; 283 | 284 | } 285 | 286 | return at::Point(hx/hw, hy/hw); 287 | 288 | } 289 | 290 | 291 | 292 | void drawPoint(cv::Mat& m, const at::Point& p, 293 | const cv::Scalar& color, at::real sz, int thickness) { 294 | //cv::Point dp(sz, sz); 295 | //cv::rectangle(m, p-dp, p+dp, color, 1, 4); 296 | const int shift = 2; 297 | const int ss = (1 << shift); 298 | cv::circle(m, p*ss, sz*ss, color, thickness, CV_AA, shift); 299 | } 300 | 301 | void drawArrow(cv::Mat& m, const at::Point& p, const at::Point& g, 302 | const cv::Scalar& color, at::real scl) { 303 | 304 | const int shift = 2; 305 | const int ss = (1 << shift); 306 | 307 | cv::line(m, ss*p, ss*(p+g*scl), color, 1, CV_AA, shift); 308 | 309 | } 310 | 311 | void dilate(cv::Rect& r, int b, const cv::Size& sz) { 312 | 313 | int x0 = r.x; 314 | int y0 = r.y; 315 | 316 | int x1 = x0 + r.width - 1; 317 | int y1 = y0 + r.height - 1; 318 | 319 | x0 = borderInterpolate(x0-b, sz.width, cv::BORDER_REPLICATE); 320 | x1 = borderInterpolate(x1+b, sz.width, cv::BORDER_REPLICATE); 321 | 322 | y0 = borderInterpolate(y0-b, sz.height, cv::BORDER_REPLICATE); 323 | y1 = borderInterpolate(y1+b, sz.height, cv::BORDER_REPLICATE); 324 | 325 | r.x = x0; 326 | r.y = y0; 327 | r.width = x1-x0+1; 328 | r.height = y1-y0+1; 329 | 330 | } 331 | 332 | cv::Rect boundingRect(const at::Point p[4], const cv::Size& sz) { 333 | 334 | // get the rectangle 335 | int x0 = p[0].x, x1 = p[0].x, y0 = p[0].y, y1 = p[0].y; 336 | 337 | for (int i=0; i<4; ++i) { 338 | 339 | x0 = std::min(x0, int(p[i].x)); 340 | y0 = std::min(y0, int(p[i].y)); 341 | 342 | x1 = std::max(x1, int(p[i].x)+1); 343 | y1 = std::max(y1, int(p[i].y)+1); 344 | 345 | } 346 | 347 | x0 = std::max(x0, 0); 348 | x1 = std::min(x1, sz.width); 349 | 350 | y0 = std::max(y0, 0); 351 | y1 = std::min(y1, sz.height); 352 | 353 | int w = x1-x0; 354 | int h = y1-y0; 355 | 356 | return cv::Rect(x0, y0, w, h); 357 | 358 | } 359 | 360 | 361 | #define NEWINTERP 362 | //#define OLDINTERP 363 | 364 | static cv::Mat enlarge(const cv::Mat& image, int scl) { 365 | cv::Mat big; 366 | cv::resize(image, big, cv::Size(scl*image.cols, scl*image.rows), 367 | 0, 0, CV_INTER_CUBIC); 368 | return big; 369 | } 370 | 371 | int refineQuad(const cv::Mat& gmat, 372 | const at::Mat& gx, 373 | const at::Mat& gy, 374 | at::Point p[4], 375 | const TPointArray& tpoints, 376 | bool debug, 377 | int max_iter, 378 | at::real max_grad) { 379 | 380 | assert( gmat.type() == CV_8UC1 ); 381 | 382 | cv::Mat_ gimage = gmat; 383 | 384 | cv::Rect rect = boundingRect(p, gimage.size()); 385 | 386 | cv::Mat subimage(gimage, rect); 387 | double dmin, dmax; 388 | cv::minMaxLoc(subimage, &dmin, &dmax); 389 | at::real amin = dmin; 390 | at::real amax = dmax; 391 | at::real ascl = 255 / (amax - amin); 392 | 393 | at::real gnscl = at::real(1) / (tpoints.size() * 255 * 255); 394 | 395 | bool done = false; 396 | int iter; 397 | 398 | 399 | #ifdef NEWINTERP 400 | at::Mat H(3,3), dhdp(8,8), dxydh(2,8), didh(1,8), didp(8,1); 401 | #endif 402 | 403 | #ifdef OLDINTERP 404 | at::Mat dxydp(2,8), didp2(8,1); 405 | #endif 406 | 407 | for (iter=0; iter 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #ifndef _REFINE_H_ 10 | #define _REFINE_H_ 11 | 12 | // Data structures and functions for refining quads. 13 | 14 | #include "AprilTypes.h" 15 | #include 16 | 17 | ////////////////////////////////////////////////////////////////////// 18 | // Template point holding (x,y) location and target 19 | // intensity. Interior pixels (on tag border) should be black (t=0), 20 | // exterior should be white (t=1). 21 | 22 | class TPoint { 23 | public: 24 | 25 | at::Point p; 26 | at::real t; 27 | 28 | TPoint(); 29 | 30 | TPoint(const at::Point& point, 31 | at::real target): p(point), t(target) {} 32 | 33 | TPoint(at::real x, at::real y, 34 | at::real target): p(x,y), t(target) {} 35 | 36 | }; 37 | 38 | typedef std::vector TPointArray; 39 | 40 | ////////////////////////////////////////////////////////////////////// 41 | // Utility functions to annotate images with point/gradient 42 | // information. 43 | 44 | void drawPoint(cv::Mat& m, const at::Point& p, 45 | const cv::Scalar& color, 46 | at::real sz=3, int thickness=1); 47 | 48 | void drawArrow(cv::Mat& m, const at::Point& p, const at::Point& g, 49 | const cv::Scalar& color, at::real scl=15.0/255.0); 50 | 51 | ////////////////////////////////////////////////////////////////////// 52 | 53 | // Get the bounding rectangle of a set of 4 points. 54 | cv::Rect boundingRect(const at::Point p[4], const cv::Size& sz); 55 | 56 | // Dilate a rectangle, respecting image boundaries. 57 | void dilate(cv::Rect& r, int b, const cv::Size& sz); 58 | 59 | // Biilinear interpolation with jacobians. 60 | at::Point interpolate(const at::Point p[4], const at::Point& uv, at::Mat* pJ=0); 61 | 62 | // Homography (perspective) interpolation, with jacobians. 63 | at::Point interpolateH(const at::Mat& H, const at::Point& uv, at::Mat* pJxy=0); 64 | 65 | // Compute a homography given 4 image points. 66 | void computeH(const at::Point p[4], at::Mat& H, at::Mat* pJh=0); 67 | 68 | // Refine a detected quadrilateral using template matching. 69 | int refineQuad(const cv::Mat& gimage, 70 | const at::Mat& gx, 71 | const at::Mat& gy, 72 | at::Point p[4], 73 | const TPointArray& tpoints, 74 | bool debug = false, 75 | int max_iter = 25, 76 | at::real max_grad = 1e-2); 77 | 78 | ////////////////////////////////////////////////////////////////////// 79 | // Sample image using nearest neighbor, clamping image coordinates to 80 | // image border. 81 | 82 | template 83 | inline Tval bsample(const cv::Mat_& image, int x, int y) { 84 | int yy = borderInterpolate(y, image.rows, cv::BORDER_REPLICATE); 85 | int xx = borderInterpolate(x, image.cols, cv::BORDER_REPLICATE); 86 | return image(yy,xx); 87 | } 88 | 89 | template 90 | inline Tval bsample(const cv::Mat_& image, const cv::Point& p) { 91 | return bsample(image, p.x, p.y); 92 | } 93 | 94 | template 95 | inline Tval bsample(const cv::Mat_& image, const at::Point& p) { 96 | return bsample(image, int(p.x+0.5), int(p.y+0.5)); 97 | } 98 | 99 | ////////////////////////////////////////////////////////////////////// 100 | // Cubic interpolation over 4 equally spaced points. 101 | 102 | inline at::real cubicInterpolate(at::real p[4], at::real x) { 103 | return p[1] + at::real(0.5) * x * 104 | (p[2] - p[0] + x*(at::real(2.0)*p[0] - 105 | at::real(5.0)*p[1] + 106 | at::real(4.0)*p[2] - 107 | p[3] + x*(at::real(3.0)*(p[1] - p[2]) + p[3] - p[0]))); 108 | } 109 | 110 | ////////////////////////////////////////////////////////////////////// 111 | // Bicubic interpolation over equally spaced 4x4 patch. 112 | 113 | inline at::real bicubicInterpolate (at::real p[4][4], at::real x, at::real y) { 114 | at::real arr[4]; 115 | arr[0] = cubicInterpolate(p[0], y); 116 | arr[1] = cubicInterpolate(p[1], y); 117 | arr[2] = cubicInterpolate(p[2], y); 118 | arr[3] = cubicInterpolate(p[3], y); 119 | return cubicInterpolate(arr, x); 120 | } 121 | 122 | ////////////////////////////////////////////////////////////////////// 123 | // Bicubic interpolation of an image at a floating-point coordinate 124 | // location, respecting border. 125 | 126 | template 127 | inline at::real bicubicInterpolate(const cv::Mat_& image, 128 | const at::Point& pt) { 129 | 130 | at::real fx = pt.x - 0.5; 131 | at::real fy = pt.y - 0.5; 132 | 133 | int ix = fx; 134 | int iy = fy; 135 | 136 | fx -= ix; 137 | fy -= iy; 138 | 139 | at::real p[4][4]; 140 | 141 | for (int py=0; py<4; ++py) { 142 | for (int px=0; px<4; ++px) { 143 | p[px][py] = bsample(image, ix+px-1, iy+py-1); 144 | } 145 | } 146 | 147 | return bicubicInterpolate(p, fx, fy); 148 | 149 | } 150 | 151 | ////////////////////////////////////////////////////////////////////// 152 | // Extract a rectangular subimage with a given amount of border. 153 | 154 | template 155 | inline cv::Mat_ subimageWithBorder(const cv::Mat_& image, 156 | const cv::Rect& r, 157 | int border) { 158 | 159 | cv::Mat_ bimage(r.height + 2*border, r.width + 2*border); 160 | 161 | for (int y=-border; y 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #ifndef _TAGDETECTION_H_ 10 | #define _TAGDETECTION_H_ 11 | 12 | #include "AprilTypes.h" 13 | 14 | class TagFamily; 15 | 16 | /** Encapsulate the result of a tag detection. */ 17 | class TagDetection { 18 | public: 19 | 20 | /** Is the detection good enough? **/ 21 | bool good; 22 | 23 | /** Observed code **/ 24 | at::code_t obsCode; 25 | 26 | /** Matched code **/ 27 | at::code_t code; 28 | 29 | /** What was the ID of the detected tag? **/ 30 | size_t id; 31 | 32 | /** The hamming distance between the detected code and the true code. **/ 33 | at::uint hammingDistance; 34 | 35 | /** How many 90 degree rotations were required to align the code. **/ 36 | int rotation; 37 | 38 | ////////////////// Fields below here are filled in by TagDetector //////////////// 39 | /** Position (in fractional pixel coordinates) of the detection. The 40 | * points travel around counter clockwise around the target, 41 | * always starting from the same corner of the tag. Dimensions 42 | * [4][2]. **/ 43 | at::Point p[4]; 44 | 45 | /** Center of tag in pixel coordinates. **/ 46 | at::Point cxy; 47 | 48 | /** Measured in pixels, how long was the observed perimeter 49 | * (i.e., excluding inferred perimeter which is used to 50 | * connect incomplete quads) **/ 51 | at::real observedPerimeter; 52 | 53 | /** A 3x3 homography that computes pixel coordinates from 54 | * tag-relative coordinates. Both the input and output coordinates 55 | * are 2D homogenous vectors, with y = Hx. y are pixel 56 | * coordinates, x are tag-relative coordinates. Tag coordinates 57 | * span from (-1,-1) to (1,1). The orientation of the homography 58 | * reflects the orientation of the target. **/ 59 | at::Mat homography; 60 | 61 | /** The homography is relative to image center, whose coordinates 62 | * are below. 63 | **/ 64 | at::Point hxy; 65 | 66 | at::Point interpolate(at::real x, at::real y) const { 67 | 68 | const at::Mat& H = homography; 69 | at::real z = H[2][0]*x + H[2][1]*y + H[2][2]; 70 | return at::Point( (H[0][0]*x + H[0][1]*y + H[0][2])/z + hxy.x, 71 | (H[1][0]*x + H[1][1]*y + H[1][2])/z + hxy.y ); 72 | 73 | } 74 | 75 | at::Point interpolate(const at::Point& p) const { 76 | return interpolate(p.x, p.y); 77 | } 78 | 79 | }; 80 | 81 | typedef std::vector TagDetectionArray; 82 | 83 | #endif 84 | -------------------------------------------------------------------------------- /src/TagDetector.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * This file is distributed as part of the C++ port of the APRIL tags 3 | * library. The code is licensed under GPLv2. 4 | * 5 | * Original author: Edwin Olson 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #ifndef _TAGDETECTOR_H_ 10 | #define _TAGDETECTOR_H_ 11 | 12 | #include "Geometry.h" 13 | #include "TagFamily.h" 14 | #include "Refine.h" 15 | #include "Profiler.h" 16 | 17 | ////////////////////////////////////////////////////////////////////// 18 | // Class to hold parameters for TagDetector. 19 | 20 | class TagDetectorParams { 21 | public: 22 | 23 | TagDetectorParams(); 24 | 25 | at::real sigma; 26 | at::real segSigma; 27 | bool segDecimate; 28 | at::real minMag; 29 | at::real maxEdgeCost; 30 | at::real thetaThresh; 31 | at::real magThresh; 32 | at::real minimumLineLength; 33 | at::real minimumSegmentSize; 34 | at::real minimumTagSize; 35 | at::real maxQuadAspectRatio; 36 | at::real adaptiveThresholdValue; 37 | int adaptiveThresholdRadius; 38 | bool refineQuads; 39 | bool refineBad; 40 | bool newQuadAlgorithm; 41 | 42 | }; 43 | 44 | 45 | ////////////////////////////////////////////////////////////////////// 46 | // A tag detector, workhorse class of apriltags system. 47 | 48 | class TagDetector { 49 | public: 50 | 51 | enum { WEIGHT_SCALE = 100 }; 52 | 53 | static const bool kDefaultDebug; 54 | static const bool kDefaultDebugNumberFiles; 55 | static const char* kDefaultDebugWindowName; 56 | 57 | // Construct with given parameters and TagFamily. 58 | explicit TagDetector(const TagFamily& f, 59 | const TagDetectorParams& parameters = 60 | TagDetectorParams()); 61 | 62 | // Fast atan2. TODO: shove this into MathUtil.h 63 | static at::real arctan2(at::real y, at::real x); 64 | 65 | // Compute the cost/weight of a pair of image gradients given in 66 | // polar form. 67 | int edgeCost(at::real theta0, at::real mag0, 68 | at::real theta1, at::real mag1) const; 69 | 70 | // Workhorse function: find detections in a given image. 71 | void process(const cv::Mat& image, 72 | const at::Point& opticalCenter, 73 | TagDetectionArray& detections) const; 74 | 75 | // Dump output of profiling timer. 76 | void reportTimers(); 77 | 78 | // Reference to tag family. 79 | const TagFamily& tagFamily; 80 | 81 | // Current parameters. 82 | TagDetectorParams params; 83 | 84 | // Display/save debug images? 85 | bool debug; 86 | 87 | // Should saved debug images be numbered by iteration? 88 | bool debugNumberFiles; 89 | 90 | // Name of window for debug images. If this is empty, will instead 91 | // emit files. 92 | std::string debugWindowName; 93 | 94 | // Hold our profiling information. This should be the only local 95 | // variable that is modified during the process function. 96 | mutable Profiler prof; 97 | 98 | private: 99 | 100 | struct Images { 101 | 102 | cv::Mat orig; 103 | cv::Mat_ origRGB; 104 | cv::Mat origBW; 105 | cv::Mat_ origBW8; 106 | 107 | at::Mat fimOrig; 108 | at::Mat fim; 109 | 110 | at::Point opticalCenter; 111 | 112 | }; 113 | 114 | static void sampleGradient(const Images& images, 115 | int x, int y, 116 | at::real& gx, at::real& gy); 117 | 118 | 119 | TPointArray tpoints; 120 | 121 | void makeImages(const cv::Mat& image, 122 | const at::Point& opticalCenter, 123 | Images& images) const; 124 | 125 | void getQuads_AT(const Images& images, 126 | QuadArray& quads) const; 127 | 128 | void getQuads_MZ(const Images& images, 129 | QuadArray& quads) const; 130 | 131 | void refineQuadL(const Images& images, Quad& quad) const; 132 | 133 | void refineQuadTT(const Images& images, Quad& quad) const; 134 | 135 | void refineQuadLSQ(const Images& images, Quad& quad) const; 136 | 137 | void refineQuads(const Images& images, 138 | QuadArray& quads) const; 139 | 140 | void debugShowQuads(const Images& images, 141 | const QuadArray& quads, 142 | int step, const std::string& label) const; 143 | 144 | void decode(const Images& images, 145 | const QuadArray& quads, 146 | TagDetectionArray& detections) const; 147 | 148 | bool decodeQuad(const Images& images, 149 | const Quad& quad, 150 | size_t quadIndex, 151 | TagDetectionArray& detections) const; 152 | 153 | void search(const at::Point& opticalCenter, 154 | QuadArray& quads, 155 | Segment* path[5], 156 | Segment* parent, 157 | int depth) const; 158 | 159 | 160 | }; 161 | 162 | 163 | 164 | #endif 165 | -------------------------------------------------------------------------------- /src/TagFamily.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * This file is distributed as part of the C++ port of the APRIL tags 3 | * library. The code is licensed under GPLv2. 4 | * 5 | * Original author: Edwin Olson 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #include "TagFamily.h" 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | TagFamily::TagFamily() { 18 | } 19 | 20 | TagFamily::TagFamily(const std::string& name) { 21 | init(name); 22 | } 23 | 24 | TagFamily::TagFamily(int bits, 25 | int minimumHammingDistance, 26 | size_t count, 27 | const code_t* data) { 28 | 29 | init(bits, minimumHammingDistance, count, data); 30 | 31 | } 32 | 33 | void TagFamily::init(int b, 34 | int mhd, 35 | size_t count, 36 | const code_t* data) { 37 | 38 | whiteBorder = 1; 39 | blackBorder = 1; 40 | 41 | bits = b; 42 | 43 | d = (int) sqrt(bits); 44 | assert(d*d == bits); 45 | 46 | minimumHammingDistance = std::max(mhd, 1); 47 | errorRecoveryBits = std::max((minimumHammingDistance-1)/2, uint(1)); 48 | codes.assign(data, data+count); 49 | 50 | } 51 | 52 | void TagFamily::setErrorRecoveryBits(int b) { 53 | errorRecoveryBits = b; 54 | } 55 | 56 | void TagFamily::setErrorRecoveryFraction(at::real v) { 57 | errorRecoveryBits = minimumHammingDistance*v/2; 58 | } 59 | 60 | TagFamily::code_t TagFamily::rotate90(TagFamily::code_t w, uint d) { 61 | 62 | code_t wr = 0; 63 | 64 | for (uint r = d-1; r < d; r--) { 65 | for (uint c = 0; c < d; c++) { 66 | code_t b = r + d*c; 67 | 68 | wr = wr << code_t(1); 69 | 70 | if ((w & (code_t(1) << b))!=0) 71 | wr |= code_t(1); 72 | 73 | } 74 | } 75 | 76 | return wr; 77 | 78 | } 79 | 80 | /** Compute the hamming distance between two code_ts. **/ 81 | at::uint TagFamily::hammingDistance(TagFamily::code_t a, TagFamily::code_t b) { 82 | return popCount(a^b); 83 | } 84 | 85 | /** How many bits are set in the code_t? **/ 86 | at::uint TagFamily::popCountReal(TagFamily::code_t w) { 87 | uint cnt = 0; 88 | while (w != 0) { 89 | w &= (w-code_t(1)); 90 | cnt++; 91 | } 92 | return cnt; 93 | } 94 | 95 | const TagFamily::ByteArray& TagFamily::getPopCountTable() { 96 | 97 | static ByteArray tbl; 98 | 99 | if (tbl.empty()) { 100 | tbl.resize(1<>= popCountTableShift; 121 | } 122 | 123 | return count; 124 | 125 | } 126 | 127 | /** Given an observed tag with code 'rcode', try to recover the 128 | * id. The corresponding fields of TagDetection will be filled 129 | * in. **/ 130 | void TagFamily::decode(TagDetection& det, TagFamily::code_t rcode) const { 131 | 132 | size_t bestid = -1; 133 | uint besthamming = uint(-1); 134 | int bestrotation = 0; 135 | code_t bestcode = 0; 136 | 137 | code_t rcodes[4]; 138 | 139 | rcodes[0] = rcode; 140 | rcodes[1] = rotate90(rcodes[0], d); 141 | rcodes[2] = rotate90(rcodes[1], d); 142 | rcodes[3] = rotate90(rcodes[2], d); 143 | 144 | for (size_t id = 0; id < codes.size(); id++) { 145 | 146 | for (int rot = 0; rot < 4; rot++) { 147 | uint thishamming = hammingDistance(rcodes[rot], codes[id]); 148 | if (thishamming < besthamming) { 149 | besthamming = thishamming; 150 | bestrotation = rot; 151 | bestid = id; 152 | bestcode = codes[id]; 153 | } 154 | } 155 | } 156 | 157 | det.id = bestid; 158 | det.hammingDistance = besthamming; 159 | det.rotation = bestrotation; 160 | det.good = (det.hammingDistance <= errorRecoveryBits); 161 | det.obsCode = rcode; 162 | det.code = bestcode; 163 | 164 | } 165 | 166 | 167 | /** Return the dimension of the tag including borders when we render it.**/ 168 | at::uint TagFamily::getTagRenderDimension() const { 169 | return whiteBorder*2 + blackBorder*2 + d; 170 | } 171 | 172 | 173 | void TagFamily::printHammingDistances() const { 174 | 175 | //int hammings[] = new int[d*d+1]; 176 | std::vector hammings(d*d+1, 0); 177 | 178 | for (size_t i = 0; i < codes.size(); i++) { 179 | code_t r0 = codes[i]; 180 | code_t r1 = rotate90(r0, d); 181 | code_t r2 = rotate90(r1, d); 182 | code_t r3 = rotate90(r2, d); 183 | 184 | for (size_t j = i+1; j < codes.size(); j++) { 185 | 186 | uint d = std::min(std::min(hammingDistance(r0, codes[j]), 187 | hammingDistance(r1, codes[j])), 188 | std::min(hammingDistance(r2, codes[j]), 189 | hammingDistance(r3, codes[j]))); 190 | 191 | assert(d < hammings.size()); 192 | hammings[d]++; 193 | } 194 | } 195 | 196 | for (size_t i = 0; i < hammings.size(); i++) { 197 | std::cout << i << " " << hammings[i] << "\n"; 198 | } 199 | 200 | } 201 | 202 | cv::Mat_ TagFamily::makeImage(size_t id) const { 203 | 204 | code_t v = codes[id]; 205 | 206 | uint width = getTagRenderDimension(); 207 | uint height = getTagRenderDimension(); 208 | 209 | cv::Mat_ rval(height, width); 210 | const byte white = 255; 211 | const byte black = 0; 212 | 213 | // Draw the borders. It's easier to do this by iterating over 214 | // the whole tag than just drawing the borders. 215 | for (uint y = 0; y < width; y++) { 216 | for (uint x = 0; x < height; x++) { 217 | if (y < whiteBorder || y+whiteBorder >= height || 218 | x < whiteBorder || x+whiteBorder >= width) 219 | rval(y,x) = white; 220 | else 221 | rval(y,x) = black; 222 | } 223 | } 224 | 225 | int bb = whiteBorder + blackBorder; 226 | 227 | // Now, draw the payload. 228 | for (uint y = 0; y < d; y++) { 229 | for (uint x = 0; x < d; x++) { 230 | if ((v&(code_t(1)< \n"); 348 | System.out.printf("Example: art.tag.Tag25h11 /tmp/tag25h11\n"); 349 | return; 350 | } 351 | 352 | String cls = args[1]; 353 | String dirpath = args[2] + "/"; 354 | 355 | TagFamily tagFamily = (TagFamily) april.util.ReflectUtil.createObject(cls); 356 | if (tagFamily == null) 357 | return; 358 | 359 | try { 360 | File f = new File(dirpath); 361 | if (!f.exists()) 362 | f.mkdirs(); 363 | 364 | tagFamily.writeAllImagesMosaic(dirpath+"mosaic.png"); 365 | tagFamily.writeAllImages(dirpath); 366 | tagFamily.writeAllImagesPostScript(dirpath+"alltags.ps"); 367 | } catch (IOException ex) { 368 | System.out.println("ex: "+ex); 369 | } 370 | } 371 | */ 372 | 373 | -------------------------------------------------------------------------------- /src/TagFamily.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * This file is distributed as part of the C++ port of the APRIL tags 3 | * library. The code is licensed under GPLv2. 4 | * 5 | * Original author: Edwin Olson 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #ifndef _TAGFAMILY_H_ 10 | #define _TAGFAMILY_H_ 11 | 12 | #include 13 | #include 14 | #include "TagDetection.h" 15 | 16 | /** Encapsulate information about a tag family. */ 17 | 18 | class TagFamily { 19 | public: 20 | 21 | typedef unsigned char byte; 22 | typedef at::code_t code_t; 23 | typedef at::uint uint; 24 | 25 | typedef std::vector CodeArray; 26 | typedef std::vector ByteArray; 27 | typedef std::vector StringArray; 28 | 29 | 30 | /** Static list of available tag families. */ 31 | static StringArray families(); 32 | 33 | /** Default constructor doesn't do much of anything, call init() 34 | later to set up. */ 35 | TagFamily(); 36 | 37 | /** Construct with named family (returned by families() method). */ 38 | TagFamily(const std::string& name); 39 | 40 | /** Construct with given data. */ 41 | TagFamily(int bits, int minimumHammingDistance, 42 | size_t count, const code_t* data); 43 | 44 | /** Initialize using the named family (returned by families() method). */ 45 | void init(const std::string& name); 46 | 47 | /** How many pixels wide is the outer-most white border? This is 48 | * only used when rendering a tag, not for decoding a tag (since 49 | * we look for the white/black edge). **/ 50 | uint whiteBorder; 51 | 52 | /** How many pixels wide is the inner black border? **/ 53 | uint blackBorder; 54 | 55 | /** number of bits in the tag. Must be a square (n^2). **/ 56 | uint bits; 57 | 58 | /** dimension of tag. e.g. for 16 bits, d=4. Must be sqrt(bits). **/ 59 | uint d; 60 | 61 | /** What is the minimum hamming distance between any two codes 62 | * (accounting for rotational ambiguity? The code can recover 63 | * (minHammingDistance-1)/2 bit errors. 64 | **/ 65 | uint minimumHammingDistance; 66 | 67 | /** The error recovery value determines our position on the ROC 68 | * curve. We will report codes that are within errorRecoveryBits 69 | * of a valid code. Small values mean greater rejection of bogus 70 | * tags (but false negatives). Large values mean aggressive 71 | * reporting of bad tags (but with a corresponding increase in 72 | * false positives). 73 | **/ 74 | uint errorRecoveryBits; 75 | 76 | /** The array of the codes. The id for a code is its index. **/ 77 | CodeArray codes; 78 | 79 | void init(int bits, 80 | int minimumHammingDistance, 81 | size_t count, 82 | const code_t* data); 83 | 84 | void setErrorRecoveryBits(int b); 85 | 86 | void setErrorRecoveryFraction(at::real v); 87 | 88 | /** if the bits in w were arranged in a d*d grid and that grid was 89 | * rotated, what would the new bits in w be? 90 | * The bits are organized like this (for d = 3): 91 | * 92 | * 8 7 6 2 5 8 0 1 2 93 | * 5 4 3 ==> 1 4 7 ==> 3 4 5 (rotate90 applied twice) 94 | * 2 1 0 0 3 6 6 7 8 95 | **/ 96 | static code_t rotate90(code_t w, uint d); 97 | 98 | /** Compute the hamming distance between two code_ts. **/ 99 | static uint hammingDistance(code_t a, code_t b); 100 | 101 | /** How many bits are set in the code_t? **/ 102 | static uint popCountReal(code_t w); 103 | 104 | enum { popCountTableShift = 12 }; 105 | 106 | static const ByteArray& getPopCountTable(); 107 | 108 | static uint popCount(code_t w); 109 | 110 | void decode(TagDetection& det, code_t rcode) const; 111 | 112 | 113 | uint getTagRenderDimension() const; 114 | 115 | void printHammingDistances() const; 116 | 117 | cv::Mat_ makeImage(size_t id) const; 118 | 119 | cv::Mat getWarp(const TagDetection& det) const; 120 | 121 | cv::Mat superimposeDetection(const cv::Mat& image, 122 | const TagDetection& det) const; 123 | 124 | cv::Mat detectionImage(const TagDetection& det, 125 | const cv::Size& size, int cvtype, 126 | const cv::Scalar& bgcolor = cv::Scalar(0,0,0,0)) const; 127 | 128 | cv::Mat superimposeDetections(const cv::Mat& image, 129 | const TagDetectionArray& detections) const; 130 | 131 | }; 132 | 133 | 134 | #endif 135 | -------------------------------------------------------------------------------- /src/UnionFindSimple.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * This file is distributed as part of the C++ port of the APRIL tags 3 | * library. The code is licensed under GPLv2. 4 | * 5 | * Original author: Edwin Olson 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #include "UnionFindSimple.h" 10 | 11 | /** @param maxid The maximum node id that will be referenced. **/ 12 | UnionFindSimple::UnionFindSimple(int maxid): data(maxid*SZ) { 13 | 14 | for (int i = 0; i < maxid; i++) { 15 | // everyone is their own cluster of size 1 16 | data[SZ*i+0] = i; 17 | data[SZ*i+1] = 1; 18 | } 19 | 20 | } 21 | 22 | int UnionFindSimple::getSetSize(int id) { 23 | return data[SZ*getRepresentative(id)+1]; 24 | } 25 | 26 | int UnionFindSimple::getRepresentative(int id) { 27 | 28 | // terminal case: a node is its own parent. 29 | if (data[SZ*id]==id) { 30 | return id; 31 | } 32 | 33 | // otherwise, recurse... 34 | int root = getRepresentative(data[SZ*id]); 35 | 36 | // short circuit the path. 37 | data[SZ*id] = root; 38 | 39 | return root; 40 | 41 | } 42 | 43 | int UnionFindSimple::connectNodes(int aid, int bid) { 44 | 45 | int aroot = getRepresentative(aid); 46 | int broot = getRepresentative(bid); 47 | 48 | if (aroot == broot) { 49 | return aroot; 50 | } 51 | 52 | int asz = data[SZ*aroot+1]; 53 | int bsz = data[SZ*broot+1]; 54 | 55 | if (asz > bsz) { 56 | data[SZ*broot] = aroot; 57 | data[SZ*aroot+1] += bsz; 58 | return aroot; 59 | } else { 60 | data[SZ*aroot] = broot; 61 | data[SZ*broot+1] += asz; 62 | return broot; 63 | } 64 | 65 | } 66 | 67 | -------------------------------------------------------------------------------- /src/UnionFindSimple.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * This file is distributed as part of the C++ port of the APRIL tags 3 | * library. The code is licensed under GPLv2. 4 | * 5 | * Original author: Edwin Olson 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #ifndef _UNIONFINDSIMPLE_H_ 10 | #define _UNIONFINDSIMPLE_H_ 11 | 12 | #include 13 | 14 | // Implement a union find algorithm (see original April tags paper for 15 | // details). 16 | 17 | class UnionFindSimple { 18 | public: 19 | 20 | std::vector data; 21 | 22 | enum { SZ = 2 }; 23 | 24 | /** @param maxid The maximum node id that will be referenced. **/ 25 | UnionFindSimple(int maxid); 26 | 27 | int getSetSize(int id); 28 | 29 | int getRepresentative(int id); 30 | 31 | int connectNodes(int aid, int bid); 32 | 33 | }; 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /src/camtest.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * This file is distributed as part of the C++ port of the APRIL tags 3 | * library. The code is licensed under GPLv2. 4 | * 5 | * Original author: Edwin Olson 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #include "TagDetector.h" 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include "CameraUtil.h" 21 | 22 | #define DEFAULT_TAG_FAMILY "Tag36h11" 23 | 24 | typedef struct CamTestOptions { 25 | CamTestOptions() : 26 | params(), 27 | family_str(DEFAULT_TAG_FAMILY), 28 | error_fraction(1), 29 | device_num(0), 30 | focal_length(500), 31 | tag_size(0.1905), 32 | frame_width(0), 33 | frame_height(0), 34 | mirror_display(true) 35 | { 36 | } 37 | TagDetectorParams params; 38 | std::string family_str; 39 | double error_fraction; 40 | int device_num; 41 | double focal_length; 42 | double tag_size; 43 | int frame_width; 44 | int frame_height; 45 | bool mirror_display; 46 | } CamTestOptions; 47 | 48 | 49 | void print_usage(const char* tool_name, FILE* output=stderr) { 50 | 51 | TagDetectorParams p; 52 | CamTestOptions o; 53 | 54 | fprintf(output, "\ 55 | Usage: %s [OPTIONS]\n\ 56 | Run a tool to test tag detection. Options:\n\ 57 | -h Show this help message.\n\ 58 | -D Use decimation for segmentation stage.\n\ 59 | -S SIGMA Set the original image sigma value (default %.2f).\n\ 60 | -s SEGSIGMA Set the segmentation sigma value (default %.2f).\n\ 61 | -a THETATHRESH Set the theta threshold for clustering (default %.1f).\n\ 62 | -m MAGTHRESH Set the magnitude threshold for clustering (default %.1f).\n\ 63 | -V VALUE Set adaptive threshold value for new quad algo (default %f).\n\ 64 | -N RADIUS Set adaptive threshold radius for new quad algo (default %d).\n\ 65 | -b Refine bad quads using template tracker.\n\ 66 | -r Refine all quads using template tracker.\n\ 67 | -n Use the new quad detection algorithm.\n\ 68 | -f FAMILY Look for the given tag family (default \"%s\")\n\ 69 | -e FRACTION Set error detection fraction (default %f)\n\ 70 | -d DEVICE Set camera device number (default %d)\n\ 71 | -F FLENGTH Set the camera's focal length in pixels (default %f)\n\ 72 | -z SIZE Set the tag size in meters (default %f)\n\ 73 | -W WIDTH Set the camera image width in pixels\n\ 74 | -H HEIGHT Set the camera image height in pixels\n\ 75 | -M Toggle display mirroring\n", 76 | tool_name, 77 | p.sigma, 78 | p.segSigma, 79 | p.thetaThresh, 80 | p.magThresh, 81 | p.adaptiveThresholdValue, 82 | p.adaptiveThresholdRadius, 83 | DEFAULT_TAG_FAMILY, 84 | o.error_fraction, 85 | o.device_num, 86 | o.focal_length, 87 | o.tag_size); 88 | 89 | 90 | fprintf(output, "Known tag families:"); 91 | TagFamily::StringArray known = TagFamily::families(); 92 | for (size_t i = 0; i < known.size(); ++i) { 93 | fprintf(output, " %s", known[i].c_str()); 94 | } 95 | fprintf(output, "\n"); 96 | } 97 | 98 | CamTestOptions parse_options(int argc, char** argv) { 99 | CamTestOptions opts; 100 | const char* options_str = "hDS:s:a:m:V:N:brnf:e:d:F:z:W:H:M"; 101 | int c; 102 | while ((c = getopt(argc, argv, options_str)) != -1) { 103 | switch (c) { 104 | // Reminder: add new options to 'options_str' above and print_usage()! 105 | case 'h': print_usage(argv[0], stdout); exit(0); break; 106 | case 'D': opts.params.segDecimate = true; break; 107 | case 'S': opts.params.sigma = atof(optarg); break; 108 | case 's': opts.params.segSigma = atof(optarg); break; 109 | case 'a': opts.params.thetaThresh = atof(optarg); break; 110 | case 'm': opts.params.magThresh = atof(optarg); break; 111 | case 'V': opts.params.adaptiveThresholdValue = atof(optarg); break; 112 | case 'N': opts.params.adaptiveThresholdRadius = atoi(optarg); break; 113 | case 'b': opts.params.refineBad = true; break; 114 | case 'r': opts.params.refineQuads = true; break; 115 | case 'n': opts.params.newQuadAlgorithm = true; break; 116 | case 'f': opts.family_str = optarg; break; 117 | case 'e': opts.error_fraction = atof(optarg); break; 118 | case 'd': opts.device_num = atoi(optarg); break; 119 | case 'F': opts.focal_length = atof(optarg); break; 120 | case 'z': opts.tag_size = atof(optarg); break; 121 | case 'W': opts.frame_width = atoi(optarg); break; 122 | case 'H': opts.frame_height = atoi(optarg); break; 123 | case 'M': opts.mirror_display = !opts.mirror_display; break; 124 | default: 125 | fprintf(stderr, "\n"); 126 | print_usage(argv[0], stderr); 127 | exit(1); 128 | } 129 | } 130 | opts.params.adaptiveThresholdRadius += (opts.params.adaptiveThresholdRadius+1) % 2; 131 | return opts; 132 | } 133 | 134 | int main(int argc, char** argv) { 135 | 136 | CamTestOptions opts = parse_options(argc, argv); 137 | 138 | TagFamily family(opts.family_str); 139 | 140 | if (opts.error_fraction >= 0 && opts.error_fraction <= 1) { 141 | family.setErrorRecoveryFraction(opts.error_fraction); 142 | } 143 | 144 | std::cout << "family.minimumHammingDistance = " << family.minimumHammingDistance << "\n"; 145 | std::cout << "family.errorRecoveryBits = " << family.errorRecoveryBits << "\n"; 146 | 147 | 148 | cv::VideoCapture vc; 149 | vc.open(opts.device_num); 150 | 151 | if (opts.frame_width && opts.frame_height) { 152 | 153 | // Use uvcdynctrl to figure this out dynamically at some point? 154 | vc.set(CV_CAP_PROP_FRAME_WIDTH, opts.frame_width); 155 | vc.set(CV_CAP_PROP_FRAME_HEIGHT, opts.frame_height); 156 | 157 | 158 | } 159 | 160 | std::cout << "Set camera to resolution: " 161 | << vc.get(CV_CAP_PROP_FRAME_WIDTH) << "x" 162 | << vc.get(CV_CAP_PROP_FRAME_HEIGHT) << "\n"; 163 | 164 | cv::Mat frame; 165 | cv::Point2d opticalCenter; 166 | 167 | vc >> frame; 168 | if (frame.empty()) { 169 | std::cerr << "no frames!\n"; 170 | exit(1); 171 | } 172 | 173 | opticalCenter.x = frame.cols * 0.5; 174 | opticalCenter.y = frame.rows * 0.5; 175 | 176 | std::string win = "Cam tag test"; 177 | 178 | TagDetectorParams& params = opts.params; 179 | TagDetector detector(family, params); 180 | 181 | TagDetectionArray detections; 182 | 183 | int cvPose = 0; 184 | 185 | while (1) { 186 | 187 | vc >> frame; 188 | if (frame.empty()) { break; } 189 | 190 | detector.process(frame, opticalCenter, detections); 191 | 192 | cv::Mat show; 193 | if (detections.empty()) { 194 | 195 | show = frame; 196 | 197 | } else { 198 | 199 | //show = family.superimposeDetections(frame, detections); 200 | show = frame; 201 | 202 | double s = opts.tag_size; 203 | double ss = 0.5*s; 204 | double sz = s; 205 | 206 | enum { npoints = 8, nedges = 12 }; 207 | 208 | cv::Point3d src[npoints] = { 209 | cv::Point3d(-ss, -ss, 0), 210 | cv::Point3d( ss, -ss, 0), 211 | cv::Point3d( ss, ss, 0), 212 | cv::Point3d(-ss, ss, 0), 213 | cv::Point3d(-ss, -ss, sz), 214 | cv::Point3d( ss, -ss, sz), 215 | cv::Point3d( ss, ss, sz), 216 | cv::Point3d(-ss, ss, sz), 217 | }; 218 | 219 | int edges[nedges][2] = { 220 | 221 | { 0, 1 }, 222 | { 1, 2 }, 223 | { 2, 3 }, 224 | { 3, 0 }, 225 | 226 | { 4, 5 }, 227 | { 5, 6 }, 228 | { 6, 7 }, 229 | { 7, 4 }, 230 | 231 | { 0, 4 }, 232 | { 1, 5 }, 233 | { 2, 6 }, 234 | { 3, 7 } 235 | 236 | }; 237 | 238 | cv::Point2d dst[npoints]; 239 | 240 | double f = opts.focal_length; 241 | 242 | double K[9] = { 243 | f, 0, opticalCenter.x, 244 | 0, f, opticalCenter.y, 245 | 0, 0, 1 246 | }; 247 | 248 | cv::Mat_ srcmat(npoints, 1, src); 249 | cv::Mat_ dstmat(npoints, 1, dst); 250 | 251 | cv::Mat_ Kmat(3, 3, K); 252 | 253 | cv::Mat_ distCoeffs = cv::Mat_::zeros(4,1); 254 | 255 | for (size_t i=0; i M = 272 | CameraUtil::homographyToPose(f, f, s, 273 | detections[i].homography, 274 | false); 275 | 276 | cv::Mat_ R = M.rowRange(0,3).colRange(0, 3); 277 | 278 | t = M.rowRange(0,3).col(3); 279 | 280 | cv::Rodrigues(R, r); 281 | 282 | } 283 | 284 | cv::projectPoints(srcmat, r, t, Kmat, distCoeffs, dstmat); 285 | 286 | for (int j=0; j 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #include "TagDetector.h" 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include "CameraUtil.h" 15 | #include 16 | #include 17 | #include 18 | 19 | #ifdef __APPLE__ 20 | #include 21 | #else 22 | #include 23 | #endif 24 | 25 | #define DEFAULT_TAG_FAMILY "Tag36h11" 26 | 27 | typedef struct GLTestOptions { 28 | GLTestOptions() : 29 | params(), 30 | family_str(DEFAULT_TAG_FAMILY), 31 | error_fraction(1), 32 | device_num(0), 33 | focal_length(500), 34 | tag_size(0.1905), 35 | frame_width(0), 36 | frame_height(0), 37 | mirror_display(true) 38 | { 39 | } 40 | TagDetectorParams params; 41 | std::string family_str; 42 | double error_fraction; 43 | int device_num; 44 | double focal_length; 45 | double tag_size; 46 | int frame_width; 47 | int frame_height; 48 | bool mirror_display; 49 | } GLTestOptions; 50 | 51 | 52 | void print_usage(const char* tool_name, FILE* output=stderr) { 53 | 54 | TagDetectorParams p; 55 | GLTestOptions o; 56 | 57 | fprintf(output, "\ 58 | Usage: %s [OPTIONS]\n\ 59 | Run a tool to test tag detection. Options:\n\ 60 | -h Show this help message.\n\ 61 | -D Use decimation for segmentation stage.\n\ 62 | -S SIGMA Set the original image sigma value (default %.2f).\n\ 63 | -s SEGSIGMA Set the segmentation sigma value (default %.2f).\n\ 64 | -a THETATHRESH Set the theta threshold for clustering (default %.1f).\n\ 65 | -m MAGTHRESH Set the magnitude threshold for clustering (default %.1f).\n\ 66 | -V VALUE Set adaptive threshold value for new quad algo (default %f).\n\ 67 | -N RADIUS Set adaptive threshold radius for new quad algo (default %d).\n\ 68 | -b Refine bad quads using template tracker.\n\ 69 | -r Refine all quads using template tracker.\n\ 70 | -n Use the new quad detection algorithm.\n\ 71 | -f FAMILY Look for the given tag family (default \"%s\")\n\ 72 | -e FRACTION Set error detection fraction (default %f)\n\ 73 | -d DEVICE Set camera device number (default %d)\n\ 74 | -F FLENGTH Set the camera's focal length in pixels (default %f)\n\ 75 | -z SIZE Set the tag size in meters (default %f)\n\ 76 | -W WIDTH Set the camera image width in pixels\n\ 77 | -H HEIGHT Set the camera image height in pixels\n\ 78 | -M Toggle display mirroring\n", 79 | tool_name, 80 | p.sigma, 81 | p.segSigma, 82 | p.thetaThresh, 83 | p.magThresh, 84 | p.adaptiveThresholdValue, 85 | p.adaptiveThresholdRadius, 86 | DEFAULT_TAG_FAMILY, 87 | o.error_fraction, 88 | o.device_num, 89 | o.focal_length, 90 | o.tag_size); 91 | 92 | 93 | fprintf(output, "Known tag families:"); 94 | TagFamily::StringArray known = TagFamily::families(); 95 | for (size_t i = 0; i < known.size(); ++i) { 96 | fprintf(output, " %s", known[i].c_str()); 97 | } 98 | fprintf(output, "\n"); 99 | } 100 | 101 | GLTestOptions parse_options(int argc, char** argv) { 102 | GLTestOptions opts; 103 | const char* options_str = "hDS:s:a:m:V:N:brnf:e:d:F:z:W:H:M"; 104 | int c; 105 | while ((c = getopt(argc, argv, options_str)) != -1) { 106 | switch (c) { 107 | // Reminder: add new options to 'options_str' above and print_usage()! 108 | case 'h': print_usage(argv[0], stdout); exit(0); break; 109 | case 'D': opts.params.segDecimate = true; break; 110 | case 'S': opts.params.sigma = atof(optarg); break; 111 | case 's': opts.params.segSigma = atof(optarg); break; 112 | case 'a': opts.params.thetaThresh = atof(optarg); break; 113 | case 'm': opts.params.magThresh = atof(optarg); break; 114 | case 'V': opts.params.adaptiveThresholdValue = atof(optarg); break; 115 | case 'N': opts.params.adaptiveThresholdRadius = atoi(optarg); break; 116 | case 'b': opts.params.refineBad = true; break; 117 | case 'r': opts.params.refineQuads = true; break; 118 | case 'n': opts.params.newQuadAlgorithm = true; break; 119 | case 'f': opts.family_str = optarg; break; 120 | case 'e': opts.error_fraction = atof(optarg); break; 121 | case 'd': opts.device_num = atoi(optarg); break; 122 | case 'F': opts.focal_length = atof(optarg); break; 123 | case 'z': opts.tag_size = atof(optarg); break; 124 | case 'W': opts.frame_width = atoi(optarg); break; 125 | case 'H': opts.frame_height = atoi(optarg); break; 126 | case 'M': opts.mirror_display = !opts.mirror_display; break; 127 | default: 128 | fprintf(stderr, "\n"); 129 | print_usage(argv[0], stderr); 130 | exit(1); 131 | } 132 | } 133 | opts.params.adaptiveThresholdRadius += (opts.params.adaptiveThresholdRadius+1) % 2; 134 | return opts; 135 | } 136 | 137 | 138 | void check_opengl_errors(const char* context) { 139 | GLenum error = glGetError(); 140 | if (!context) { context = "error"; } 141 | if (error) { 142 | std::cerr << context << ": " << gluErrorString(error) << "\n"; 143 | } 144 | } 145 | 146 | 147 | 148 | cv::VideoCapture* capture = 0; 149 | 150 | GLuint camera_texture; 151 | std::map tag_textures; 152 | 153 | cv::Mat frame; 154 | cv::Mat_ uframe; 155 | std::vector rgbbuf; 156 | 157 | GLTestOptions opts; 158 | 159 | TagDetector* detector = 0; 160 | TagFamily family; 161 | TagDetectionArray detections; 162 | 163 | int width = 640; 164 | int height = 480; 165 | 166 | //double f = 500; 167 | //double s = 0.01; 168 | 169 | // wb = 1, bb = 1, offs = 1.5/d 170 | // wb = 2, bb = 1, offs = 3/d 171 | 172 | //int n = 0;//family.getTagRenderDimension(); 173 | //double 174 | 175 | void init() { 176 | 177 | glMatrixMode(GL_PROJECTION); 178 | glLoadIdentity(); 179 | 180 | glMatrixMode(GL_MODELVIEW); 181 | glLoadIdentity(); 182 | 183 | glLineWidth(2.0); 184 | glEnable(GL_LINE_SMOOTH); 185 | 186 | glGenTextures(1, &camera_texture); 187 | glBindTexture(GL_TEXTURE_2D, camera_texture); 188 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); 189 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); 190 | 191 | check_opengl_errors("init"); 192 | 193 | } 194 | 195 | void drawCube() { 196 | 197 | 198 | double ss = 0.5*opts.tag_size; 199 | double sz = opts.tag_size; 200 | 201 | enum { npoints = 8, nedges = 12 }; 202 | 203 | cv::Point3d src[npoints] = { 204 | cv::Point3d(-ss, -ss, 0), 205 | cv::Point3d( ss, -ss, 0), 206 | cv::Point3d( ss, ss, 0), 207 | cv::Point3d(-ss, ss, 0), 208 | cv::Point3d(-ss, -ss, sz), 209 | cv::Point3d( ss, -ss, sz), 210 | cv::Point3d( ss, ss, sz), 211 | cv::Point3d(-ss, ss, sz), 212 | }; 213 | 214 | int edges[nedges][2] = { 215 | 216 | { 0, 1 }, 217 | { 1, 2 }, 218 | { 2, 3 }, 219 | { 3, 0 }, 220 | 221 | { 4, 5 }, 222 | { 5, 6 }, 223 | { 6, 7 }, 224 | { 7, 4 }, 225 | 226 | { 0, 4 }, 227 | { 1, 5 }, 228 | { 2, 6 }, 229 | { 3, 7 } 230 | 231 | }; 232 | 233 | glBegin(GL_LINES); 234 | 235 | for (int i=0; i::const_iterator i = tag_textures.find(id); 270 | 271 | if (i != tag_textures.end()) { 272 | 273 | glBindTexture(GL_TEXTURE_2D, i->second); 274 | 275 | } else { 276 | 277 | GLuint tex; 278 | glGenTextures(1, &tex); 279 | glBindTexture(GL_TEXTURE_2D, tex); 280 | 281 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); 282 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); 283 | 284 | tag_textures[id] = tex; 285 | 286 | cv::Mat_ img = family.makeImage(id); 287 | rgbbuf.resize(img.rows*img.cols*4); 288 | 289 | int offs=0; 290 | for (int y=0; y r, R, t, M = cv::Mat_::eye(4,4); 372 | CameraUtil::homographyToPoseCV(opts.focal_length, opts.focal_length, 373 | opts.tag_size, 374 | detections[i].homography, 375 | r, t); 376 | cv::Rodrigues(r, R); 377 | 378 | for (int i=0; i<3; ++i) { 379 | for (int j=0; j<3; ++j) { 380 | M(i,j) = R(j,i); 381 | } 382 | M(3,i) = t(i); 383 | } 384 | 385 | glPushMatrix(); 386 | glMultMatrixd(&(M(0,0))); 387 | drawCube(); 388 | glPopMatrix(); 389 | 390 | } 391 | 392 | glMatrixMode(GL_PROJECTION); 393 | glPopMatrix(); 394 | 395 | glutSwapBuffers(); 396 | 397 | check_opengl_errors("display"); 398 | 399 | 400 | } 401 | 402 | void reshape(int w, int h) { 403 | 404 | width = w; 405 | height = h; 406 | 407 | glClearColor(1,1,1,1); 408 | glViewport(0,0,w,h); 409 | 410 | check_opengl_errors("reshape"); 411 | 412 | } 413 | 414 | void keyboard(unsigned char value, int x, int y) { 415 | 416 | switch (value) { 417 | case 27: // esc 418 | detector->reportTimers(); 419 | exit(1); 420 | break; 421 | case 'f': 422 | case 'F': 423 | std::cout << "flip!\n"; 424 | opts.mirror_display = !opts.mirror_display; 425 | break; 426 | } 427 | 428 | 429 | } 430 | 431 | void idle() { 432 | 433 | *capture >> frame; 434 | 435 | if (frame.type() != CV_8UC3) { 436 | std::cerr << "bad frame!\n"; 437 | exit(1); 438 | } 439 | 440 | cv::Point2d opticalCenter(0.5*frame.cols, 0.5*frame.rows); 441 | detector->process(frame, opticalCenter, detections); 442 | //std::cout << "got " << detections.size() << " detections.\n"; 443 | 444 | uframe = frame; 445 | 446 | 447 | rgbbuf.resize(frame.cols*frame.rows*3); 448 | 449 | int offs = 0; 450 | 451 | for (int y=0; y= 0 && opts.error_fraction <= 1) { 482 | family.setErrorRecoveryFraction(opts.error_fraction); 483 | } 484 | 485 | capture = new cv::VideoCapture(opts.device_num); 486 | 487 | if (opts.frame_width && opts.frame_height) { 488 | 489 | // Use uvcdynctrl to figure this out dynamically at some point? 490 | capture->set(CV_CAP_PROP_FRAME_WIDTH, opts.frame_width); 491 | capture->set(CV_CAP_PROP_FRAME_HEIGHT, opts.frame_height); 492 | 493 | 494 | } 495 | 496 | cv::Mat frame; 497 | *capture >> frame; 498 | 499 | width = frame.cols; 500 | height = frame.rows; 501 | 502 | std::cout << "Set camera to resolution: " 503 | << width << "x" 504 | << height << "\n"; 505 | 506 | detector = new TagDetector(family, opts.params); 507 | 508 | glutInitDisplayMode(GLUT_DOUBLE | GLUT_DEPTH | 509 | GLUT_RGB | GLUT_MULTISAMPLE); 510 | 511 | glutInitWindowPosition(100, 100); 512 | glutInitWindowSize(width, height); 513 | 514 | glutCreateWindow("AprilTags GL Demo"); 515 | 516 | glutDisplayFunc(display); 517 | glutReshapeFunc(reshape); 518 | glutKeyboardFunc(keyboard); 519 | glutIdleFunc(idle); 520 | 521 | 522 | init(); 523 | glutMainLoop(); 524 | 525 | return 0; 526 | 527 | } 528 | -------------------------------------------------------------------------------- /src/maketags.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * This file is distributed as part of the C++ port of the APRIL tags 3 | * library. The code is licensed under GPLv2. 4 | * 5 | * Original author: Edwin Olson 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #include "TagFamily.h" 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | struct Unit { 20 | std::string name; 21 | double points; 22 | }; 23 | 24 | enum { 25 | num_units = 5 26 | }; 27 | 28 | static const Unit units[num_units] = { 29 | { "pt", 1 }, 30 | { "in", 72 }, 31 | { "ft", 72*12 }, 32 | { "cm", 28.346456693 }, 33 | { "mm", 2.8346456693 }, 34 | }; 35 | 36 | static int last_unit = 0; 37 | 38 | struct PaperSize { 39 | std::string name; 40 | double width_points; 41 | double height_points; 42 | }; 43 | 44 | static const PaperSize papers[] = { 45 | { "letter", 612, 792 }, 46 | { "a6", 297.5, 419.6 }, 47 | { "a5", 419.3, 595.4 }, 48 | { "a4", 595, 842 }, 49 | { "a3", 841.5, 1190.7 }, 50 | { "a2", 1190, 1684 }, 51 | { "a1", 1683, 2384.2 }, 52 | { "a0", 2382.8, 3370.8 }, 53 | { "", 0 } 54 | }; 55 | 56 | int lookup_unit(const std::string& label) { 57 | 58 | for (int i=0; i>rval)) { 76 | std::cerr << "error: expected number\n"; 77 | exit(1); 78 | } 79 | 80 | if (istr.eof()) { 81 | return rval; 82 | } 83 | 84 | int i = istr.peek(); 85 | 86 | while (isspace(i)) { 87 | istr.get(); 88 | i = istr.peek(); 89 | } 90 | 91 | if (isalpha(i)) { 92 | std::string label; 93 | istr >> label; 94 | int i = lookup_unit(label); 95 | rval *= units[i].points; 96 | } 97 | 98 | return rval; 99 | 100 | }; 101 | 102 | double parse_unit(const std::string& str) { 103 | std::istringstream istr(str); 104 | return parse_unit(istr); 105 | } 106 | 107 | template 108 | Tval parse_value(const std::string& str) { 109 | std::istringstream istr(str); 110 | Tval rval; 111 | if (!(istr >> rval) || istr.peek() != EOF) { 112 | std::cerr << "error parsing value\n"; 113 | exit(1); 114 | } 115 | return rval; 116 | } 117 | 118 | void usage(std::ostream& ostr) { 119 | 120 | ostr << "usage: maketags FAMILY [OPTIONS]\n\n" 121 | << " Known tag families:"; 122 | 123 | TagFamily::StringArray known = TagFamily::families(); 124 | for (size_t i=0; i ids; 196 | 197 | if (std::string(argv[1]) == "--help") { 198 | usage(std::cout); 199 | exit(0); 200 | } 201 | 202 | TagFamily family(argv[1]); 203 | 204 | for (int i=2; i(argv[++i]); 234 | } else if (optarg == "--labelfmt") { 235 | label_fmt = argv[++i]; 236 | } else if (optarg == "--tagsize") { 237 | size = parse_unit(argv[++i]); 238 | stype = SizeFull; 239 | label_unit = last_unit; 240 | } else if (optarg == "--innersize") { 241 | size = parse_unit(argv[++i]); 242 | stype = SizeInner; 243 | label_unit = last_unit; 244 | } else if (optarg == "--cropmark") { 245 | do_crop = true; 246 | crop_x = parse_unit(argv[++i]); 247 | crop_y = parse_unit(argv[++i]); 248 | crop_l = parse_unit(argv[++i]); 249 | crop_gray = parse_value(argv[++i]); 250 | } else if (optarg == "--padding") { 251 | padding = parse_unit(argv[++i]); 252 | } else if (optarg == "--tagfrac") { 253 | tagfrac = parse_value(argv[++i]); 254 | stype = SizeFrac; 255 | if (tagfrac < 1) { 256 | std::cerr << "tag fraction must be greater than one \n"; 257 | exit(1); 258 | } 259 | } else if (optarg == "--id") { 260 | ids.push_back(parse_value(argv[++i])); 261 | } else if (optarg == "--idrange") { 262 | size_t start = parse_value(argv[++i]); 263 | size_t end = parse_value(argv[++i]); 264 | for (size_t i=start; i<=end; ++i) { 265 | ids.push_back(i); 266 | } 267 | } else if (optarg == "--outfile") { 268 | outfile = argv[++i]; 269 | } else if (optarg == "--help") { 270 | usage(std::cout); 271 | exit(0); 272 | } else { 273 | std::cerr << "unrecognized option: " << optarg << "\n"; 274 | exit(1); 275 | } 276 | } 277 | 278 | if (label_unit < 0) { label_unit = lookup_unit("mm"); } 279 | 280 | int tags_per_row = 0; 281 | double sq_size = 0; 282 | 283 | if (width <= margins[ML] + margins[MR]) { 284 | std::cerr << "width must be greater than L+R margins\n"; 285 | exit(1); 286 | } else if (height <= margins[MT] + margins[MB]) { 287 | std::cerr << "height must be greater than T+B margins\n"; 288 | exit(1); 289 | } 290 | 291 | double orig_width = width; 292 | double orig_height = height; 293 | 294 | width -= margins[ML] + margins[MR]; 295 | height -= margins[MT] + margins[MB]; 296 | 297 | int rd = family.getTagRenderDimension(); 298 | int id = rd - 2*family.whiteBorder; 299 | 300 | if (stype == SizeInner) { 301 | sq_size = size; 302 | size = sq_size * double(rd)/id; 303 | } else if (stype == SizeFrac) { 304 | size = (width - (tagfrac-1)*padding) / tagfrac; 305 | tags_per_row = int(tagfrac); 306 | } 307 | 308 | if (!sq_size) { sq_size = size * double(id)/rd; } 309 | 310 | if (!tags_per_row) { 311 | 312 | if (size > width) { 313 | std::cerr << "tag size wider than page!\n"; 314 | } 315 | tags_per_row = 1 + (width - size) / (padding + size); 316 | 317 | } 318 | 319 | double px_size = size / rd; 320 | 321 | double row_size = size; 322 | 323 | if (font_size == 0) { 324 | font_size = px_size; 325 | } 326 | 327 | if (draw_labels) { 328 | row_size += font_size; 329 | } 330 | 331 | if (row_size > height) { 332 | std::cerr << "tag size taller than page!\n"; 333 | exit(1); 334 | } 335 | 336 | int rows_per_page = 1 + int((height - row_size) / (padding + row_size)); 337 | 338 | int tags_per_page = tags_per_row * rows_per_page; 339 | 340 | std::vector tmp; 341 | for (size_t i=0; i= lstr.length()) { break; } 495 | 496 | size_t l = pos2-pos + 1; 497 | 498 | char buf[1024]; 499 | 500 | if (tolower(lstr[pos2]) == 'f') { 501 | lstr.replace(pos, l, argv[1]); 502 | } else if (tolower(lstr[pos2]) == 'i' || 503 | tolower(lstr[pos2]) == 't' || 504 | tolower(lstr[pos2]) == 'p') { 505 | double meas; 506 | switch (tolower(lstr[pos2])) { 507 | case 'i': 508 | meas = sq_size; 509 | break; 510 | case 't': 511 | meas = size; 512 | break; 513 | default: 514 | meas = px_size; 515 | break; 516 | } 517 | meas /= units[label_unit].points; 518 | snprintf(buf, 1024, "%g %s", meas, units[label_unit].name.c_str()); 519 | lstr.replace(pos, l, buf); 520 | } else if (tolower(lstr[pos2]) == 'd') { 521 | snprintf(buf, 1024, lstr.substr(pos,l).c_str(), id); 522 | lstr.replace(pos, l, buf); 523 | } 524 | 525 | start = pos2; 526 | 527 | } 528 | 529 | 530 | 531 | 532 | 533 | cairo_text_extents_t extents; 534 | cairo_text_extents (cr, lstr.c_str(), &extents); 535 | 536 | double tx = 0.5 * (sq_size - extents.width); 537 | cairo_move_to(cr, x + tx, y + sq_size + px_size * wb + font_size); 538 | cairo_show_text(cr, lstr.c_str()); 539 | 540 | } 541 | 542 | /* 543 | cairo_rectangle(cr, x, y + sq_size + px_size * wb, sq_size, font_size); 544 | cairo_fill(cr); 545 | */ 546 | 547 | cv::Mat_ m = family.makeImage(ids[i]); 548 | 549 | cairo_set_source_rgb(cr, 1, 1, 1); 550 | 551 | for (size_t i=0; i= tags_per_row) { 569 | col = 0; 570 | ++row; 571 | if (row >= rows_per_page) { 572 | row = 0; 573 | newpage = true; 574 | } 575 | } 576 | 577 | } 578 | 579 | cairo_show_page(cr); 580 | cairo_surface_destroy(surface); 581 | cairo_destroy(cr); 582 | 583 | return 0; 584 | 585 | 586 | } 587 | -------------------------------------------------------------------------------- /src/quadtest.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * This file is distributed as part of the C++ port of the APRIL tags 3 | * library. The code is licensed under GPLv2. 4 | * 5 | * Original author: Edwin Olson 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include "Geometry.h" 20 | #include "TagDetector.h" 21 | #include "Refine.h" 22 | 23 | enum { 24 | test_img_w = 30, 25 | test_img_h = 30, 26 | test_img_tagsz = 12, 27 | test_img_scl = 8 28 | }; 29 | 30 | cv::Mat shrink(const cv::Mat& image, int scl) { 31 | cv::Mat small; 32 | cv::resize(image, small, cv::Size(image.cols/scl, image.rows/scl), 0, 0, 33 | CV_INTER_AREA); 34 | return small; 35 | } 36 | 37 | cv::Mat addNoise(const cv::Mat& src, cv::RNG& rng, double stddev) { 38 | 39 | cv::Mat input; 40 | 41 | if (src.depth() != at::REAL_IMAGE_TYPE) { 42 | src.convertTo(input, CV_MAKETYPE(at::REAL_IMAGE_TYPE, src.channels())); 43 | } else { 44 | input = src.clone(); 45 | } 46 | 47 | std::vector chans; 48 | 49 | cv::split(input, chans); 50 | 51 | for (size_t c=0; c 1 && argv[1] == std::string("-d")) { 146 | debug = true; 147 | } 148 | 149 | cv::RNG rng(12345); 150 | 151 | TagFamily tagFamily("Tag36h11"); 152 | 153 | int dd = tagFamily.d + 2 * tagFamily.blackBorder; 154 | 155 | TPointArray tpoints; 156 | 157 | for (int i=-1; i<=dd; ++i) { 158 | at::real fi = at::real(i+0.5) / dd; 159 | for (int j=-1; j<=dd; ++j) { 160 | at::real fj = at::real(j+0.5) / dd; 161 | at::real t = -1; 162 | if (i == -1 || j == -1 || i == dd || j == dd) { 163 | t = 255; 164 | } else if (i == 0 || j == 0 || i+1 == dd || j+1 == dd) { 165 | t = 0; 166 | } 167 | if (t >= 0) { 168 | tpoints.push_back(TPoint(fi, fj, t)); 169 | } 170 | } 171 | } 172 | 173 | int ntrials = 100; 174 | double total_time = 0; 175 | double total_err1 = 0; 176 | double total_err2 = 0; 177 | 178 | for (int trial=0; trial gsmall; 188 | cv::cvtColor(small, gsmall, CV_RGB2GRAY); 189 | 190 | at::Mat gx = at::Mat::zeros(gsmall.size()); 191 | at::Mat gy = at::Mat::zeros(gsmall.size()); 192 | 193 | /* 194 | for (int y=1; y= 0; iy--) { 264 | for (int ix = 0; ix < tagFamily.d; ix++) { 265 | 266 | at::real y = (tagFamily.blackBorder + iy + .5) / dd; 267 | at::real x = (tagFamily.blackBorder + ix + .5) / dd; 268 | 269 | at::Point uv(x, y); 270 | at::Point pi = interpolate(badp, uv); 271 | cv::Point ii = pi + at::Point(0.5, 0.5); 272 | 273 | int tagbit = img(tagFamily.d-iy-1+b, ix+b) ? 1 : 0; 274 | int imgbit = gbig(ii) > 127; 275 | 276 | tagCode = tagCode << TagFamily::code_t(1); 277 | if (imgbit) { 278 | tagCode |= TagFamily::code_t(1); 279 | } 280 | 281 | errors += (tagbit != imgbit); 282 | 283 | cv::Scalar tc = tagbit ? CV_RGB(255,0,0) : CV_RGB(0,0,255); 284 | cv::Scalar dc = imgbit ? CV_RGB(255,0,0) : CV_RGB(0,0,255); 285 | drawPoint(big, pi, tc, 5, CV_FILLED); 286 | drawPoint(big, pi, dc, 5, 2); 287 | 288 | } 289 | } 290 | 291 | */ 292 | 293 | -------------------------------------------------------------------------------- /src/tagtest.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * This file is distributed as part of the C++ port of the APRIL tags 3 | * library. The code is licensed under GPLv2. 4 | * 5 | * Original author: Edwin Olson 6 | * C++ port and modifications: Matt Zucker 7 | ********************************************************************/ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include "TagDetector.h" 17 | #include "DebugImage.h" 18 | 19 | #define DEFAULT_TAG_FAMILY "Tag36h11" 20 | 21 | typedef struct TagTestOptions { 22 | TagTestOptions() : 23 | show_debug_info(false), 24 | show_timing(false), 25 | show_results(false), 26 | be_verbose(false), 27 | no_images(false), 28 | generate_output_files(false), 29 | params(), 30 | family_str(DEFAULT_TAG_FAMILY), 31 | error_fraction(1){ 32 | } 33 | bool show_debug_info; 34 | bool show_timing; 35 | bool show_results; 36 | bool be_verbose; 37 | bool no_images; 38 | bool generate_output_files; 39 | TagDetectorParams params; 40 | std::string family_str; 41 | double error_fraction; 42 | } TagTestOptions; 43 | 44 | 45 | void print_usage(const char* tool_name, FILE* output=stderr) { 46 | 47 | TagDetectorParams p; 48 | 49 | fprintf(output, "\ 50 | Usage: %s [OPTIONS] IMAGE1 [IMAGE2 ...]\n\ 51 | Run a tool to test tag detection. Options:\n\ 52 | -h Show this help message.\n\ 53 | -d Show debug images and data during tag detection.\n\ 54 | -t Show timing information for tag detection.\n\ 55 | -R Show textual results of tag detection.\n\ 56 | -v Be verbose (includes -d -t -R).\n\ 57 | -x Do not generate any non-debug visuals.\n\ 58 | -o Generate debug visuals as output files vs. using X11.\n\ 59 | -D Use decimation for segmentation stage.\n\ 60 | -S SIGMA Set the original image sigma value (default %.2f).\n\ 61 | -s SEGSIGMA Set the segmentation sigma value (default %.2f).\n\ 62 | -a THETATHRESH Set the theta threshold for clustering (default %.1f).\n\ 63 | -m MAGTHRESH Set the magnitude threshold for clustering (default %.1f).\n\ 64 | -V VALUE Set adaptive threshold value for new quad algo (default %f).\n\ 65 | -N RADIUS Set adaptive threshold radius for new quad algo (default %d).\n\ 66 | -b Refine bad quads using template tracker.\n\ 67 | -r Refine all quads using template tracker.\n\ 68 | -n Use the new quad detection algorithm.\n\ 69 | -f FAMILY Look for the given tag family (default \"%s\")\n\ 70 | -e FRACTION Set error detection fraction (default 1)\n", 71 | tool_name, 72 | p.sigma, 73 | p.segSigma, 74 | p.thetaThresh, 75 | p.magThresh, 76 | p.adaptiveThresholdValue, 77 | p.adaptiveThresholdRadius, 78 | DEFAULT_TAG_FAMILY); 79 | 80 | fprintf(output, "Known tag families:"); 81 | TagFamily::StringArray known = TagFamily::families(); 82 | for (size_t i = 0; i < known.size(); ++i) { 83 | fprintf(output, " %s", known[i].c_str()); 84 | } 85 | fprintf(output, "\n"); 86 | } 87 | 88 | TagTestOptions parse_options(int argc, char** argv) { 89 | TagTestOptions opts; 90 | const char* options_str = "hdtRvxoDS:s:a:m:V:N:brnf:e:"; 91 | int c; 92 | while ((c = getopt(argc, argv, options_str)) != -1) { 93 | switch (c) { 94 | // Reminder: add new options to 'options_str' above and print_usage()! 95 | case 'h': print_usage(argv[0], stdout); exit(0); break; 96 | case 'd': opts.show_debug_info = true; break; 97 | case 't': opts.show_timing = true; break; 98 | case 'R': opts.show_results = true; break; 99 | case 'v': opts.be_verbose = true; break; 100 | case 'x': opts.no_images = true; break; 101 | case 'o': opts.generate_output_files = true; break; 102 | case 'D': opts.params.segDecimate = true; break; 103 | case 'S': opts.params.sigma = atof(optarg); break; 104 | case 's': opts.params.segSigma = atof(optarg); break; 105 | case 'a': opts.params.thetaThresh = atof(optarg); break; 106 | case 'm': opts.params.magThresh = atof(optarg); break; 107 | case 'V': opts.params.adaptiveThresholdValue = atof(optarg); break; 108 | case 'N': opts.params.adaptiveThresholdRadius = atoi(optarg); break; 109 | case 'b': opts.params.refineBad = true; break; 110 | case 'r': opts.params.refineQuads = true; break; 111 | case 'n': opts.params.newQuadAlgorithm = true; break; 112 | case 'f': opts.family_str = optarg; break; 113 | case 'e': opts.error_fraction = atof(optarg); break; 114 | default: 115 | fprintf(stderr, "\n"); 116 | print_usage(argv[0], stderr); 117 | exit(1); 118 | } 119 | } 120 | opts.params.adaptiveThresholdRadius += (opts.params.adaptiveThresholdRadius+1) % 2; 121 | if (opts.be_verbose) { 122 | opts.show_debug_info = opts.show_timing = opts.show_results = true; 123 | } 124 | return opts; 125 | } 126 | 127 | int main(int argc, char** argv) { 128 | 129 | const std::string win = "Tag test"; 130 | 131 | TagTestOptions opts = parse_options(argc, argv); 132 | 133 | TagFamily family(opts.family_str); 134 | 135 | if (opts.error_fraction >= 0 && opts.error_fraction < 1) { 136 | family.setErrorRecoveryFraction(opts.error_fraction); 137 | } 138 | 139 | TagDetector detector(family, opts.params); 140 | detector.debug = opts.show_debug_info; 141 | detector.debugWindowName = opts.generate_output_files ? "" : win; 142 | if (opts.params.segDecimate && opts.be_verbose) { 143 | std::cout << "Will decimate for segmentation!\n"; 144 | } 145 | 146 | TagDetectionArray detections; 147 | for (int i=optind; i 800) { 152 | cv::Mat tmp; 153 | cv::resize(src, tmp, cv::Size(0,0), 0.5, 0.5); 154 | src = tmp; 155 | } 156 | cv::Point2d opticalCenter(0.5*src.rows, 0.5*src.cols); 157 | 158 | if (!detector.debug && !opts.no_images) { 159 | labelAndWaitForKey(win, "Original", src, ScaleNone, true); 160 | } 161 | 162 | clock_t start = clock(); 163 | detector.process(src, opticalCenter, detections); 164 | clock_t end = clock(); 165 | 166 | if (opts.show_results) { 167 | if (opts.show_debug_info) std::cout << "\n"; 168 | std::cout << "Got " << detections.size() << " detections in " 169 | << double(end-start)/CLOCKS_PER_SEC << " seconds.\n"; 170 | for (size_t i=0; i