pi/ar_basalt
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

v01

Browse Source
This commit is contained in:
Ivan
2022-04-05 11:42:28 +03:00
commit 6dc0eb0fcf
5565 changed files with 1200500 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
thirdparty/apriltag/CMakeLists.txt vendored Normal file
View File

@@ -0,0 +1,25 @@
cmake_minimum_required(VERSION 3.2...3.18)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC")
SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC")
#file(GLOB APRILTAG_SRCS "${CMAKE_CURRENT_SOURCE_DIR}/apriltag-2016-12-01/*.c" "${CMAKE_CURRENT_SOURCE_DIR}/apriltag-2016-12-01/common/*.c")
#include_directories(apriltag-2016-12-01)
include_directories(${OpenCV_INCLUDE_DIR})
file(GLOB APRILTAG_SRCS "ethz_apriltag2/src/*.cc")
include_directories(ethz_apriltag2/include)
include_directories(../../include)
include_directories(../basalt-headers/include)
include_directories(../basalt-headers/thirdparty/Sophus)
add_library(apriltag STATIC ${APRILTAG_SRCS} src/apriltag.cpp)
target_include_directories(apriltag PUBLIC include)
target_link_libraries(apriltag PUBLIC basalt::opencv Sophus::Sophus)

35
thirdparty/apriltag/ethz_apriltag2/CMakeLists.txt vendored Normal file
View File

@@ -0,0 +1,35 @@
cmake_minimum_required(VERSION 2.8)
project(ethz_apriltag2)
find_package(catkin REQUIRED COMPONENTS cmake_modules)
include_directories(${catkin_INCLUDE_DIRS})
find_package(Eigen REQUIRED)
catkin_package(
DEPENDS eigen opencv
INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS}
LIBRARIES ${PROJECT_NAME}
)
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake/)
find_package(Eigen REQUIRED)
find_package(OpenCV REQUIRED)
add_definitions(-fPIC -O3)
include_directories(include ${Eigen_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
#library
file(GLOB SOURCE_FILES "src/*.cc")
add_library(${PROJECT_NAME} ${SOURCE_FILES})
target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS} ${catkin_LIBRARIES})
#demo
if(NOT APPLE)
add_executable(apriltags_demo src/example/apriltags_demo.cpp src/example/Serial.cpp)
target_link_libraries(apriltags_demo ${PROJECT_NAME} v4l2)
endif()

34
thirdparty/apriltag/ethz_apriltag2/LICENSE vendored Normal file
View File

@@ -0,0 +1,34 @@
Copyright (c) 2014, Paul Furgale, Jérôme Maye and Jörn Rehder, Autonomous Systems Lab,
ETH Zurich, Switzerland
Copyright (c) 2014, Thomas Schneider, Skybotix AG, Switzerland
All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
All advertising materials mentioning features or use of this software must
display the following acknowledgement: This product includes software developed
by the Autonomous Systems Lab and Skybotix AG.
Neither the name of the Autonomous Systems Lab and Skybotix AG nor the names
of its contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE AUTONOMOUS SYSTEMS LAB AND SKYBOTIX AG ''AS IS''
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
IN NO EVENT SHALL the AUTONOMOUS SYSTEMS LAB OR SKYBOTIX AG BE LIABLE FOR ANY DIRECT,
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
OF SUCH DAMAGE.

59
thirdparty/apriltag/ethz_apriltag2/include/apriltags/Edge.h vendored Normal file
View File

@@ -0,0 +1,59 @@
#ifndef EDGE_H
#define EDGE_H
#include <vector>
#include "apriltags/FloatImage.h"
namespace AprilTags {
class FloatImage;
class UnionFindSimple;
using std::min;
using std::max;
//! Represents an edge between adjacent pixels in the image.
/*! The edge is encoded by the indices of the two pixels. Edge cost
* is proportional to the difference in local orientations.
*/
class Edge {
public:
static float const minMag; //!< minimum intensity gradient for an edge to be recognized
static float const maxEdgeCost; //!< 30 degrees = maximum acceptable difference in local orientations
static int const WEIGHT_SCALE; // was 10000
static float const thetaThresh; //!< theta threshold for merging edges
static float const magThresh; //!< magnitude threshold for merging edges
int pixelIdxA;
int pixelIdxB;
int cost;
//! Constructor
Edge() : pixelIdxA(), pixelIdxB(), cost() {}
//! Compare edges based on cost
inline bool operator< (const Edge &other) const { return (cost < other.cost); }
//! Cost of an edge between two adjacent pixels; -1 if no edge here
/*! An edge exists between adjacent pixels if the magnitude of the
intensity gradient at both pixels is above threshold. The edge
cost is proportional to the difference in the local orientation at
the two pixels. Lower cost is better. A cost of -1 means there
is no edge here (intensity gradien fell below threshold).
*/
static int edgeCost(float theta0, float theta1, float mag1);
//! Calculates and inserts up to four edges into 'edges', a vector of Edges.
static void calcEdges(float theta0, int x, int y,
const FloatImage& theta, const FloatImage& mag,
std::vector<Edge> &edges, size_t &nEdges);
//! Process edges in order of increasing cost, merging clusters if we can do so without exceeding the thetaThresh.
static void mergeEdges(std::vector<Edge> &edges, UnionFindSimple &uf, float tmin[], float tmax[], float mmin[], float mmax[]);
};
} // namespace
#endif

64
thirdparty/apriltag/ethz_apriltag2/include/apriltags/FloatImage.h vendored Normal file
View File

@@ -0,0 +1,64 @@
#ifndef FLOATIMAGE_H
#define FLOATIMAGE_H
#include <algorithm>
#include <vector>
namespace DualCoding {
typedef unsigned char uchar;
template <typename T>
class Sketch;
} // namespace DualCoding
namespace AprilTags {
//! Represent an image as a vector of floats in [0,1]
class FloatImage {
private:
int width;
int height;
std::vector<float> pixels;
public:
//! Default constructor
FloatImage();
//! Construct an empty image
FloatImage(int widthArg, int heightArg);
//! Constructor that copies pixels from an array
FloatImage(int widthArg, int heightArg, const std::vector<float>& pArg);
FloatImage(const FloatImage& other) = default;
FloatImage& operator=(const FloatImage& other);
float get(int x, int y) const { return pixels[y * width + x]; }
void set(int x, int y, float v) { pixels[y * width + x] = v; }
int getWidth() const { return width; }
int getHeight() const { return height; }
int getNumFloatImagePixels() const { return width * height; }
const std::vector<float>& getFloatImagePixels() const { return pixels; }
//! TODO: Fix decimateAvg function. DO NOT USE!
void decimateAvg();
//! Rescale all values so that they are between [0,1]
void normalize();
void filterFactoredCentered(const std::vector<float>& fhoriz,
const std::vector<float>& fvert);
template <typename T>
void copyToSketch(DualCoding::Sketch<T>& sketch) {
for (int i = 0; i < getNumFloatImagePixels(); i++)
sketch[i] = (T)(255.0f * getFloatImagePixels()[i]);
}
void printMinMax() const;
};
} // namespace AprilTags
#endif

74
thirdparty/apriltag/ethz_apriltag2/include/apriltags/GLine2D.h vendored Normal file
View File

@@ -0,0 +1,74 @@
#ifndef GLINE2D_H
#define GLINE2D_H
#include <cmath>
#include <utility>
#include <vector>
#include "apriltags/MathUtil.h"
#include "apriltags/XYWeight.h"
namespace AprilTags {
//! A 2D line
class GLine2D {
public:
//! Create a new line.
GLine2D();
//! Create a new line.
/* @param slope the slope
* @param b the y intercept
*/
GLine2D(float slope, float b);
//! Create a new line.
/* @param dx A change in X corresponding to dy
* @param dy A change in Y corresponding to dx
* @param p A point that the line passes through
*/
GLine2D(float dX, float dY, const std::pair<float,float>& pt);
//! Create a new line through two points.
/* @param p1 the first point
* @param p2 the second point
*/
GLine2D(const std::pair<float,float>& p1, const std::pair<float,float>& p2);
//! Get the coordinate of a point (on this line), with zero corresponding to the point
//! on the that is perpendicular toa line passing through the origin and the line.
/* This allows easy computation if one point is between two other points on the line:
* compute the line coordinate of all three points and test if a<=b<=c. This is
* implemented by computing the dot product of the vector 'p' with the
* line's direct unit vector.
*/
float getLineCoordinate(const std::pair<float,float>& p);
//! The inverse of getLineCoordinate.
std::pair<float,float> getPointOfCoordinate(float coord);
//!Compute the point where two lines intersect, or (-1,0) if the lines are parallel.
std::pair<float,float> intersectionWith(const GLine2D& line) const;
static GLine2D lsqFitXYW(const std::vector<XYWeight>& xyweights);
inline float getDx() const { return dx; }
inline float getDy() const { return dy; }
inline float getFirst() const { return p.first; }
inline float getSecond() const { return p.second; }
protected:
void normalizeSlope();
void normalizeP();
private:
float dx, dy;
std::pair<float,float> p; //!< A point the line passes through; when normalized, it is the point closest to the origin (hence perpendicular to the line)
bool didNormalizeSlope;
bool didNormalizeP;
};
} // namespace
#endif

31
thirdparty/apriltag/ethz_apriltag2/include/apriltags/GLineSegment2D.h vendored Normal file
View File

@@ -0,0 +1,31 @@
#ifndef GLINESEGMENT2D_H
#define GLINESEGMENT2D_H
#include <algorithm>
#include <cfloat>
#include <cmath>
#include <utility>
#include "apriltags/GLine2D.h"
#include "apriltags/XYWeight.h"
namespace AprilTags {
//! A 2D line with endpoints.
class GLineSegment2D {
public:
GLineSegment2D(const std::pair<float,float> &p0Arg, const std::pair<float,float> &p1Arg);
static GLineSegment2D lsqFitXYW(const std::vector<XYWeight>& xyweight);
std::pair<float,float> getP0() const { return p0; }
std::pair<float,float> getP1() const { return p1; }
private:
GLine2D line;
std::pair<float,float> p0;
std::pair<float,float> p1;
int weight;
};
} // namespace
#endif

37
thirdparty/apriltag/ethz_apriltag2/include/apriltags/Gaussian.h vendored Normal file
View File

@@ -0,0 +1,37 @@
#ifndef GAUSSIAN_H
#define GAUSSIAN_H
#include <cmath>
#include <vector>
namespace AprilTags {
class Gaussian {
public:
static bool warned;
//! Returns a Gaussian filter of size n.
/*! @param sigma standard deviation of the Gaussian
* @param n length of the Gaussian (must be odd)
*/
static std::vector<float> makeGaussianFilter(float sigma, int n);
//! Convolve the input 'a' (which begins at offset aoff and is alen elements in length) with the filter 'f'.
/*! The result is deposited in 'r' at offset 'roff'. f.size() should be odd.
* The output is shifted by -f.size()/2, so that there is no net time delay.
* @param a input vector of pixels
* @param aoff
* @param alen
* @param f
* @param r the resultant array of pixels
* @param roff
*/
static void convolveSymmetricCentered(const std::vector<float>& a, unsigned int aoff, unsigned int alen,
const std::vector<float>& f, std::vector<float>& r, unsigned int roff);
};
} // namespace
#endif

46
thirdparty/apriltag/ethz_apriltag2/include/apriltags/GrayModel.h vendored Normal file
View File

@@ -0,0 +1,46 @@
//-*-c++-*-
#ifndef GRAYMODEL_H
#define GRAYMODEL_H
#include <Eigen/Dense>
#include <vector>
namespace AprilTags {
//! Fits a grayscale model over an area of pixels.
/*! The model is of the form: c1*x + c2*y + c3*x*y + c4 = value
*
* We use this model to compute spatially-varying thresholds for
* reading bits.
*/
class GrayModel {
public:
GrayModel();
void addObservation(float x, float y, float gray);
inline int getNumObservations() { return nobs; }
float interpolate(float x, float y);
private:
void compute();
// We're solving Av = b.
//
// For each observation, we add a row to A of the form [x y xy 1]
// and to b of the form gray*[x y xy 1]. v is the vector [c1 c2 c3 c4].
//
// The least-squares solution to the system is v = inv(A'A)A'b
Eigen::Matrix4d A;
Eigen::Vector4d v;
Eigen::Vector4d b;
int nobs;
bool dirty; //!< True if we've added an observation and need to recompute v
};
} // namespace
#endif

180
thirdparty/apriltag/ethz_apriltag2/include/apriltags/Gridder.h vendored Normal file
View File

@@ -0,0 +1,180 @@
#ifndef GRIDDER_H
#define GRIDDER_H
#include <algorithm>
#include <iterator>
#include <vector>
#include "apriltags/Segment.h"
namespace AprilTags {
//! A lookup table in 2D for implementing nearest neighbor.
template <class T>
class Gridder {
private:
Gridder(const Gridder&); //!< don't call
Gridder& operator=(const Gridder&); //!< don't call
struct Cell {
T* object;
Cell *next;
Cell() : object(NULL), next(NULL) {}
Cell(const Cell& c) : object(c.object), next(c.next) {}
// Destructor
~Cell() {
delete next;
}
Cell& operator=(const Cell &other) {
if (this == &other)
return *this;
object = other.object;
next = other.next;
return *this;
}
};
//! Initializes Gridder constructor
void gridderInit(float x0Arg, float y0Arg, float x1Arg, float y1Arg, float ppCell) {
width = (int) ((x1Arg - x0Arg)/ppCell + 1);
height = (int) ((y1Arg - y0Arg)/ppCell + 1);
x1 = x0Arg + ppCell*width;
y1 = y0Arg + ppCell*height;
cells = std::vector< std::vector<Cell*> >(height, std::vector<Cell*>(width,(Cell*)NULL));
}
float x0,y0,x1,y1;
int width, height;
float pixelsPerCell; //pixels per cell
std::vector< std::vector<Cell*> > cells;
public:
Gridder(float x0Arg, float y0Arg, float x1Arg, float y1Arg, float ppCell)
: x0(x0Arg), y0(y0Arg), x1(), y1(), width(), height(), pixelsPerCell(ppCell),
cells() { gridderInit(x0Arg, y0Arg, x1Arg, y1Arg, ppCell); }
// Destructor
~Gridder() {
for (unsigned int i = 0; i < cells.size(); i++) {
for (unsigned int j = 0; j < cells[i].size(); j++)
delete cells[i][j];
}
}
void add(float x, float y, T* object) {
int ix = (int) ((x - x0)/pixelsPerCell);
int iy = (int) ((y - y0)/pixelsPerCell);
if (ix>=0 && iy>=0 && ix<width && iy<height) {
Cell *c = new Cell;
c->object = object;
c->next = cells[iy][ix];
cells[iy][ix] = c;
// cout << "Gridder placed seg " << o->getId() << " at (" << ix << "," << iy << ")" << endl;
}
}
// iterator begin();
// iterator end();
//! Iterator for Segment class.
class Iterator {
public:
Iterator(Gridder* grid, float x, float y, float range)
: outer(grid), ix0(), ix1(), iy0(), iy1(), ix(), iy(), c(NULL) { iteratorInit(x,y,range); }
Iterator(const Iterator& it)
: outer(it.outer), ix0(it.ix0), ix1(it.ix1), iy0(it.iy0), iy1(it.iy1), ix(it.ix), iy(it.iy), c(it.c) {}
Iterator& operator=(const Iterator& it) {
outer = it.outer;
ix0 = it.ix0;
ix1 = it.ix1;
iy0 = it.iy0;
iy1 = it.iy1;
ix = it.ix;
iy = it.iy;
c = it.c;
}
bool hasNext() {
if (c == NULL)
findNext();
return (c != NULL);
}
T& next() {
T* thisObj = c->object;
findNext();
return *thisObj; // return Segment
}
private:
void findNext() {
if (c != NULL)
c = c->next;
if (c != NULL)
return;
ix++;
while (true) {
if (ix > ix1) {
iy++;
ix = ix0;
}
if (iy > iy1)
break;
c = outer->cells[iy][ix];
if (c != NULL)
break;
ix++;
}
}
//! Initializes Iterator constructor
void iteratorInit(float x, float y, float range) {
ix0 = (int) ((x - range - outer->x0)/outer->pixelsPerCell);
iy0 = (int) ((y - range - outer->y0)/outer->pixelsPerCell);
ix1 = (int) ((x + range - outer->x0)/outer->pixelsPerCell);
iy1 = (int) ((y + range - outer->y0)/outer->pixelsPerCell);
ix0 = std::max(0, ix0);
ix0 = std::min(outer->width-1, ix0);
ix1 = std::max(0, ix1);
ix1 = std::min(outer->width-1, ix1);
iy0 = std::max(0, iy0);
iy0 = std::min(outer->height-1, iy0);
iy1 = std::max(0, iy1);
iy1 = std::min(outer->height-1, iy1);
ix = ix0;
iy = iy0;
c = outer->cells[iy][ix];
}
Gridder* outer;
int ix0, ix1, iy0, iy1;
int ix, iy;
Cell *c;
};
typedef Iterator iterator;
iterator find(float x, float y, float range) { return Iterator(this,x,y,range); }
};
} // namespace
#endif

77
thirdparty/apriltag/ethz_apriltag2/include/apriltags/Homography33.h vendored Normal file
View File

@@ -0,0 +1,77 @@
//-*-c++-*-
#ifndef HOMOGRAPHY33_H
#define HOMOGRAPHY33_H
#include <utility>
#include <vector>
#include <Eigen/Dense>
// NOTE: In Basalt we use homography, since for fisheye-distorted
// images, interpolation is not good enough, resulting in a lot less
// valid detections. At the same time we don't case about speed too
// much.
// interpolate points instead of using homography
//#define INTERPOLATE
// use stable version of homography recover (opencv, includes refinement step)
#define STABLE_H
//! Compute 3x3 homography using Direct Linear Transform
/*
*
* DEPRECATED - DEPRECATED - DEPRECATED - DEPRECATED
*
* use TagDetection::getRelativeTransform() instead
*
*
* y = Hx (y = image coordinates in homogeneous coordinates, H = 3x3
* homography matrix, x = homogeneous 2D world coordinates)
*
* For each point correspondence, constrain y x Hx = 0 (where x is
* cross product). This means that they have the same direction, and
* ignores scale factor.
*
* We rewrite this as Ah = 0, where h is the 9x1 column vector of the
* elements of H. Each point correspondence gives us 3 equations of
* rank 2. The solution h is the minimum right eigenvector of A,
* which we can obtain via SVD, USV' = A. h is the right-most column
* of V'.
*
* We will actually maintain A'A internally, which is 9x9 regardless
* of how many correspondences we're given, and has the same
* eigenvectors as A.
*/
class Homography33 {
public:
//! Constructor
Homography33(const std::pair<float,float> &opticalCenter);
#ifdef STABLE_H
void setCorrespondences(const std::vector< std::pair<float,float> > &srcPts,
const std::vector< std::pair<float,float> > &dstPts);
#else
void addCorrespondence(float worldx, float worldy, float imagex, float imagey);
#endif
//! Note that the returned H matrix does not reflect cxy.
Eigen::Matrix3d& getH();
const std::pair<float,float> getCXY() const { return cxy; }
void compute();
std::pair<float,float> project(float worldx, float worldy);
private:
std::pair<float,float> cxy;
Eigen::Matrix<double,9,9> fA;
Eigen::Matrix3d H;
bool valid;
#ifdef STABLE_H
std::vector< std::pair<float,float> > srcPts, dstPts;
#endif
};
#endif

69
thirdparty/apriltag/ethz_apriltag2/include/apriltags/MathUtil.h vendored Normal file
View File

@@ -0,0 +1,69 @@
//-*-c++-*-
#ifndef MATHUTIL_H
#define MATHUTIL_H
#include <cmath>
#include <cfloat>
#include <cstdlib>
#include <ostream>
#include <utility>
namespace AprilTags {
std::ostream& operator<<(std::ostream &os, const std::pair<float,float> &pt);
//! Miscellaneous math utilities and fast exp functions.
class MathUtil {
public:
//! Returns the square of a value.
static inline float square(float x) { return x*x; }
static inline float distance2D(const std::pair<float,float> &p0, const std::pair<float,float> &p1) {
float dx = p0.first - p1.first;
float dy = p0.second - p1.second;
return std::sqrt(dx*dx + dy*dy);
}
//! Returns a result in [-Pi, Pi]
static inline float mod2pi(float vin) {
const float twopi = 2 * (float)M_PI;
const float twopi_inv = 1.f / (2.f * (float)M_PI);
float absv = std::abs(vin);
float q = absv*twopi_inv + 0.5f;
int qi = (int) q;
float r = absv - qi*twopi;
return (vin<0) ? -r : r;
}
//! Returns a value of v wrapped such that ref and v differ by no more than +/- Pi
static inline float mod2pi(float ref, float v) { return ref + mod2pi(v-ref); }
// lousy approximation of arctan function, but good enough for our purposes (about 4 degrees)
static inline double fast_atan2(double y, double x) {
double coeff_1 = M_PI/4;
double coeff_2 = 3*coeff_1;
double abs_y = fabs(y)+1e-10; // kludge to prevent 0/0 condition
double angle;
if (x >= 0) {
double r = (x - abs_y) / (x + abs_y);
angle = coeff_1 - coeff_1 * r;
} else {
double r = (x + abs_y) / (abs_y - x);
angle = coeff_2 - coeff_1 * r;
}
if (y < 0)
return -angle; // negate if in quad III or IV
else
return angle;
}
};
} // namespace
#endif

72
thirdparty/apriltag/ethz_apriltag2/include/apriltags/Quad.h vendored Normal file
View File

@@ -0,0 +1,72 @@
#ifndef QUAD_H
#define QUAD_H
#include <utility>
#include <vector>
#include <Eigen/Dense>
#include "apriltags/Homography33.h"
namespace AprilTags {
class FloatImage;
class Segment;
using std::min;
using std::max;
//! Represents four segments that form a loop, and might be a tag.
class Quad {
public:
static const int minimumEdgeLength = 6; //!< Minimum size of a tag (in pixels) as measured along edges and diagonals
static float const maxQuadAspectRatio; //!< Early pruning of quads with insane ratios.
//! Constructor
/*! (x,y) are the optical center of the camera, which is
* needed to correctly compute the homography. */
Quad(const std::vector< std::pair<float,float> >& p, const std::pair<float,float>& opticalCenter);
//! Interpolate given that the lower left corner of the lower left cell is at (-1,-1) and the upper right corner of the upper right cell is at (1,1).
std::pair<float,float> interpolate(float x, float y);
//! Same as interpolate, except that the coordinates are interpreted between 0 and 1, instead of -1 and 1.
std::pair<float,float> interpolate01(float x, float y);
//! Points for the quad (in pixel coordinates), in counter clockwise order. These points are the intersections of segments.
std::vector< std::pair<float,float> > quadPoints;
//! Segments composing this quad
std::vector<Segment*> segments;
//! Total length (in pixels) of the actual perimeter observed for the quad.
/*! This is in contrast to the geometric perimeter, some of which
* may not have been directly observed but rather inferred by
* intersecting segments. Quads with more observed perimeter are
* preferred over others. */
float observedPerimeter;
//! Given that the whole quad spans from (0,0) to (1,1) in "quad space", compute the pixel coordinates for a given point within that quad.
/*! Note that for most of the Quad's existence, we will not know the correct orientation of the tag. */
Homography33 homography;
//! Searches through a vector of Segments to form Quads.
/* @param quads any discovered quads will be added to this list
* @param path the segments currently part of the search
* @param parent the first segment in the quad
* @param depth how deep in the search are we?
*/
static void search(const FloatImage& fImage, std::vector<Segment*>& path,
Segment& parent, int depth, std::vector<Quad>& quads,
const std::pair<float,float>& opticalCenter);
#ifdef INTERPOLATE
private:
Eigen::Vector2f p0, p3, p01, p32;
#endif
};
} // namespace
#endif

56
thirdparty/apriltag/ethz_apriltag2/include/apriltags/Segment.h vendored Normal file
View File

@@ -0,0 +1,56 @@
#ifndef SEGMENT_H
#define SEGMENT_H
#include <cmath>
#include <vector>
namespace AprilTags {
//! Represents a line fit to a set of pixels whose gradients are similiar.
class Segment {
public:
Segment();
static int const minimumSegmentSize = 4; //!< Minimum number of pixels in a segment before we'll fit a line to it.
static float const minimumLineLength; //!< In pixels. Calculated based on minimum plausible decoding size for Tag9 family.
float getX0() const { return x0; }
void setX0(float newValue) { x0 = newValue; }
float getY0() const { return y0; }
void setY0(float newValue) { y0 = newValue; }
float getX1() const { return x1; }
void setX1(float newValue) { x1 = newValue; }
float getY1() const { return y1; }
void setY1(float newValue) { y1 = newValue; }
float getTheta() const { return theta; }
void setTheta(float newValue) { theta = newValue; }
float getLength() const { return length; }
void setLength(float newValue) { length = newValue; }
//! Returns the length of the Segment.
float segmentLength();
//! Print endpoint coordinates of this segment.
void printSegment();
//! ID of Segment.
int getId() const { return segmentId; }
std::vector<Segment*> children;
private:
float x0, y0, x1, y1;
float theta; // gradient direction (points towards white)
float length; // length of line segment in pixels
int segmentId;
static int idCounter;
};
} // namsepace
#endif

41
thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag16h5.h vendored Normal file
View File

@@ -0,0 +1,41 @@
/** Tag family with 30 distinct codes.
bits: 16, minimum hamming: 5, minimum complexity: 5
Max bits corrected False positive rate
0 0.045776 %
1 0.778198 %
2 6.271362 %
Generation time: 0.309000 s
Hamming distance between pairs of codes (accounting for rotation):
0 0
1 0
2 0
3 0
4 0
5 120
6 172
7 91
8 33
9 13
10 6
11 0
12 0
13 0
14 0
15 0
16 0
**/
#pragma once
namespace AprilTags {
const unsigned long long t16h5[] =
{ 0x231bLL, 0x2ea5LL, 0x346aLL, 0x45b9LL, 0x79a6LL, 0x7f6bLL, 0xb358LL, 0xe745LL, 0xfe59LL, 0x156dLL, 0x380bLL, 0xf0abLL, 0x0d84LL, 0x4736LL, 0x8c72LL, 0xaf10LL, 0x093cLL, 0x93b4LL, 0xa503LL, 0x468fLL, 0xe137LL, 0x5795LL, 0xdf42LL, 0x1c1dLL, 0xe9dcLL, 0x73adLL, 0xad5fLL, 0xd530LL, 0x07caLL, 0xaf2eLL };
static const TagCodes tagCodes16h5 = TagCodes(16, 5, t16h5, sizeof(t16h5)/sizeof(t16h5[0]));
}

17
thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag16h5_other.h vendored Normal file
View File

@@ -0,0 +1,17 @@
// These codes were generated by TagFamilyGenerator.java from Ed Olson
// tag16h5Codes : 16 bits, minimum Hamming distance 5, minimum complexity 5
#pragma once
namespace AprilTags {
const unsigned long long t16h5_other[] =
{ 0x231bLL, 0x2ea5LL, 0x346aLL, 0x45b9LL, 0x6857LL, 0x7f6bLL, 0xad93LL, 0xb358LL,
0xb91dLL, 0xe745LL, 0x156dLL, 0xd3d2LL, 0xdf5cLL, 0x4736LL, 0x8c72LL, 0x5a02LL,
0xd32bLL, 0x1867LL, 0x468fLL, 0xdc91LL, 0x4940LL, 0xa9edLL, 0x2bd5LL, 0x599aLL,
0x9009LL, 0x61f6LL, 0x3850LL, 0x8157LL, 0xbfcaLL, 0x987cLL};
static const TagCodes tagCodes16h5_other = TagCodes(16, 5, t16h5_other, sizeof(t16h5_other)/sizeof(t16h5_other[0]));
}

51
thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag25h7.h vendored Normal file
View File

@@ -0,0 +1,51 @@
/** Tag family with 242 distinct codes.
bits: 25, minimum hamming: 7, minimum complexity: 8
Max bits corrected False positive rate
0 0.000721 %
1 0.018752 %
2 0.235116 %
3 1.893914 %
Generation time: 72.585000 s
Hamming distance between pairs of codes (accounting for rotation):
0 0
1 0
2 0
3 0
4 0
5 0
6 0
7 2076
8 4161
9 5299
10 6342
11 5526
12 3503
13 1622
14 499
15 114
16 16
17 3
18 0
19 0
20 0
21 0
22 0
23 0
24 0
25 0
**/
#pragma once
namespace AprilTags {
const unsigned long long t25h7[] =
{ 0x4b770dLL, 0x11693e6LL, 0x1a599abLL, 0xc3a535LL, 0x152aafaLL, 0xaccd98LL, 0x1cad922LL, 0x2c2fadLL, 0xbb3572LL, 0x14a3b37LL, 0x186524bLL, 0xc99d4cLL, 0x23bfeaLL, 0x141cb74LL, 0x1d0d139LL, 0x1670aebLL, 0x851675LL, 0x150334eLL, 0x6e3ed8LL, 0xfd449dLL, 0xaa55ecLL, 0x1c86176LL, 0x15e9b28LL, 0x7ca6b2LL, 0x147c38bLL, 0x1d6c950LL, 0x8b0e8cLL, 0x11a1451LL, 0x1562b65LL, 0x13f53c8LL, 0xd58d7aLL, 0x829ec9LL, 0xfaccf1LL, 0x136e405LL, 0x7a2f06LL, 0x10934cbLL, 0x16a8b56LL, 0x1a6a26aLL, 0xf85545LL, 0x195c2e4LL, 0x24c8a9LL, 0x12bfc96LL, 0x16813aaLL, 0x1a42abeLL, 0x1573424LL, 0x1044573LL, 0xb156c2LL, 0x5e6811LL, 0x1659bfeLL, 0x1d55a63LL, 0x5bf065LL, 0xe28667LL, 0x1e9ba54LL, 0x17d7c5aLL, 0x1f5aa82LL, 0x1a2bbd1LL, 0x1ae9f9LL, 0x1259e51LL, 0x134062bLL, 0xe1177aLL, 0xed07a8LL, 0x162be24LL, 0x59128bLL, 0x1663e8fLL, 0x1a83cbLL, 0x45bb59LL, 0x189065aLL, 0x4bb370LL, 0x16fb711LL, 0x122c077LL, 0xeca17aLL, 0xdbc1f4LL, 0x88d343LL, 0x58ac5dLL, 0xba02e8LL, 0x1a1d9dLL, 0x1c72eecLL, 0x924bc5LL, 0xdccab3LL, 0x886d15LL, 0x178c965LL, 0x5bc69aLL, 0x1716261LL, 0x174e2ccLL, 0x1ed10f4LL, 0x156aa8LL, 0x3e2a8aLL, 0x2752edLL, 0x153c651LL, 0x1741670LL, 0x765b05LL, 0x119c0bbLL, 0x172a783LL, 0x4faca1LL, 0xf31257LL, 0x12441fcLL, 0x0d3748LL, 0xc21f15LL, 0xac5037LL, 0x180e592LL, 0x7d3210LL, 0xa27187LL, 0x2beeafLL, 0x26ff57LL, 0x690e82LL, 0x77765cLL, 0x1a9e1d7LL, 0x140be1aLL, 0x1aa1e3aLL, 0x1944f5cLL, 0x19b5032LL, 0x169897LL, 0x1068eb9LL, 0xf30dbcLL, 0x106a151LL, 0x1d53e95LL, 0x1348ceeLL, 0xcf4fcaLL, 0x1728bb5LL, 0xdc1eecLL, 0x69e8dbLL, 0x16e1523LL, 0x105fa25LL, 0x18abb0cLL, 0xc4275dLL, 0x6d8e76LL, 0xe8d6dbLL, 0xe16fd7LL, 0x1ac2682LL, 0x77435bLL, 0xa359ddLL, 0x3a9c4eLL, 0x123919aLL, 0x1e25817LL, 0x02a836LL, 0x1545a4LL, 0x1209c8dLL, 0xbb5f69LL, 0x1dc1f02LL, 0x5d5f7eLL, 0x12d0581LL, 0x13786c2LL, 0xe15409LL, 0x1aa3599LL, 0x139aad8LL, 0xb09d2aLL, 0x54488fLL, 0x13c351cLL, 0x976079LL, 0xb25b12LL, 0x1addb34LL, 0x1cb23aeLL, 0x1175738LL, 0x1303bb8LL, 0xd47716LL, 0x188ceeaLL, 0xbaf967LL, 0x1226d39LL, 0x135e99bLL, 0x34adc5LL, 0x2e384dLL, 0x90d3faLL, 0x232713LL, 0x17d49b1LL, 0xaa84d6LL, 0xc2ddf8LL, 0x1665646LL, 0x4f345fLL, 0x2276b1LL, 0x1255dd7LL, 0x16f4cccLL, 0x4aaffcLL, 0xc46da6LL, 0x85c7b3LL, 0x1311fcbLL, 0x9c6c4fLL, 0x187d947LL, 0x8578e4LL, 0xe2bf0bLL, 0xa01b4cLL, 0xa1493bLL, 0x7ad766LL, 0xccfe82LL, 0x1981b5bLL, 0x1cacc85LL, 0x562cdbLL, 0x15b0e78LL, 0x8f66c5LL, 0x3332bfLL, 0x12ce754LL, 0x096a76LL, 0x1d5e3baLL, 0x27ea41LL, 0x14412dfLL, 0x67b9b4LL, 0xdaa51aLL, 0x1dcb17LL, 0x4d4afdLL, 0x6335d5LL, 0xee2334LL, 0x17d4e55LL, 0x1b8b0f0LL, 0x14999e3LL, 0x1513dfaLL, 0x765cf2LL, 0x56af90LL, 0x12e16acLL, 0x1d3d86cLL, 0xff279bLL, 0x18822ddLL, 0x99d478LL, 0x8dc0d2LL, 0x34b666LL, 0xcf9526LL, 0x186443dLL, 0x7a8e29LL, 0x19c6aa5LL, 0x1f2a27dLL, 0x12b2136LL, 0xd0cd0dLL, 0x12cb320LL, 0x17ddb0bLL, 0x05353bLL, 0x15b2cafLL, 0x1e5a507LL, 0x120f1e5LL, 0x114605aLL, 0x14efe4cLL, 0x568134LL, 0x11b9f92LL, 0x174d2a7LL, 0x692b1dLL, 0x39e4feLL, 0xaaff3dLL, 0x96224cLL, 0x13c9f77LL, 0x110ee8fLL, 0xf17beaLL, 0x99fb5dLL, 0x337141LL, 0x02b54dLL, 0x1233a70LL };
static const TagCodes tagCodes25h7 = TagCodes(25, 7, t25h7, sizeof(t25h7)/sizeof(t25h7[0]));
}

52
thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag25h9.h vendored Normal file
View File

@@ -0,0 +1,52 @@
/** Tag family with 35 distinct codes.
bits: 25, minimum hamming: 9, minimum complexity: 8
Max bits corrected False positive rate
0 0.000104 %
1 0.002712 %
2 0.034004 %
3 0.273913 %
4 1.593411 %
Generation time: 31.283000 s
Hamming distance between pairs of codes (accounting for rotation):
0 0
1 0
2 0
3 0
4 0
5 0
6 0
7 0
8 0
9 156
10 214
11 120
12 64
13 29
14 11
15 1
16 0
17 0
18 0
19 0
20 0
21 0
22 0
23 0
24 0
25 0
**/
#pragma once
namespace AprilTags {
const unsigned long long t25h9[] =
{ 0x155cbf1LL, 0x1e4d1b6LL, 0x17b0b68LL, 0x1eac9cdLL, 0x12e14ceLL, 0x3548bbLL, 0x7757e6LL, 0x1065dabLL, 0x1baa2e7LL, 0xdea688LL, 0x81d927LL, 0x51b241LL, 0xdbc8aeLL, 0x1e50e19LL, 0x15819d2LL, 0x16d8282LL, 0x163e035LL, 0x9d9b81LL, 0x173eec4LL, 0xae3a09LL, 0x5f7c51LL, 0x1a137fcLL, 0xdc9562LL, 0x1802e45LL, 0x1c3542cLL, 0x870fa4LL, 0x914709LL, 0x16684f0LL, 0xc8f2a5LL, 0x833ebbLL, 0x59717fLL, 0x13cd050LL, 0xfa0ad1LL, 0x1b763b0LL, 0xb991ceLL };
static const TagCodes tagCodes25h9 = TagCodes(25, 9, t25h9, sizeof(t25h9)/sizeof(t25h9[0]));
}

64
thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag36h11.h vendored Normal file
View File

File diff suppressed because one or more lines are too long

88
thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag36h11_other.h vendored Normal file
View File

@@ -0,0 +1,88 @@
// These codes were generated by TagFamilyGenerator.java from Ed Olson
// tag36h11Codes : 36 bits, minimum Hamming distance 11, minimum complexity 10
#pragma once
namespace AprilTags {
const unsigned long long t36h11_other[] =
{ 0xd5d628584LL, 0xd97f18b49LL, 0xdd280910eLL, 0xe479e9c98LL, 0xebcbca822LL, 0xf31dab3acLL, 0x056a5d085LL, 0x10652e1d4LL,
0x17b70ed5eLL, 0x22b1dfeadLL, 0x265ad0472LL, 0x34fe91b86LL, 0x3ff962cd5LL, 0x43a25329aLL, 0x474b4385fLL, 0x4e9d243e9LL,
0x5246149aeLL, 0x5997f5538LL, 0x683bb6c4cLL, 0x6be4a7211LL, 0x7e3158eeaLL, 0x81da494afLL, 0x858339a74LL, 0x8cd51a5feLL,
0x9f21cc2d7LL, 0xa2cabc89cLL, 0xadc58d9ebLL, 0xb16e7dfb0LL, 0xb8c05eb3aLL, 0xd25ef139dLL, 0xd607e1962LL, 0xe4aba3076LL,
0x2dde6a3daLL, 0x43d40c678LL, 0x5620be351LL, 0x64c47fa65LL, 0x686d7002aLL, 0x6c16605efLL, 0x6fbf50bb4LL, 0x8d06d39dcLL,
0x9baa950f0LL, 0x9f53856b5LL, 0xadf746dc9LL, 0xbc9b084ddLL, 0xd290aa77bLL, 0xd9e28b305LL, 0xe4dd5c454LL, 0xfad2fe6f2LL,
0x181a8151aLL, 0x26be42c2eLL, 0x2e10237b8LL, 0x405cd5491LL, 0x6ff109f92LL, 0x7742eab1cLL, 0x85e6ac230LL, 0x8d388cdbaLL,
0x9f853ea93LL, 0xc41ea2445LL, 0xcf1973594LL, 0x14a34a333LL, 0x31eacd15bLL, 0x44377ee34LL, 0x6c79d2dabLL, 0x73cbb3935LL,
0x89c155bd3LL, 0x8d6a46198LL, 0x91133675dLL, 0xa708d89fbLL, 0xae5ab9585LL, 0xb9558a6d4LL, 0xb98743ab2LL, 0xd6cec68daLL,
0x1506bcaefLL, 0x4becd217aLL, 0x4f95c273fLL, 0x658b649ddLL, 0xa76c4b1b7LL, 0xecf621f56LL, 0x1c8a56a57LL, 0x3628e92baLL,
0x53706c0e2LL, 0x7809cfa94LL, 0xc88e77982LL, 0xe97eead6fLL, 0x5af40604aLL, 0xffa6463ebLL, 0x5eceaf9edLL, 0x7c1632815LL,
0xc1a0095b4LL, 0xe9e25d52bLL, 0x3a6705419LL, 0xa8333012fLL, 0x4ce5704d0LL, 0x508e60a95LL, 0x877476120LL, 0xa864e950dLL,
0xea45cfce7LL, 0x19da047e8LL, 0x24d4d5937LL, 0x54690a438LL, 0x6e079cc9bLL, 0x99f2e11d7LL, 0x499ff26c7LL, 0x50f1d3251LL,
0x66e7754efLL, 0x49d1abaa5LL, 0x96ad633ceLL, 0x9a5653993LL, 0xaca30566cLL, 0x8be44b65dLL, 0xb4269f5d4LL, 0xdc68f354bLL,
0x4dde0e826LL, 0xd548cbd9fLL, 0xfd8b1fd16LL, 0x76521bb7bLL, 0xd1d194bb8LL, 0xd57a8517dLL, 0xd92375742LL, 0xcab16d40cLL,
0x730c9dd72LL, 0xad9ba39c2LL, 0xb14493f87LL, 0x185409cadLL, 0x77ae2c68dLL, 0x94f5af4b5LL, 0x0a13bad55LL, 0x61ea437cdLL,
0xa022399e2LL, 0xbd69bc80aLL, 0x203b163d1LL, 0x7bba8f40eLL, 0x784358227LL, 0xc92b728d1LL, 0x92a8cfa02LL, 0x9da3a0b51LL,
0xdcd4350bcLL, 0x4aa05fdd2LL, 0x60c7bb44eLL, 0x4b358b96cLL, 0x612b2dc0aLL, 0x775289286LL, 0x7ea469e10LL, 0x09e9d0d2cLL,
0x067299b45LL, 0xb9c89b5faLL, 0x9560f1026LL, 0x62b8f7afaLL, 0xbac139950LL, 0x58b6c4d01LL, 0xa5927c62aLL, 0xe77362e04LL,
0xf29fed331LL, 0x903205f26LL, 0xc36f2afecLL, 0xae72270a4LL, 0x3d2ec51a7LL, 0x82ea55324LL, 0x1ca1c4576LL, 0xa40c81aefLL,
0xbddccd730LL, 0x0e617561eLL, 0x585b218faLL, 0x969317b0fLL, 0x588cdacd8LL, 0x67309c3ecLL, 0x8c5f2b938LL, 0x4142f72ddLL,
0x06e5aaa6bLL, 0x626523aa8LL, 0xb6c475339LL, 0xc56836a4dLL, 0x4647f83b4LL, 0x0908a04f5LL, 0x7862950fbLL, 0xd445808f4LL,
0x28d68b563LL, 0x04d25374bLL, 0xc8bd52fc0LL, 0x06f5491d5LL, 0x27e5bc5c2LL, 0x2bc065f65LL, 0x96dc3ea0cLL, 0xfdb9fb354LL,
0x47b3a7630LL, 0xd7372a6abLL, 0x372678c25LL, 0xe768b5cafLL, 0x437d5a886LL, 0x2b091f757LL, 0x91b522cc1LL, 0x62846097cLL,
0x3aa57f1c1LL, 0x263da6e13LL, 0xfa841bcb5LL, 0x157ebf02aLL, 0xf586e9f93LL, 0xecaf0e8ceLL, 0x82ef46939LL, 0x847d10829LL,
0x68919e513LL, 0x2aeed3e98LL, 0x11265760cLL, 0x1ce80d6d3LL, 0x0e4c1b374LL, 0x68b0db3e7LL, 0x1c389627aLL, 0xfb79dc26bLL,
0x9379975a4LL, 0x064ac3391LL, 0x706dfdae2LL, 0x44edfb117LL, 0x86a4f78c8LL, 0xaebd61816LL, 0xf53fd690bLL, 0xb8f91cda2LL,
0xf0a6173a5LL, 0x12c0e1ec6LL, 0xd1a6e0664LL, 0x6bf37b450LL, 0x62b82d5cfLL, 0xe5f46d153LL, 0x0d1438d4bLL, 0x5af82d134LL,
0x6ea0ef91fLL, 0x9ff4a76eeLL, 0xde5e56ce1LL, 0xb5c82ed18LL, 0x5b50f279bLL, 0xd7f297fa3LL, 0xef444ad53LL, 0xa8c9a5013LL,
0x3d300e4f1LL, 0x33fc8fa25LL, 0x43d277c22LL, 0x9d2c1d435LL, 0x5f8952dbaLL, 0xf0cc59103LL, 0xd2f779af6LL, 0xb5e97f461LL,
0x7f0b3918bLL, 0xe42e63e1aLL, 0x769bc1897LL, 0x97bdee062LL, 0x792229addLL, 0x816ca89bdLL, 0xd41446335LL, 0x3f572b065LL,
0x2a93af8b0LL, 0xcadde9ac9LL, 0x7176b93c1LL, 0x84b15c29bLL, 0x9b9f9c88fLL, 0x537511febLL, 0xee8891d4fLL, 0x5dc83b096LL,
0x05bd1b4a0LL, 0x2e073e7ccLL, 0x0b5f174c6LL, 0xb184d5e98LL, 0xd0ef4e74aLL, 0x0ddad25b7LL, 0xbd16e63f6LL, 0x4aa64f025LL,
0xa252eda74LL, 0xd940d24b4LL, 0x9745a041bLL, 0x055322c79LL, 0x7022f6a83LL, 0xa31416eacLL, 0x96b2880f6LL, 0x48d385322LL,
0x14d46c8f9LL, 0x11e4d63b7LL, 0x379614f93LL, 0x71eda5cc5LL, 0xaa05e1e39LL, 0xcee09d333LL, 0x52c976570LL, 0x023252178LL,
0x599fac8f4LL, 0xbb0a48854LL, 0x98cd630bfLL, 0x2d8f6f9a4LL, 0xf5c05a72dLL, 0x9ed9d672bLL, 0x50d8b8ce3LL, 0xe59ac55c8LL,
0xe09d938a6LL, 0x4ada4f38bLL, 0xbb85a794eLL, 0x5544e5f55LL, 0x9a3db8244LL, 0xe3784e95dLL, 0x796c81d2bLL, 0x6cebb60a1LL,
0x27b2d55b4LL, 0x6de945a0cLL, 0x4a69d0af9LL, 0x6afea0adfLL, 0x158987899LL, 0x1b528fb48LL, 0x6d183275eLL, 0x73afeed3aLL,
0x1a7a77a10LL, 0x4be59d2feLL, 0x2ad522b12LL, 0xa82d445fdLL, 0xbbcb59c93LL, 0xe71e94895LL, 0x75b14896fLL, 0xb0afb721aLL,
0x065d8e6c8LL, 0x372810d9cLL, 0xb77603728LL, 0xad78c1f44LL, 0x90ca91da0LL, 0x2e74184b4LL, 0xc2964c0aaLL, 0xb07f7a899LL,
0x8ee694eddLL, 0x1ad7caf87LL, 0x2035916c5LL, 0xcd1670631LL, 0x1611c2a77LL, 0x8a1a06962LL, 0xdb970149aLL, 0x5778c6bb4LL,
0x3fab695feLL, 0x014b9cc35LL, 0x604be4377LL, 0xfd49501f1LL, 0xe2b710c4dLL, 0x6bf7f4a88LL, 0x0adf98124LL, 0xc5ee49adeLL,
0xe4c34b0eaLL, 0x9b5e0047dLL, 0x4002b5929LL, 0x4e9a35492LL, 0x908aedae9LL, 0xa0bc790edLL, 0xd12b583baLL, 0x431b08264LL,
0xb7b33afc8LL, 0xd115672f8LL, 0x253296b16LL, 0xbd5e4f6edLL, 0xf1276fc55LL, 0x5feaa426bLL, 0x2d0955cbfLL, 0xcb2ade90eLL,
0x08b0fe749LL, 0x2709d6730LL, 0x0edc7ec97LL, 0xa7c74d431LL, 0x1536402eaLL, 0x61936f66dLL, 0x7ec973bc9LL, 0xa00a12d3dLL,
0xc6ed2ccf6LL, 0x8f87c4b9aLL, 0xf049ee52bLL, 0x0d1fa9777LL, 0x85175a497LL, 0x2d917c5c5LL, 0xfc61287b4LL, 0x63ce55156LL,
0x659cac663LL, 0xbb4b8174fLL, 0x70bd5be0bLL, 0xfb3da5f18LL, 0x917b001e3LL, 0x516870f16LL, 0x03bb5ac33LL, 0x2a510ec0cLL,
0x07ecd1ae2LL, 0x06642c91aLL, 0xcc7c83662LL, 0xb88d2c60eLL, 0x40d35e87eLL, 0x452f5656eLL, 0xf8c5e5640LL, 0xc68372145LL,
0x6b61cb49eLL, 0x066ce5035LL, 0x151c05dd0LL, 0xad92f9119LL, 0x8fa874156LL, 0xd7d545982LL, 0x2602c7a8eLL, 0x0d9054ac8LL,
0xb85332ce0LL, 0xa8e7f583cLL, 0xa2534a713LL, 0x77ce78732LL, 0x6b605c6c5LL, 0x8101603e6LL, 0x9573cd8f0LL, 0xa18bba0abLL,
0xf5d224ae1LL, 0x9ecb4dfd4LL, 0x2e48c9e03LL, 0x79b4c6ae0LL, 0x60e6b0713LL, 0xea6a8420dLL, 0xa22971a8fLL, 0x605c053fdLL,
0x57678633aLL, 0xea85c3395LL, 0x7fda1da74LL, 0x9824459caLL, 0xb2eee31f2LL, 0x4a34b0db1LL, 0xb5bbbd933LL, 0x583d9c190LL,
0xc93e6091cLL, 0xdca7c6e3bLL, 0x214d69b74LL, 0x525894f7fLL, 0x21be0e083LL, 0xf0dbd2784LL, 0x0ffac88d9LL, 0x57f7e33e5LL,
0x4a7301d85LL, 0x8887af6f6LL, 0x1b8ccb3a1LL, 0x68c1b2878LL, 0x78b6bf950LL, 0x63b9aa851LL, 0x7ed12f23aLL, 0x350eb35b2LL,
0x561503189LL, 0x3f16ac63cLL, 0xd2fd4b06cLL, 0xa7c49627eLL, 0x36b9f5d0aLL, 0xbca21c149LL, 0xba5bc28efLL, 0xce2c2b89cLL,
0x776bc0448LL, 0xce170f268LL, 0x8f57303c3LL, 0x74e5fcc9eLL, 0x46de67b7eLL, 0x2b98bd7aaLL, 0xb5c41dc2eLL, 0x12e1e50f8LL,
0x875f6fcdaLL, 0xf90ea702dLL, 0x7ed051595LL, 0x9d8da07b8LL, 0xbc30d09edLL, 0x77ad8306bLL, 0x82d4a0885LL, 0xf4e1b7a04LL,
0xb427eabdaLL, 0xdb1b28f5eLL, 0xe5f911de5LL, 0xb8ff4c115LL, 0x3185fbcf4LL, 0xefda16bdfLL, 0xeaa3f6c63LL, 0x9a3f4f520LL,
0x6317c6e21LL, 0xde1ac8909LL, 0xb962e4d06LL, 0x8a8cc1536LL, 0x0abebf2d5LL, 0x6a3787f5fLL, 0x62cc2622fLL, 0x3196aa59fLL,
0x9b6816c6bLL, 0x95f398661LL, 0x2b1673eb7LL, 0xc9cf19ba7LL, 0x44394782aLL, 0xd02e2d199LL, 0xe517f16dcLL, 0x433df5666LL,
0xdbfb6521fLL, 0x6316ed9fbLL, 0x0681b072aLL, 0x24e7e3614LL, 0x3049f22daLL, 0x245b47d67LL, 0x032e59d5dLL, 0x78f512121LL,
0xf76e98aacLL, 0x20e313ad6LL, 0x9947bd319LL, 0x0719aab9fLL, 0xabe40e6b2LL, 0x9bbec96c6LL, 0xcf05e6446LL, 0xee3c76b79LL,
0xe9317cc92LL, 0x9bf9aa92bLL, 0x13fe98495LL, 0xd931239a6LL, 0x7264aced9LL, 0x04ed957b4LL, 0xbb7021cbbLL, 0x4609308b5LL,
0x2d5c52c38LL, 0xceb22ad4fLL, 0x82a47a446LL, 0x04d68909aLL, 0x832cdf368LL, 0x242ed1118LL, 0xd1dda2014LL, 0x901b6a04eLL,
0x19c1e9514LL, 0x3e9c0c97fLL, 0x1e814bce3LL, 0x55b40b44dLL, 0xeff162399LL, 0x4012da58aLL, 0x1d4d90e43LL, 0x50e79facdLL,
0x35ef088baLL, 0x774709d00LL, 0xd32e575b3LL, 0x54c5cfff2LL, 0x59bbd73fbLL, 0x3b2dc7e8bLL, 0x13a74d09fLL, 0xc5a21e37fLL,
0x7aab49b28LL, 0x2793aa17aLL, 0x3a6673575LL, 0x78c27371eLL, 0x1788da29cLL, 0x8b5bb6078LL, 0xa5c506a80LL, 0xc2c487d6aLL,
0xf238647acLL, 0x601f5e6e2LL, 0x2db5d412aLL, 0xd7c46f24aLL, 0xe4e0b67f5LL, 0xe94793093LL, 0xe07e85846LL, 0xa04f6f205LL,
0xe47cbc125LL, 0x9022fb419LL, 0xc2127ead8LL, 0xc670f0e63LL, 0xd282518cfLL, 0x63e8d8335LL, 0x2aa63408eLL, 0xb011851aaLL,
0x2df8c686fLL, 0xa31f8c5f1LL, 0xe8c09cecbLL, 0x4cd645fb9LL, 0xa63b5d9f5LL, 0xd74dd32d6LL, 0xf6869a32aLL, 0xb725dae3eLL,
0xc5b27c981LL, 0x67d0118b8LL, 0xb3fdb04faLL, 0xf11838f31LL, 0x7e73b5fb9LL, 0x24ec2067aLL, 0x8aaf3bceaLL, 0x04a06dca0LL,
0x70a03ed6bLL, 0x70dc29b18LL, 0x4d75699a9LL, 0xaedc558d9LL, 0x62590b9afLL, 0x5f9258453LL, 0xf04a9a9ceLL, 0xcd1feb280LL,
0x7300b05a7LL, 0xadb7ab52cLL, 0x59afeb236LL, 0xf1de62e03LL, 0xf103ba210LL, 0x7cf6472d5LL, 0xf84bf1908LL, 0xa4952dc03LL,
0x43d506f47LL, 0x8e90eed24LL, 0x9c04974e9LL, 0x953aef583LL, 0x3d839c1bcLL, 0x0348ac64fLL, 0x2a1284fc1LL, 0x9fc565ccdLL,
0x57118e8c4LL};
static const TagCodes tagCodes36h11_other = TagCodes(36, 11, t36h11_other, sizeof(t36h11_other)/sizeof(t36h11_other[0]));
}

63
thirdparty/apriltag/ethz_apriltag2/include/apriltags/Tag36h9.h vendored Normal file
View File

File diff suppressed because one or more lines are too long

99
thirdparty/apriltag/ethz_apriltag2/include/apriltags/TagDetection.h vendored Normal file
View File

@@ -0,0 +1,99 @@
#ifndef TAGDETECTION_H
#define TAGDETECTION_H
#include <Eigen/Dense>
#include "opencv2/opencv.hpp"
#include <utility>
#include <vector>
namespace AprilTags {
struct TagDetection {
//! Constructor
TagDetection();
//! Constructor for manually creating tags in a world map
TagDetection(int id);
//! Is the detection good enough?
bool good;
//! Observed code
long long obsCode;
//! Matched code
long long code;
//! What was the ID of the detected tag?
int id;
//! The hamming distance between the detected code and the true code
int hammingDistance;
//! How many 90 degree rotations were required to align the code (internal use only)
int rotation;
/////////////// Fields below are filled in by TagDetector ///////////////
//! Position (in fractional pixel coordinates) of the detection.
/* The points travel counter-clockwise around the target, always
* starting from the same corner of the tag.
*/
std::pair<float,float> p[4];
//! Center of tag in pixel coordinates.
std::pair<float,float> cxy;
//! Measured in pixels, how long was the observed perimeter.
/*! Observed perimeter excludes the inferred perimeter which is used to connect incomplete quads. */
float observedPerimeter;
//! A 3x3 homography that computes pixel coordinates from tag-relative coordinates.
/* Both the input and output coordinates are 2D homogeneous vectors, with y = Hx.
* 'y' are pixel coordinates, 'x' are tag-relative coordinates. Tag coordinates span
* from (-1,-1) to (1,1). The orientation of the homography reflects the orientation
* of the target.
*/
Eigen::Matrix3d homography;
//! Orientation in the xy plane
float getXYOrientation() const;
//! The homography is relative to image center, whose coordinates are below.
std::pair<float,float> hxy;
//! Interpolate point given (x,y) is in tag coordinate space from (-1,-1) to (1,1).
std::pair<float,float> interpolate(float x, float y) const;
//! Used to eliminate redundant tags
bool overlapsTooMuch(const TagDetection &other) const;
//! Relative pose of tag with respect to the camera
/* Returns the relative location and orientation of the tag using a
4x4 homogeneous transformation matrix (see Hartley&Zisserman,
Multi-View Geometry, 2003). Requires knowledge of physical tag
size (side length of black square in meters) as well as camera
calibration (focal length and principal point); Result is in
camera frame (z forward, x right, y down)
*/
Eigen::Matrix4d getRelativeTransform(double tag_size, double fx, double fy,
double px, double py) const;
//! Recover rotation matrix and translation vector of April tag relative to camera.
// Result is in object frame (x forward, y left, z up)
void getRelativeTranslationRotation(double tag_size, double fx, double fy, double px, double py,
Eigen::Vector3d& trans, Eigen::Matrix3d& rot) const;
//! Draw the detection within the supplied image, including boarders and tag ID.
void draw(cv::Mat& image) const;
//! Compare function to sort detections by std::sort
static bool sortByIdCompare (const TagDetection &a, const TagDetection &b) { return (a.id<b.id); }
};
} // namespace
#endif

29
thirdparty/apriltag/ethz_apriltag2/include/apriltags/TagDetector.h vendored Normal file
View File

@@ -0,0 +1,29 @@
#ifndef TAGDETECTOR_H
#define TAGDETECTOR_H
#include <vector>
#include "opencv2/opencv.hpp"
#include "apriltags//TagDetection.h"
#include "apriltags//TagFamily.h"
#include "apriltags//FloatImage.h"
namespace AprilTags {
class TagDetector {
public:
const TagFamily thisTagFamily;
//! Constructor
// note: TagFamily is instantiated here from TagCodes
TagDetector(const TagCodes& tagCodes, const size_t blackBorder=2) : thisTagFamily(tagCodes, blackBorder) {}
std::vector<TagDetection> extractTags(const cv::Mat& image);
};
} // namespace
#endif

106
thirdparty/apriltag/ethz_apriltag2/include/apriltags/TagFamily.h vendored Normal file
View File

@@ -0,0 +1,106 @@
#ifndef TAGFAMILY_H
#define TAGFAMILY_H
#include <climits>
#include <cmath>
#include <stdio.h>
#include <vector>
#include <map>
#include "apriltags//TagDetection.h"
using namespace std;
namespace AprilTags {
class TagCodes {
public:
int bits;
int minHammingDistance;
std::vector<unsigned long long> codes;
public:
TagCodes(int bits, int minHammingDistance,
const unsigned long long* codesA, int num)
: bits(bits), minHammingDistance(minHammingDistance),
codes(codesA, codesA+num) // created vector for all entries of codesA
{}
};
//! Generic class for all tag encoding families
class TagFamily {
public:
//! The codes array is not copied internally and so must not be modified externally.
TagFamily(const TagCodes& tagCodes, const size_t blackBorder);
void setErrorRecoveryBits(int b);
void setErrorRecoveryFraction(float v);
/* if the bits in w were arranged in a d*d grid and that grid was
* rotated, what would the new bits in w be?
* The bits are organized like this (for d = 3):
*
* 8 7 6 2 5 8 0 1 2
* 5 4 3 ==> 1 4 7 ==> 3 4 5 (rotate90 applied twice)
* 2 1 0 0 3 6 6 7 8
*/
static unsigned long long rotate90(unsigned long long w, int d);
//! Computes the hamming distance between two unsigned long longs.
static int hammingDistance(unsigned long long a, unsigned long long b);
//! How many bits are set in the unsigned long long?
static unsigned char popCountReal(unsigned long long w);
static int popCount(unsigned long long w);
//! Given an observed tag with code 'rCode', try to recover the id.
/* The corresponding fields of TagDetection will be filled in. */
void decode(TagDetection& det, unsigned long long rCode) const;
//! Prints the hamming distances of the tag codes.
void printHammingDistances() const;
//! Numer of pixels wide of the inner black border.
int blackBorder;
//! Number of bits in the tag. Must be n^2.
int bits;
//! Dimension of tag. e.g. for 16 bits, dimension=4. Must be sqrt(bits).
int dimension;
//! Minimum hamming distance between any two codes.
/* Accounting for rotational ambiguity? The code can recover
* (minHammingDistance-1)/2 bit errors.
*/
int minimumHammingDistance;
/* The error recovery value determines our position on the ROC
* curve. We will report codes that are within errorRecoveryBits
* of a valid code. Small values mean greater rejection of bogus
* tags (but false negatives). Large values mean aggressive
* reporting of bad tags (but with a corresponding increase in
* false positives).
*/
int errorRecoveryBits;
//! The array of the codes. The id for a code is its index.
std::vector<unsigned long long> codes;
static const int popCountTableShift = 12;
static const unsigned int popCountTableSize = 1 << popCountTableShift;
static unsigned char popCountTable[popCountTableSize];
//! Initializes the static popCountTable
static class TableInitializer {
public:
TableInitializer() {
for (unsigned int i = 0; i < TagFamily::popCountTableSize; i++)
TagFamily::popCountTable[i] = TagFamily::popCountReal(i);
}
} initializer;
};
} // namespace
#endif

41
thirdparty/apriltag/ethz_apriltag2/include/apriltags/UnionFindSimple.h vendored Normal file
View File

@@ -0,0 +1,41 @@
#ifndef UNIONFINDSIMPLE_H
#define UNIONFINDSIMPLE_H
#include <vector>
namespace AprilTags {
//! Implementation of disjoint set data structure using the union-find algorithm
class UnionFindSimple {
//! Identifies parent ids and sizes.
struct Data {
int id;
int size;
};
public:
explicit UnionFindSimple(int maxId) : data(maxId) {
init();
};
int getSetSize(int thisId) { return data[getRepresentative(thisId)].size; }
int getRepresentative(int thisId);
//! Returns the id of the merged node.
/* @param aId
* @param bId
*/
int connectNodes(int aId, int bId);
void printDataVector() const;
private:
void init();
std::vector<Data> data;
};
} // namespace
#endif

19
thirdparty/apriltag/ethz_apriltag2/include/apriltags/XYWeight.h vendored Normal file
View File

@@ -0,0 +1,19 @@
#ifndef XYWeight_H_
#define XYWeight_H_
namespace AprilTags {
//! Represents a triple holding an x value, y value, and weight value.
struct XYWeight {
float x;
float y;
float weight;
XYWeight(float xval, float yval, float weightval) :
x(xval), y(yval), weight(weightval) {}
};
} // namespace
#endif

17
thirdparty/apriltag/ethz_apriltag2/include/apriltags/pch.h vendored Normal file
View File

@@ -0,0 +1,17 @@
#include "apriltags/Edge.h"
#include "apriltags/FloatImage.h"
#include "apriltags/GLine2D.h"
//#include "apriltags/GLineSegment2D.h"
#include "apriltags/Gaussian.h"
//#include "apriltags/GrayModel.h"
//#include "apriltags/Gridder.h"
#include "apriltags/Homography33.h"
#include "apriltags/MathUtil.h"
//#include "apriltags/Quad.h"
//#include "apriltags/Segment.h"
//#include "apriltags/TagDetection.h"
//#include "apriltags/TagDetector.h"
//#include "apriltags/TagFamily.h"
#include "apriltags/UnionFindSimple.h"
#include "apriltags/XYWeight.h"

13
thirdparty/apriltag/ethz_apriltag2/package.xml vendored Normal file
View File

@@ -0,0 +1,13 @@
<?xml version="1.0" encoding="utf-8"?>
<package>
<name>ethz_apriltag2</name>
<description>ethz_apriltag2</description>
<version>0.0.1</version>
<maintainer email="paulfurgale@mavt.ethz.ch">Paul Furgale</maintainer>
<author>Paul Furgale</author>
<license>New BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>opencv</build_depend>
<build_depend>eigen2</build_depend>
</package>

119
thirdparty/apriltag/ethz_apriltag2/src/Edge.cc vendored Normal file
View File

@@ -0,0 +1,119 @@
#include "apriltags/Edge.h"
#include "apriltags/FloatImage.h"
#include "apriltags/MathUtil.h"
#include "apriltags/UnionFindSimple.h"
namespace AprilTags {
float const Edge::minMag = 0.004f;
float const Edge::maxEdgeCost = 30.f * float(M_PI) / 180.f;
int const Edge::WEIGHT_SCALE = 100;
float const Edge::thetaThresh = 100;
float const Edge::magThresh = 1200;
int Edge::edgeCost(float theta0, float theta1, float mag1) {
if (mag1 < minMag) // mag0 was checked by the main routine so no need to recheck here
return -1;
const float thetaErr = std::abs(MathUtil::mod2pi(theta1 - theta0));
if (thetaErr > maxEdgeCost)
return -1;
const float normErr = thetaErr / maxEdgeCost;
return (int) (normErr*WEIGHT_SCALE);
}
void Edge::calcEdges(float theta0, int x, int y,
const FloatImage& theta, const FloatImage& mag,
std::vector<Edge> &edges, size_t &nEdges) {
int width = theta.getWidth();
int thisPixel = y*width+x;
// horizontal edge
int cost1 = edgeCost(theta0, theta.get(x+1,y), mag.get(x+1,y));
if (cost1 >= 0) {
edges[nEdges].cost = cost1;
edges[nEdges].pixelIdxA = thisPixel;
edges[nEdges].pixelIdxB = y*width+x+1;
++nEdges;
}
// vertical edge
int cost2 = edgeCost(theta0, theta.get(x, y+1), mag.get(x,y+1));
if (cost2 >= 0) {
edges[nEdges].cost = cost2;
edges[nEdges].pixelIdxA = thisPixel;
edges[nEdges].pixelIdxB = (y+1)*width+x;
++nEdges;
}
// downward diagonal edge
int cost3 = edgeCost(theta0, theta.get(x+1, y+1), mag.get(x+1,y+1));
if (cost3 >= 0) {
edges[nEdges].cost = cost3;
edges[nEdges].pixelIdxA = thisPixel;
edges[nEdges].pixelIdxB = (y+1)*width+x+1;
++nEdges;
}
// updward diagonal edge
int cost4 = (x == 0) ? -1 : edgeCost(theta0, theta.get(x-1, y+1), mag.get(x-1,y+1));
if (cost4 >= 0) {
edges[nEdges].cost = cost4;
edges[nEdges].pixelIdxA = thisPixel;
edges[nEdges].pixelIdxB = (y+1)*width+x-1;
++nEdges;
}
}
void Edge::mergeEdges(std::vector<Edge> &edges, UnionFindSimple &uf,
float tmin[], float tmax[], float mmin[], float mmax[]) {
for (size_t i = 0; i < edges.size(); i++) {
int ida = edges[i].pixelIdxA;
int idb = edges[i].pixelIdxB;
ida = uf.getRepresentative(ida);
idb = uf.getRepresentative(idb);
if (ida == idb)
continue;
int sza = uf.getSetSize(ida);
int szb = uf.getSetSize(idb);
float tmina = tmin[ida], tmaxa = tmax[ida];
float tminb = tmin[idb], tmaxb = tmax[idb];
float costa = (tmaxa-tmina);
float costb = (tmaxb-tminb);
// bshift will be a multiple of 2pi that aligns the spans of 'b' with 'a'
// so that we can properly take the union of them.
float bshift = MathUtil::mod2pi((tmina+tmaxa)/2, (tminb+tmaxb)/2) - (tminb+tmaxb)/2;
float tminab = min(tmina, tminb + bshift);
float tmaxab = max(tmaxa, tmaxb + bshift);
if (tmaxab-tminab > 2*(float)M_PI) // corner case that's probably not too useful to handle correctly, oh well.
tmaxab = tminab + 2*(float)M_PI;
float mminab = min(mmin[ida], mmin[idb]);
float mmaxab = max(mmax[ida], mmax[idb]);
// merge these two clusters?
float costab = (tmaxab - tminab);
if (costab <= (min(costa, costb) + Edge::thetaThresh/(sza+szb)) &&
(mmaxab-mminab) <= min(mmax[ida]-mmin[ida], mmax[idb]-mmin[idb]) + Edge::magThresh/(sza+szb)) {
int idab = uf.connectNodes(ida, idb);
tmin[idab] = tminab;
tmax[idab] = tmaxab;
mmin[idab] = mminab;
mmax[idab] = mmaxab;
}
}
}
} // namespace

77
thirdparty/apriltag/ethz_apriltag2/src/FloatImage.cc vendored Normal file
View File

@@ -0,0 +1,77 @@
#include "apriltags/FloatImage.h"
#include "apriltags/Gaussian.h"
#include <iostream>
namespace AprilTags {
FloatImage::FloatImage() : width(0), height(0), pixels() {}
FloatImage::FloatImage(int widthArg, int heightArg)
: width(widthArg), height(heightArg), pixels(widthArg*heightArg) {}
FloatImage::FloatImage(int widthArg, int heightArg, const std::vector<float>& pArg)
: width(widthArg), height(heightArg), pixels(pArg) {}
FloatImage& FloatImage::operator=(const FloatImage& other) {
width = other.width;
height = other.height;
if (pixels.size() != other.pixels.size())
pixels.resize(other.pixels.size());
pixels = other.pixels;
return *this;
}
void FloatImage::decimateAvg() {
int nWidth = width/2;
int nHeight = height/2;
for (int y = 0; y < nHeight; y++)
for (int x = 0; x < nWidth; x++)
pixels[y*nWidth+x] = pixels[(2*y)*width + (2*x)];
width = nWidth;
height = nHeight;
pixels.resize(nWidth*nHeight);
}
void FloatImage::normalize() {
const float maxVal = *max_element(pixels.begin(),pixels.end());
const float minVal = *min_element(pixels.begin(),pixels.end());
const float range = maxVal - minVal;
const float rescale = 1 / range;
for ( unsigned int i = 0; i < pixels.size(); i++ )
pixels[i] = (pixels[i]-minVal) * rescale;
}
void FloatImage::filterFactoredCentered(const std::vector<float>& fhoriz, const std::vector<float>& fvert) {
// do horizontal
std::vector<float> r(pixels);
for (int y = 0; y < height; y++) {
Gaussian::convolveSymmetricCentered(pixels, y*width, width, fhoriz, r, y*width);
}
// do vertical
std::vector<float> tmp(height); // column before convolution
std::vector<float> tmp2(height); // column after convolution
for (int x = 0; x < width; x++) {
// copy the column out for locality
for (int y = 0; y < height; y++)
tmp[y] = r[y*width + x];
Gaussian::convolveSymmetricCentered(tmp, 0, height, fvert, tmp2, 0);
for (int y = 0; y < height; y++)
pixels[y*width + x] = tmp2[y];
}
}
void FloatImage::printMinMax() const {
std::cout << "Min: " << *min_element(pixels.begin(),pixels.end()) << ", Max: " << *max_element(pixels.begin(),pixels.end()) << std::endl;
//for (int i = 0; i < getNumFloatImagePixels(); i++)
// std::cout << "Index[" << i << "]: " << this->normalize().getFloatImagePixels()[i] << endl;
}
} // namespace

109
thirdparty/apriltag/ethz_apriltag2/src/GLine2D.cc vendored Normal file
View File

@@ -0,0 +1,109 @@
#include "apriltags/GLine2D.h"
namespace AprilTags {
GLine2D::GLine2D()
: dx(0), dy(0), p(0,0), didNormalizeSlope(false), didNormalizeP(false) {}
GLine2D::GLine2D(float slope, float b)
: dx(1), dy(slope), p(0,b), didNormalizeSlope(false), didNormalizeP(false){}
GLine2D::GLine2D(float dX, float dY, const std::pair<float,float>& pt)
: dx(dX), dy(dY), p(pt), didNormalizeSlope(false), didNormalizeP(false) {}
GLine2D::GLine2D(const std::pair<float,float>& p1, const std::pair<float,float>& p2)
: dx(p2.first - p1.first), dy(p2.second - p1.second), p(p1), didNormalizeSlope(false), didNormalizeP(false) {}
float GLine2D::getLineCoordinate(const std::pair<float,float>& pt) {
normalizeSlope();
return pt.first*dx + pt.second*dy;
}
std::pair<float,float> GLine2D::getPointOfCoordinate(float coord) {
normalizeP();
return std::pair<float,float>(p.first + coord*dx, p.second + coord*dy);
}
std::pair<float,float> GLine2D::intersectionWith(const GLine2D& line) const {
float m00 = dx;
float m01 = -line.getDx();
float m10 = dy;
float m11 = -line.getDy();
// determinant of 'm'
float det = m00*m11 - m01*m10;
// parallel lines? if so, return (-1,0).
if (fabs(det) < 1e-10)
return std::pair<float,float>(-1,0);
// inverse of 'm'
float i00 = m11/det;
// float i11 = m00/det;
float i01 = -m01/det;
// float i10 = -m10/det;
float b00 = line.getFirst() - p.first;
float b10 = line.getSecond() - p.second;
float x00 = i00*b00 + i01*b10;
return std::pair<float,float>(dx*x00+p.first, dy*x00+p.second);
}
GLine2D GLine2D::lsqFitXYW(const std::vector<XYWeight>& xyweights) {
float Cxx=0, Cyy=0, Cxy=0, Ex=0, Ey=0, mXX=0, mYY=0, mXY=0, mX=0, mY=0;
float n=0;
int idx = 0;
for (unsigned int i = 0; i < xyweights.size(); i++) {
float x = xyweights[i].x;
float y = xyweights[i].y;
float alpha = xyweights[i].weight;
mY += y*alpha;
mX += x*alpha;
mYY += y*y*alpha;
mXX += x*x*alpha;
mXY += x*y*alpha;
n += alpha;
idx++;
}
Ex = mX/n;
Ey = mY/n;
Cxx = mXX/n - MathUtil::square(mX/n);
Cyy = mYY/n - MathUtil::square(mY/n);
Cxy = mXY/n - (mX/n)*(mY/n);
// find dominant direction via SVD
float phi = 0.5f*std::atan2(-2*Cxy,(Cyy-Cxx));
// float rho = Ex*cos(phi) + Ey*sin(phi); //why is this needed if he never uses it?
std::pair<float,float> pts = std::pair<float,float>(Ex,Ey);
// compute line parameters
return GLine2D(-std::sin(phi), std::cos(phi), pts);
}
void GLine2D::normalizeSlope() {
if ( !didNormalizeSlope ) {
float mag = std::sqrt(dx*dx+dy*dy);
dx /= mag;
dy /= mag;
didNormalizeSlope=true;
}
}
void GLine2D::normalizeP() {
if ( !didNormalizeP ) {
normalizeSlope();
// we already have a point (P) on the line, and we know the line vector U
// and its perpendicular vector V: so, P'=P.*V *V
float dotprod = -dy*p.first + dx*p.second;
p = std::pair<float,float>(-dy*dotprod, dx*dotprod);
didNormalizeP = true;
}
}
} // namespace

26
thirdparty/apriltag/ethz_apriltag2/src/GLineSegment2D.cc vendored Normal file
View File

@@ -0,0 +1,26 @@
#include "apriltags/GLineSegment2D.h"
#include <limits>
namespace AprilTags {
GLineSegment2D::GLineSegment2D(const std::pair<float,float>& p0Arg, const std::pair<float,float>& p1Arg)
: line(p0Arg,p1Arg), p0(p0Arg), p1(p1Arg), weight() {}
GLineSegment2D GLineSegment2D::lsqFitXYW(const std::vector<XYWeight>& xyweight) {
GLine2D gline = GLine2D::lsqFitXYW(xyweight);
float maxcoord = -std::numeric_limits<float>::infinity();
float mincoord = std::numeric_limits<float>::infinity();;
for (unsigned int i = 0; i < xyweight.size(); i++) {
std::pair<float,float> p(xyweight[i].x, xyweight[i].y);
float coord = gline.getLineCoordinate(p);
maxcoord = std::max(maxcoord, coord);
mincoord = std::min(mincoord, coord);
}
std::pair<float,float> minValue = gline.getPointOfCoordinate(mincoord);
std::pair<float,float> maxValue = gline.getPointOfCoordinate(maxcoord);
return GLineSegment2D(minValue,maxValue);
}
} // namespace

71
thirdparty/apriltag/ethz_apriltag2/src/Gaussian.cc vendored Normal file
View File

@@ -0,0 +1,71 @@
#include "apriltags/Gaussian.h"
#include <iostream>
namespace AprilTags {
bool Gaussian::warned = false;
std::vector<float> Gaussian::makeGaussianFilter(float sigma, int n) {
std::vector<float> f(n,0.0f);
if (sigma == 0) {
for (int i = 0; i < n; i++)
f[i] = 0;
f[n/2] = 1;
return f;
}
double const inv_variance = 1./(2*sigma*sigma);
float sum = 0;
for (int i = 0; i < n; i++) {
int j = i - n/2;
f[i] = (float)std::exp(-j*j * inv_variance);
sum += f[i];
}
// normalize the gaussian
for (int i = 0; i < n; i++)
f[i] /= sum;
return f;
}
void Gaussian::convolveSymmetricCentered(const std::vector<float>& a, unsigned int aoff, unsigned int alen,
const std::vector<float>& f, std::vector<float>& r, unsigned int roff) {
if ((f.size()&1)== 0 && !warned) {
std::cout<<"convolveSymmetricCentered Warning: filter is not odd length\n";
warned = true;
}
for (size_t i = f.size()/2; i < f.size(); i++) {
double acc = 0;
for (size_t j = 0; j < f.size(); j++) {
if ((aoff + i) < j || (aoff + i) >= (alen + j))
acc += a[aoff] * f[j];
else
acc += a[aoff + i - j] * f[j];
}
r[roff + i - f.size()/2] = (float)acc;
}
for (size_t i = f.size(); i < alen; i++) {
double acc = 0;
for (unsigned int j = 0; j < f.size(); j++) {
acc += a[aoff + i - j] * f[j];
}
r[roff + i - f.size()/2] = (float)acc;
}
for (size_t i = alen; i < alen + f.size()/2; i++) {
double acc = 0;
for (size_t j = 0; j < f.size(); j++) {
if ((aoff + i) >= (alen + j) || (aoff + i) < j)
acc += a[aoff + alen - 1] * f[j];
else
acc += a[aoff + i - j] * f[j];
}
r[roff + i - f.size()/2] = (float)acc;
}
}
} // namespace

81
thirdparty/apriltag/ethz_apriltag2/src/GrayModel.cc vendored Normal file
View File

@@ -0,0 +1,81 @@
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/LU>
#include "apriltags/GrayModel.h"
namespace AprilTags {
GrayModel::GrayModel() : A(), v(), b(), nobs(0), dirty(false) {
A.setZero();
v.setZero();
b.setZero();
}
void GrayModel::addObservation(float x, float y, float gray) {
float xy = x*y;
// update only upper-right elements. A'A is symmetric,
// we'll fill the other elements in later.
A(0,0) += x*x;
A(0,1) += x*y;
A(0,2) += x*xy;
A(0,3) += x;
A(1,1) += y*y;
A(1,2) += y*xy;
A(1,3) += y;
A(2,2) += xy*xy;
A(2,3) += xy;
A(3,3) += 1;
b[0] += x*gray;
b[1] += y*gray;
b[2] += xy*gray;
b[3] += gray;
nobs++;
dirty = true;
}
float GrayModel::interpolate(float x, float y) {
if (dirty) compute();
return v[0]*x + v[1]*y + v[2]*x*y + v[3];
}
void GrayModel::compute() {
// we really only need 4 linearly independent observations to fit our answer, but we'll be very
// sensitive to noise if we don't have an over-determined system. Thus, require at least 6
// observations (or we'll use a constant model below).
dirty = false;
if (nobs >= 6) {
// make symmetric
Eigen::Matrix4d Ainv;
for (int i = 0; i < 4; i++)
for (int j = i+1; j < 4; j++)
A(j,i) = A(i,j);
// try {
// Ainv = A.inverse();
bool invertible;
double det_unused;
A.computeInverseAndDetWithCheck(Ainv, det_unused, invertible);
if (invertible) {
v = Ainv * b;
return;
}
std::cerr << "AprilTags::GrayModel::compute() has underflow in matrix inverse\n";
// }
// catch (std::underflow_error&) {
// std::cerr << "AprilTags::GrayModel::compute() has underflow in matrix inverse\n";
// }
}
// If we get here, either nobs < 6 or the matrix inverse generated
// an underflow, so use a constant model.
v.setZero(); // need the cast to avoid operator= ambiguity wrt. const-ness
v[3] = b[3] / nobs;
}
} // namespace

216
thirdparty/apriltag/ethz_apriltag2/src/Homography33.cc vendored Normal file
View File

@@ -0,0 +1,216 @@
//-*-c++-*-
#include <iostream>
#include <Eigen/Dense>
#include <opencv2/opencv.hpp>
#include "apriltags/Homography33.h"
Homography33::Homography33(const std::pair<float,float> &opticalCenter) : cxy(opticalCenter), fA(), H(), valid(false) {
fA.setZero();
H.setZero();
}
Eigen::Matrix3d& Homography33::getH() {
compute();
return H;
}
#ifdef STABLE_H
void Homography33::setCorrespondences(const std::vector< std::pair<float,float> > &sPts,
const std::vector< std::pair<float,float> > &dPts) {
valid = false;
srcPts = sPts;
dstPts = dPts;
}
#else
void Homography33::addCorrespondence(float worldx, float worldy, float imagex, float imagey) {
valid = false;
imagex -= cxy.first;
imagey -= cxy.second;
/* Here are the rows of matrix A. We will compute A'*A
* A[3*i+0][3] = -worldxyh[i][0]*imagexy[i][2];
* A[3*i+0][4] = -worldxyh[i][1]*imagexy[i][2];
* A[3*i+0][5] = -worldxyh[i][2]*imagexy[i][2];
* A[3*i+0][6] = worldxyh[i][0]*imagexy[i][1];
* A[3*i+0][7] = worldxyh[i][1]*imagexy[i][1];
* A[3*i+0][8] = worldxyh[i][2]*imagexy[i][1];
*
* A[3*i+1][0] = worldxyh[i][0]*imagexy[i][2];
* A[3*i+1][1] = worldxyh[i][1]*imagexy[i][2];
* A[3*i+1][2] = worldxyh[i][2]*imagexy[i][2];
* A[3*i+1][6] = -worldxyh[i][0]*imagexy[i][0];
* A[3*i+1][7] = -worldxyh[i][1]*imagexy[i][0];
* A[3*i+1][8] = -worldxyh[i][2]*imagexy[i][0];
*
* A[3*i+2][0] = -worldxyh[i][0]*imagexy[i][1];
* A[3*i+2][1] = -worldxyh[i][1]*imagexy[i][1];
* A[3*i+2][2] = -worldxyh[i][2]*imagexy[i][1];
* A[3*i+2][3] = worldxyh[i][0]*imagexy[i][0];
* A[3*i+2][4] = worldxyh[i][1]*imagexy[i][0];
* A[3*i+2][5] = worldxyh[i][2]*imagexy[i][0];
*/
// only update upper-right. A'A is symmetric, we'll finish the lower left later.
float a03 = -worldx;
float a04 = -worldy;
float a05 = -1;
float a06 = worldx*imagey;
float a07 = worldy*imagey;
float a08 = imagey;
fA(3, 3) += a03*a03;
fA(3, 4) += a03*a04;
fA(3, 5) += a03*a05;
fA(3, 6) += a03*a06;
fA(3, 7) += a03*a07;
fA(3, 8) += a03*a08;
fA(4, 4) += a04*a04;
fA(4, 5) += a04*a05;
fA(4, 6) += a04*a06;
fA(4, 7) += a04*a07;
fA(4, 8) += a04*a08;
fA(5, 5) += a05*a05;
fA(5, 6) += a05*a06;
fA(5, 7) += a05*a07;
fA(5, 8) += a05*a08;
fA(6, 6) += a06*a06;
fA(6, 7) += a06*a07;
fA(6, 8) += a06*a08;
fA(7, 7) += a07*a07;
fA(7, 8) += a07*a08;
fA(8, 8) += a08*a08;
float a10 = worldx;
float a11 = worldy;
float a12 = 1;
float a16 = -worldx*imagex;
float a17 = -worldy*imagex;
float a18 = -imagex;
fA(0, 0) += a10*a10;
fA(0, 1) += a10*a11;
fA(0, 2) += a10*a12;
fA(0, 6) += a10*a16;
fA(0, 7) += a10*a17;
fA(0, 8) += a10*a18;
fA(1, 1) += a11*a11;
fA(1, 2) += a11*a12;
fA(1, 6) += a11*a16;
fA(1, 7) += a11*a17;
fA(1, 8) += a11*a18;
fA(2, 2) += a12*a12;
fA(2, 6) += a12*a16;
fA(2, 7) += a12*a17;
fA(2, 8) += a12*a18;
fA(6, 6) += a16*a16;
fA(6, 7) += a16*a17;
fA(6, 8) += a16*a18;
fA(7, 7) += a17*a17;
fA(7, 8) += a17*a18;
fA(8, 8) += a18*a18;
float a20 = -worldx*imagey;
float a21 = -worldy*imagey;
float a22 = -imagey;
float a23 = worldx*imagex;
float a24 = worldy*imagex;
float a25 = imagex;
fA(0, 0) += a20*a20;
fA(0, 1) += a20*a21;
fA(0, 2) += a20*a22;
fA(0, 3) += a20*a23;
fA(0, 4) += a20*a24;
fA(0, 5) += a20*a25;
fA(1, 1) += a21*a21;
fA(1, 2) += a21*a22;
fA(1, 3) += a21*a23;
fA(1, 4) += a21*a24;
fA(1, 5) += a21*a25;
fA(2, 2) += a22*a22;
fA(2, 3) += a22*a23;
fA(2, 4) += a22*a24;
fA(2, 5) += a22*a25;
fA(3, 3) += a23*a23;
fA(3, 4) += a23*a24;
fA(3, 5) += a23*a25;
fA(4, 4) += a24*a24;
fA(4, 5) += a24*a25;
fA(5, 5) += a25*a25;
}
#endif
#ifdef STABLE_H
void Homography33::compute() {
if ( valid ) return;
std::vector<cv::Point2f> sPts;
std::vector<cv::Point2f> dPts;
for (int i=0; i<4; i++) {
sPts.push_back(cv::Point2f(srcPts[i].first, srcPts[i].second));
}
for (int i=0; i<4; i++) {
dPts.push_back(cv::Point2f(dstPts[i].first - cxy.first, dstPts[i].second - cxy.second));
}
cv::Mat homography = cv::findHomography(sPts, dPts);
for (int c=0; c<3; c++) {
for (int r=0; r<3; r++) {
H(r,c) = homography.at<double>(r,c);
}
}
valid = true;
}
#else
void Homography33::compute() {
if ( valid ) return;
// make symmetric
for (int i = 0; i < 9; i++)
for (int j = i+1; j < 9; j++)
fA(j,i) = fA(i,j);
Eigen::JacobiSVD<Eigen::MatrixXd> svd(fA, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::MatrixXd eigV = svd.matrixV();
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
H(i,j) = eigV(i*3+j, eigV.cols()-1);
}
}
valid = true;
}
#endif
std::pair<float,float> Homography33::project(float worldx, float worldy) {
compute();
std::pair<float,float> ixy;
ixy.first = H(0,0)*worldx + H(0,1)*worldy + H(0,2);
ixy.second = H(1,0)*worldx + H(1,1)*worldy + H(1,2);
float z = H(2,0)*worldx + H(2,1)*worldy + H(2,2);
ixy.first = ixy.first/z + cxy.first;
ixy.second = ixy.second/z + cxy.second;
return ixy;
}

11
thirdparty/apriltag/ethz_apriltag2/src/MathUtil.cc vendored Normal file
View File

@@ -0,0 +1,11 @@
#include "apriltags/MathUtil.h"
namespace AprilTags{
// Output operator for std::pair<float,float>, useful for debugging
std::ostream& operator<<(std::ostream &os, const std::pair<float,float> &pt) {
os << pt.first << "," << pt.second;
return os;
}
} // namespace

159
thirdparty/apriltag/ethz_apriltag2/src/Quad.cc vendored Normal file
View File

@@ -0,0 +1,159 @@
#include <Eigen/Dense>
#include "apriltags/FloatImage.h"
#include "apriltags/MathUtil.h"
#include "apriltags/GLine2D.h"
#include "apriltags/Quad.h"
#include "apriltags/Segment.h"
namespace AprilTags {
const float Quad::maxQuadAspectRatio = 32;
Quad::Quad(const std::vector< std::pair<float,float> >& p, const std::pair<float,float>& opticalCenter)
: quadPoints(p), segments(), observedPerimeter(), homography(opticalCenter) {
#ifdef STABLE_H
std::vector< std::pair<float,float> > srcPts;
srcPts.push_back(std::make_pair(-1, -1));
srcPts.push_back(std::make_pair( 1, -1));
srcPts.push_back(std::make_pair( 1, 1));
srcPts.push_back(std::make_pair(-1, 1));
homography.setCorrespondences(srcPts, p);
#else
homography.addCorrespondence(-1, -1, quadPoints[0].first, quadPoints[0].second);
homography.addCorrespondence( 1, -1, quadPoints[1].first, quadPoints[1].second);
homography.addCorrespondence( 1, 1, quadPoints[2].first, quadPoints[2].second);
homography.addCorrespondence(-1, 1, quadPoints[3].first, quadPoints[3].second);
#endif
#ifdef INTERPOLATE
p0 = Eigen::Vector2f(p[0].first, p[0].second);
p3 = Eigen::Vector2f(p[3].first, p[3].second);
p01 = (Eigen::Vector2f(p[1].first, p[1].second) - p0);
p32 = (Eigen::Vector2f(p[2].first, p[2].second) - p3);
#endif
}
std::pair<float,float> Quad::interpolate(float x, float y) {
#ifdef INTERPOLATE
Eigen::Vector2f r1 = p0 + p01 * (x+1.)/2.;
Eigen::Vector2f r2 = p3 + p32 * (x+1.)/2.;
Eigen::Vector2f r = r1 + (r2-r1) * (y+1)/2;
return std::pair<float,float>(r(0), r(1));
#else
return homography.project(x,y);
#endif
}
std::pair<float,float> Quad::interpolate01(float x, float y) {
return interpolate(2*x-1, 2*y-1);
}
void Quad::search(const FloatImage& fImage, std::vector<Segment*>& path,
Segment& parent, int depth, std::vector<Quad>& quads,
const std::pair<float,float>& opticalCenter) {
// cout << "Searching segment " << parent.getId() << ", depth=" << depth << ", #children=" << parent.children.size() << endl;
// terminal depth occurs when we've found four segments.
if (depth == 4) {
// cout << "Entered terminal depth" << endl; // debug code
// Is the first segment the same as the last segment (i.e., a loop?)
if (path[4] == path[0]) {
// the 4 corners of the quad as computed by the intersection of segments.
std::vector< std::pair<float,float> > p(4);
float calculatedPerimeter = 0;
bool bad = false;
for (int i = 0; i < 4; i++) {
// compute intersections between all the lines. This will give us
// sub-pixel accuracy for the corners of the quad.
GLine2D linea(std::make_pair(path[i]->getX0(),path[i]->getY0()),
std::make_pair(path[i]->getX1(),path[i]->getY1()));
GLine2D lineb(std::make_pair(path[i+1]->getX0(),path[i+1]->getY0()),
std::make_pair(path[i+1]->getX1(),path[i+1]->getY1()));
p[i] = linea.intersectionWith(lineb);
calculatedPerimeter += path[i]->getLength();
// no intersection? Occurs when the lines are almost parallel.
if (p[i].first == -1)
bad = true;
}
// cout << "bad = " << bad << endl;
// eliminate quads that don't form a simply connected loop, i.e., those
// that form an hour glass, or wind the wrong way.
if (!bad) {
float t0 = std::atan2(p[1].second-p[0].second, p[1].first-p[0].first);
float t1 = std::atan2(p[2].second-p[1].second, p[2].first-p[1].first);
float t2 = std::atan2(p[3].second-p[2].second, p[3].first-p[2].first);
float t3 = std::atan2(p[0].second-p[3].second, p[0].first-p[3].first);
// double ttheta = fmod(t1-t0, 2*M_PI) + fmod(t2-t1, 2*M_PI) +
// fmod(t3-t2, 2*M_PI) + fmod(t0-t3, 2*M_PI);
float ttheta = MathUtil::mod2pi(t1-t0) + MathUtil::mod2pi(t2-t1) +
MathUtil::mod2pi(t3-t2) + MathUtil::mod2pi(t0-t3);
// cout << "ttheta=" << ttheta << endl;
// the magic value is -2*PI. It should be exact,
// but we allow for (lots of) numeric imprecision.
if (ttheta < -7 || ttheta > -5)
bad = true;
}
if (!bad) {
float d0 = MathUtil::distance2D(p[0], p[1]);
float d1 = MathUtil::distance2D(p[1], p[2]);
float d2 = MathUtil::distance2D(p[2], p[3]);
float d3 = MathUtil::distance2D(p[3], p[0]);
float d4 = MathUtil::distance2D(p[0], p[2]);
float d5 = MathUtil::distance2D(p[1], p[3]);
// check sizes
if (d0 < Quad::minimumEdgeLength || d1 < Quad::minimumEdgeLength || d2 < Quad::minimumEdgeLength ||
d3 < Quad::minimumEdgeLength || d4 < Quad::minimumEdgeLength || d5 < Quad::minimumEdgeLength) {
bad = true;
// cout << "tagsize too small" << endl;
}
// check aspect ratio
float dmax = max(max(d0,d1), max(d2,d3));
float dmin = min(min(d0,d1), min(d2,d3));
if (dmax > dmin*Quad::maxQuadAspectRatio) {
bad = true;
// cout << "aspect ratio too extreme" << endl;
}
}
if (!bad) {
Quad q(p, opticalCenter);
q.segments=path;
q.observedPerimeter = calculatedPerimeter;
quads.push_back(q);
}
}
return;
}
// if (depth >= 1) // debug code
//cout << "depth: " << depth << endl;
// Not terminal depth. Recurse on any children that obey the correct handedness.
for (unsigned int i = 0; i < parent.children.size(); i++) {
Segment &child = *parent.children[i];
// cout << " Child " << child.getId() << ": ";
// (handedness was checked when we created the children)
// we could rediscover each quad 4 times (starting from
// each corner). If we had an arbitrary ordering over
// points, we can eliminate the redundant detections by
// requiring that the first corner have the lowest
// value. We're arbitrarily going to use theta...
if ( child.getTheta() > path[0]->getTheta() ) {
// cout << "theta failed: " << child.getTheta() << " > " << path[0]->getTheta() << endl;
continue;
}
path[depth+1] = &child;
search(fImage, path, child, depth+1, quads, opticalCenter);
}
}
} // namespace

21
thirdparty/apriltag/ethz_apriltag2/src/Segment.cc vendored Normal file
View File

@@ -0,0 +1,21 @@
#include "apriltags/Segment.h"
#include <iostream>
namespace AprilTags {
const float Segment::minimumLineLength = 4;
Segment::Segment()
: children(), x0(0), y0(0), x1(0), y1(0), theta(0), length(0), segmentId(++idCounter) {}
float Segment::segmentLength() {
return std::sqrt((x1-x0)*(x1-x0) + (y1-y0)*(y1-y0));
}
void Segment::printSegment() {
std::cout <<"("<< x0 <<","<< y0 <<"), "<<"("<< x1 <<","<< y1 <<")" << std::endl;
}
int Segment::idCounter = 0;
} // namespace

161
thirdparty/apriltag/ethz_apriltag2/src/TagDetection.cc vendored Normal file
View File

@@ -0,0 +1,161 @@
#include "opencv2/opencv.hpp"
#include "apriltags/TagDetection.h"
#include "apriltags/MathUtil.h"
#ifdef PLATFORM_APERIOS
//missing/broken isnan
namespace std {
static bool isnan(float x) {
const int EXP = 0x7f800000;
const int FRAC = 0x007fffff;
const int y = *((int*)(&x));
return ((y&EXP)==EXP && (y&FRAC)!=0);
}
}
#endif
namespace AprilTags {
TagDetection::TagDetection()
: good(false), obsCode(), code(), id(), hammingDistance(), rotation(), p(),
cxy(), observedPerimeter(), homography(), hxy() {
homography.setZero();
}
TagDetection::TagDetection(int _id)
: good(false), obsCode(), code(), id(_id), hammingDistance(), rotation(), p(),
cxy(), observedPerimeter(), homography(), hxy() {
homography.setZero();
}
float TagDetection::getXYOrientation() const {
// Because the order of segments in a quad is arbitrary, so is the
// homography's rotation, so we can't determine orientation directly
// from the homography. Instead, use the homography to find two
// bottom corners of a properly oriented tag in pixel coordinates,
// and then compute orientation from that.
std::pair<float,float> p0 = interpolate(-1,-1); // lower left corner of tag
std::pair<float,float> p1 = interpolate(1,-1); // lower right corner of tag
float orient = atan2(p1.second - p0.second, p1.first - p0.first);
return ! std::isnan(float(orient)) ? orient : 0.;
}
std::pair<float,float> TagDetection::interpolate(float x, float y) const {
float z = homography(2,0)*x + homography(2,1)*y + homography(2,2);
if ( z == 0 )
return std::pair<float,float>(0,0); // prevents returning a pair with a -NaN, for which gcc 4.4 flubs isnan
float newx = (homography(0,0)*x + homography(0,1)*y + homography(0,2))/z + hxy.first;
float newy = (homography(1,0)*x + homography(1,1)*y + homography(1,2))/z + hxy.second;
return std::pair<float,float>(newx,newy);
}
bool TagDetection::overlapsTooMuch(const TagDetection &other) const {
// Compute a sort of "radius" of the two targets. We'll do this by
// computing the average length of the edges of the quads (in
// pixels).
float radius =
( MathUtil::distance2D(p[0], p[1]) +
MathUtil::distance2D(p[1], p[2]) +
MathUtil::distance2D(p[2], p[3]) +
MathUtil::distance2D(p[3], p[0]) +
MathUtil::distance2D(other.p[0], other.p[1]) +
MathUtil::distance2D(other.p[1], other.p[2]) +
MathUtil::distance2D(other.p[2], other.p[3]) +
MathUtil::distance2D(other.p[3], other.p[0]) ) / 16.0f;
// distance (in pixels) between two tag centers
float dist = MathUtil::distance2D(cxy, other.cxy);
// reject pairs where the distance between centroids is smaller than
// the "radius" of one of the tags.
return ( dist < radius );
}
Eigen::Matrix4d TagDetection::getRelativeTransform(double tag_size, double fx, double fy, double px, double py) const {
std::vector<cv::Point3f> objPts;
std::vector<cv::Point2f> imgPts;
double s = tag_size/2.;
objPts.push_back(cv::Point3f(-s,-s, 0));
objPts.push_back(cv::Point3f( s,-s, 0));
objPts.push_back(cv::Point3f( s, s, 0));
objPts.push_back(cv::Point3f(-s, s, 0));
std::pair<float, float> p1 = p[0];
std::pair<float, float> p2 = p[1];
std::pair<float, float> p3 = p[2];
std::pair<float, float> p4 = p[3];
imgPts.push_back(cv::Point2f(p1.first, p1.second));
imgPts.push_back(cv::Point2f(p2.first, p2.second));
imgPts.push_back(cv::Point2f(p3.first, p3.second));
imgPts.push_back(cv::Point2f(p4.first, p4.second));
cv::Mat rvec, tvec;
cv::Matx33f cameraMatrix(
fx, 0, px,
0, fy, py,
0, 0, 1);
cv::Vec4f distParam(0,0,0,0); // all 0?
cv::solvePnP(objPts, imgPts, cameraMatrix, distParam, rvec, tvec);
cv::Matx33d r;
cv::Rodrigues(rvec, r);
Eigen::Matrix3d wRo;
wRo << r(0,0), r(0,1), r(0,2), r(1,0), r(1,1), r(1,2), r(2,0), r(2,1), r(2,2);
Eigen::Matrix4d T;
T.topLeftCorner(3,3) = wRo;
T.col(3).head(3) << tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
T.row(3) << 0,0,0,1;
return T;
}
void TagDetection::getRelativeTranslationRotation(double tag_size, double fx, double fy, double px, double py,
Eigen::Vector3d& trans, Eigen::Matrix3d& rot) const {
Eigen::Matrix4d T =
getRelativeTransform(tag_size, fx, fy, px, py);
// converting from camera frame (z forward, x right, y down) to
// object frame (x forward, y left, z up)
Eigen::Matrix4d M;
M <<
0, 0, 1, 0,
-1, 0, 0, 0,
0, -1, 0, 0,
0, 0, 0, 1;
Eigen::Matrix4d MT = M*T;
// translation vector from camera to the April tag
trans = MT.col(3).head(3);
// orientation of April tag with respect to camera: the camera
// convention makes more sense here, because yaw,pitch,roll then
// naturally agree with the orientation of the object
rot = T.block(0,0,3,3);
}
// draw one April tag detection on actual image
void TagDetection::draw(cv::Mat& image) const {
// use corner points detected by line intersection
std::pair<float, float> p1 = p[0];
std::pair<float, float> p2 = p[1];
std::pair<float, float> p3 = p[2];
std::pair<float, float> p4 = p[3];
// plot outline
cv::line(image, cv::Point2f(p1.first, p1.second), cv::Point2f(p2.first, p2.second), cv::Scalar(255,0,0,0) );
cv::line(image, cv::Point2f(p2.first, p2.second), cv::Point2f(p3.first, p3.second), cv::Scalar(0,255,0,0) );
cv::line(image, cv::Point2f(p3.first, p3.second), cv::Point2f(p4.first, p4.second), cv::Scalar(0,0,255,0) );
cv::line(image, cv::Point2f(p4.first, p4.second), cv::Point2f(p1.first, p1.second), cv::Scalar(255,0,255,0) );
// mark center
cv::circle(image, cv::Point2f(cxy.first, cxy.second), 8, cv::Scalar(0,0,255,0), 2);
// print ID
std::ostringstream strSt;
strSt << "#" << id;
cv::putText(image, strSt.str(),
cv::Point2f(cxy.first + 10, cxy.second + 10),
cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0,0,255));
}
} // namespace

613
thirdparty/apriltag/ethz_apriltag2/src/TagDetector.cc vendored Normal file
View File

@@ -0,0 +1,613 @@
#include <algorithm>
#include <climits>
#include <cmath>
#include <iostream>
#include <map>
#include <vector>
#include <Eigen/Dense>
#include "apriltags/Edge.h"
#include "apriltags/FloatImage.h"
#include "apriltags/GLine2D.h"
#include "apriltags/GLineSegment2D.h"
#include "apriltags/Gaussian.h"
#include "apriltags/GrayModel.h"
#include "apriltags/Gridder.h"
#include "apriltags/Homography33.h"
#include "apriltags/MathUtil.h"
#include "apriltags/Quad.h"
#include "apriltags/Segment.h"
#include "apriltags/TagFamily.h"
#include "apriltags/UnionFindSimple.h"
#include "apriltags/XYWeight.h"
#include "apriltags/TagDetector.h"
//#define DEBUG_APRIL
#ifdef DEBUG_APRIL
#include <opencv/cv.h>
#include <opencv/highgui.h>
#endif
using namespace std;
namespace AprilTags {
std::vector<TagDetection> TagDetector::extractTags(const cv::Mat &image) {
// convert to internal AprilTags image (todo: slow, change internally to
// OpenCV)
int width = image.cols;
int height = image.rows;
AprilTags::FloatImage fimOrig(width, height);
int i = 0;
for (int y = 0; y < height; y++) {
for (int x = 0; x < width; x++) {
fimOrig.set(x, y, image.data[i] / 255.);
i++;
}
}
std::pair<int, int> opticalCenter(width / 2, height / 2);
#ifdef DEBUG_APRIL
#if 0
{ // debug - write
int height_ = fimOrig.getHeight();
int width_ = fimOrig.getWidth();
cv::Mat image(height_, width_, CV_8UC3);
{
for (int y=0; y<height_; y++) {
for (int x=0; x<width_; x++) {
cv::Vec3b v;
// float vf = fimMag.get(x,y);
float vf = fimOrig.get(x,y);
int val = (int)(vf * 255.);
if ((val & 0xffff00) != 0) {printf("problem... %i\n", val);}
for (int k=0; k<3; k++) {
v(k) = val;
}
image.at<cv::Vec3b>(y, x) = v;
}
}
}
imwrite("out.bmp", image);
}
#endif
#if 0
FloatImage fimOrig = fimOrig_;
{ // debug - read
cv::Mat image = cv::imread("test.bmp");
int height_ = fimOrig.getHeight();
int width_ = fimOrig.getWidth();
{
for (int y=0; y<height_; y++) {
for (int x=0; x<width_; x++) {
cv::Vec3b v = image.at<cv::Vec3b>(y,x);
float val = (float)v(0)/255.;
fimOrig.set(x,y,val);
}
}
}
}
#endif
#endif
//================================================================
// Step one: preprocess image (convert to grayscale) and low pass if necessary
FloatImage fim = fimOrig;
//! Gaussian smoothing kernel applied to image (0 == no filter).
/*! Used when sampling bits. Filtering is a good idea in cases
* where A) a cheap camera is introducing artifical sharpening, B)
* the bayer pattern is creating artifcats, C) the sensor is very
* noisy and/or has hot/cold pixels. However, filtering makes it
* harder to decode very small tags. Reasonable values are 0, or
* [0.8, 1.5].
*/
float sigma = 0;
//! Gaussian smoothing kernel applied to image (0 == no filter).
/*! Used when detecting the outline of the box. It is almost always
* useful to have some filtering, since the loss of small details
* won't hurt. Recommended value = 0.8. The case where sigma ==
* segsigma has been optimized to avoid a redundant filter
* operation.
*/
float segSigma = 0.8f;
if (sigma > 0) {
int filtsz = ((int)max(3.0f, 3 * sigma)) | 1;
std::vector<float> filt = Gaussian::makeGaussianFilter(sigma, filtsz);
fim.filterFactoredCentered(filt, filt);
}
//================================================================
// Step two: Compute the local gradient. We store the direction and magnitude.
// This step is quite sensitve to noise, since a few bad theta estimates will
// break up segments, causing us to miss Quads. It is useful to do a Gaussian
// low pass on this step even if we don't want it for encoding.
FloatImage fimSeg;
if (segSigma > 0) {
if (segSigma == sigma) {
fimSeg = fim;
} else {
// blur anew
int filtsz = ((int)max(3.0f, 3 * segSigma)) | 1;
std::vector<float> filt = Gaussian::makeGaussianFilter(segSigma, filtsz);
fimSeg = fimOrig;
fimSeg.filterFactoredCentered(filt, filt);
}
} else {
fimSeg = fimOrig;
}
FloatImage fimTheta(fimSeg.getWidth(), fimSeg.getHeight());
FloatImage fimMag(fimSeg.getWidth(), fimSeg.getHeight());
for (int y = 1; y < fimSeg.getHeight() - 1; y++) {
for (int x = 1; x < fimSeg.getWidth() - 1; x++) {
float Ix = fimSeg.get(x + 1, y) - fimSeg.get(x - 1, y);
float Iy = fimSeg.get(x, y + 1) - fimSeg.get(x, y - 1);
float mag = Ix * Ix + Iy * Iy;
#if 0 // kaess: fast version, but maybe less accurate?
float theta = MathUtil::fast_atan2(Iy, Ix);
#else
float theta = atan2(Iy, Ix);
#endif
fimTheta.set(x, y, theta);
fimMag.set(x, y, mag);
}
}
#ifdef DEBUG_APRIL
int height_ = fimSeg.getHeight();
int width_ = fimSeg.getWidth();
cv::Mat dbgImage(height_, width_, CV_8UC3);
{
for (int y = 0; y < height_; y++) {
for (int x = 0; x < width_; x++) {
cv::Vec3b v;
// float vf = fimMag.get(x,y);
float vf = fimOrig.get(x, y);
int val = (int)(vf * 255.);
if ((val & 0xffff00) != 0) {
printf("problem... %i\n", val);
}
for (int k = 0; k < 3; k++) {
v(k) = val;
}
dbgImage.at<cv::Vec3b>(y, x) = v;
}
}
}
#endif
//================================================================
// Step three. Extract edges by grouping pixels with similar
// thetas together. This is a greedy algorithm: we start with
// the most similar pixels. We use 4-connectivity.
UnionFindSimple uf(fimSeg.getWidth() * fimSeg.getHeight());
vector<Edge> edges(width * height * 4);
size_t nEdges = 0;
// Bounds on the thetas assigned to this group. Note that because
// theta is periodic, these are defined such that the average
// value is contained *within* the interval.
{ // limit scope of storage
/* Previously all this was on the stack, but this is 1.2MB for 320x240
* images
* That's already a problem for OS X (default 512KB thread stack size),
* could be a problem elsewhere for bigger images... so store on heap */
vector<float> storage(
width * height *
4); // do all the memory in one big block, exception safe
float *tmin = &storage[width * height * 0];
float *tmax = &storage[width * height * 1];
float *mmin = &storage[width * height * 2];
float *mmax = &storage[width * height * 3];
for (int y = 0; y + 1 < height; y++) {
for (int x = 0; x + 1 < width; x++) {
float mag0 = fimMag.get(x, y);
if (mag0 < Edge::minMag) continue;
mmax[y * width + x] = mag0;
mmin[y * width + x] = mag0;
float theta0 = fimTheta.get(x, y);
tmin[y * width + x] = theta0;
tmax[y * width + x] = theta0;
// Calculates then adds edges to 'vector<Edge> edges'
Edge::calcEdges(theta0, x, y, fimTheta, fimMag, edges, nEdges);
// XXX Would 8 connectivity help for rotated tags?
// Probably not much, so long as input filtering hasn't been disabled.
}
}
edges.resize(nEdges);
std::stable_sort(edges.begin(), edges.end());
Edge::mergeEdges(edges, uf, tmin, tmax, mmin, mmax);
}
//================================================================
// Step four: Loop over the pixels again, collecting statistics for each
// cluster.
// We will soon fit lines (segments) to these points.
map<int, vector<XYWeight> > clusters;
for (int y = 0; y + 1 < fimSeg.getHeight(); y++) {
for (int x = 0; x + 1 < fimSeg.getWidth(); x++) {
if (uf.getSetSize(y * fimSeg.getWidth() + x) <
Segment::minimumSegmentSize)
continue;
int rep = (int)uf.getRepresentative(y * fimSeg.getWidth() + x);
map<int, vector<XYWeight> >::iterator it = clusters.find(rep);
if (it == clusters.end()) {
clusters[rep] = vector<XYWeight>();
it = clusters.find(rep);
}
vector<XYWeight> &points = it->second;
points.push_back(XYWeight(x, y, fimMag.get(x, y)));
}
}
//================================================================
// Step five: Loop over the clusters, fitting lines (which we call Segments).
std::vector<Segment> segments; // used in Step six
std::map<int, std::vector<XYWeight> >::const_iterator clustersItr;
for (clustersItr = clusters.begin(); clustersItr != clusters.end();
clustersItr++) {
std::vector<XYWeight> points = clustersItr->second;
GLineSegment2D gseg = GLineSegment2D::lsqFitXYW(points);
// filter short lines
float length = MathUtil::distance2D(gseg.getP0(), gseg.getP1());
if (length < Segment::minimumLineLength) continue;
Segment seg;
float dy = gseg.getP1().second - gseg.getP0().second;
float dx = gseg.getP1().first - gseg.getP0().first;
float tmpTheta = std::atan2(dy, dx);
seg.setTheta(tmpTheta);
seg.setLength(length);
// We add an extra semantic to segments: the vector
// p1->p2 will have dark on the left, white on the right.
// To do this, we'll look at every gradient and each one
// will vote for which way they think the gradient should
// go. This is way more retentive than necessary: we
// could probably sample just one point!
float flip = 0, noflip = 0;
for (unsigned int i = 0; i < points.size(); i++) {
XYWeight xyw = points[i];
float theta = fimTheta.get((int)xyw.x, (int)xyw.y);
float mag = fimMag.get((int)xyw.x, (int)xyw.y);
// err *should* be +M_PI/2 for the correct winding, but if we
// got the wrong winding, it'll be around -M_PI/2.
float err = MathUtil::mod2pi(theta - seg.getTheta());
if (err < 0)
noflip += mag;
else
flip += mag;
}
if (flip > noflip) {
float temp = seg.getTheta() + (float)M_PI;
seg.setTheta(temp);
}
float dot = dx * std::cos(seg.getTheta()) + dy * std::sin(seg.getTheta());
if (dot > 0) {
seg.setX0(gseg.getP1().first);
seg.setY0(gseg.getP1().second);
seg.setX1(gseg.getP0().first);
seg.setY1(gseg.getP0().second);
} else {
seg.setX0(gseg.getP0().first);
seg.setY0(gseg.getP0().second);
seg.setX1(gseg.getP1().first);
seg.setY1(gseg.getP1().second);
}
segments.push_back(seg);
}
#ifdef DEBUG_APRIL
#if 0
{
for (vector<Segment>::iterator it = segments.begin(); it!=segments.end(); it++) {
long int r = random();
cv::line(dbgImage,
cv::Point2f(it->getX0(), it->getY0()),
cv::Point2f(it->getX1(), it->getY1()),
cv::Scalar(r%0xff,(r%0xff00)>>8,(r%0xff0000)>>16,0) );
}
}
#endif
#endif
// Step six: For each segment, find segments that begin where this segment
// ends.
// (We will chain segments together next...) The gridder accelerates the
// search by
// building (essentially) a 2D hash table.
Gridder<Segment> gridder(0, 0, width, height, 10);
// add every segment to the hash table according to the position of the
// segment's
// first point. Remember that the first point has a specific meaning due to
// our
// left-hand rule above.
for (unsigned int i = 0; i < segments.size(); i++) {
gridder.add(segments[i].getX0(), segments[i].getY0(), &segments[i]);
}
// Now, find child segments that begin where each parent segment ends.
for (unsigned i = 0; i < segments.size(); i++) {
Segment &parentseg = segments[i];
// compute length of the line segment
GLine2D parentLine(
std::pair<float, float>(parentseg.getX0(), parentseg.getY0()),
std::pair<float, float>(parentseg.getX1(), parentseg.getY1()));
Gridder<Segment>::iterator iter = gridder.find(
parentseg.getX1(), parentseg.getY1(), 0.5f * parentseg.getLength());
while (iter.hasNext()) {
Segment &child = iter.next();
if (MathUtil::mod2pi(child.getTheta() - parentseg.getTheta()) > 0) {
continue;
}
// compute intersection of points
GLine2D childLine(std::pair<float, float>(child.getX0(), child.getY0()),
std::pair<float, float>(child.getX1(), child.getY1()));
std::pair<float, float> p = parentLine.intersectionWith(childLine);
if (p.first == -1) {
continue;
}
float parentDist = MathUtil::distance2D(
p, std::pair<float, float>(parentseg.getX1(), parentseg.getY1()));
float childDist = MathUtil::distance2D(
p, std::pair<float, float>(child.getX0(), child.getY0()));
if (max(parentDist, childDist) > parentseg.getLength()) {
// cout << "intersection too far" << endl;
continue;
}
// everything's OK, this child is a reasonable successor.
parentseg.children.push_back(&child);
}
}
//================================================================
// Step seven: Search all connected segments to see if any form a loop of
// length 4.
// Add those to the quads list.
vector<Quad> quads;
vector<Segment *> tmp(5);
for (unsigned int i = 0; i < segments.size(); i++) {
tmp[0] = &segments[i];
Quad::search(fimOrig, tmp, segments[i], 0, quads, opticalCenter);
}
#ifdef DEBUG_APRIL
{
for (unsigned int qi = 0; qi < quads.size(); qi++) {
Quad &quad = quads[qi];
std::pair<float, float> p1 = quad.quadPoints[0];
std::pair<float, float> p2 = quad.quadPoints[1];
std::pair<float, float> p3 = quad.quadPoints[2];
std::pair<float, float> p4 = quad.quadPoints[3];
cv::line(dbgImage, cv::Point2f(p1.first, p1.second),
cv::Point2f(p2.first, p2.second), cv::Scalar(0, 0, 255, 0));
cv::line(dbgImage, cv::Point2f(p2.first, p2.second),
cv::Point2f(p3.first, p3.second), cv::Scalar(0, 0, 255, 0));
cv::line(dbgImage, cv::Point2f(p3.first, p3.second),
cv::Point2f(p4.first, p4.second), cv::Scalar(0, 0, 255, 0));
cv::line(dbgImage, cv::Point2f(p4.first, p4.second),
cv::Point2f(p1.first, p1.second), cv::Scalar(0, 0, 255, 0));
p1 = quad.interpolate(-1, -1);
p2 = quad.interpolate(-1, 1);
p3 = quad.interpolate(1, 1);
p4 = quad.interpolate(1, -1);
cv::circle(dbgImage, cv::Point2f(p1.first, p1.second), 3,
cv::Scalar(0, 255, 0, 0), 1);
cv::circle(dbgImage, cv::Point2f(p2.first, p2.second), 3,
cv::Scalar(0, 255, 0, 0), 1);
cv::circle(dbgImage, cv::Point2f(p3.first, p3.second), 3,
cv::Scalar(0, 255, 0, 0), 1);
cv::circle(dbgImage, cv::Point2f(p4.first, p4.second), 3,
cv::Scalar(0, 255, 0, 0), 1);
}
cv::imshow("debug_april", dbgImage);
}
#endif
//================================================================
// Step eight. Decode the quads. For each quad, we first estimate a
// threshold color to decide between 0 and 1. Then, we read off the
// bits and see if they make sense.
std::vector<TagDetection> detections;
for (unsigned int qi = 0; qi < quads.size(); qi++) {
Quad &quad = quads[qi];
// Find a threshold
GrayModel blackModel, whiteModel;
const int dd = 2 * thisTagFamily.blackBorder + thisTagFamily.dimension;
for (int iy = -1; iy <= dd; iy++) {
float y = (iy + 0.5f) / dd;
for (int ix = -1; ix <= dd; ix++) {
float x = (ix + 0.5f) / dd;
std::pair<float, float> pxy = quad.interpolate01(x, y);
int irx = (int)(pxy.first + 0.5);
int iry = (int)(pxy.second + 0.5);
if (irx < 0 || irx >= width || iry < 0 || iry >= height) continue;
float v = fim.get(irx, iry);
if (iy == -1 || iy == dd || ix == -1 || ix == dd)
whiteModel.addObservation(x, y, v);
else if (iy == 0 || iy == (dd - 1) || ix == 0 || ix == (dd - 1))
blackModel.addObservation(x, y, v);
}
}
bool bad = false;
unsigned long long tagCode = 0;
for (int iy = thisTagFamily.dimension - 1; iy >= 0; iy--) {
float y = (thisTagFamily.blackBorder + iy + 0.5f) / dd;
for (int ix = 0; ix < thisTagFamily.dimension; ix++) {
float x = (thisTagFamily.blackBorder + ix + 0.5f) / dd;
std::pair<float, float> pxy = quad.interpolate01(x, y);
int irx = (int)(pxy.first + 0.5);
int iry = (int)(pxy.second + 0.5);
if (irx < 0 || irx >= width || iry < 0 || iry >= height) {
// cout << "*** bad: irx=" << irx << " iry=" << iry << endl;
bad = true;
continue;
}
float threshold =
(blackModel.interpolate(x, y) + whiteModel.interpolate(x, y)) *
0.5f;
float v = fim.get(irx, iry);
tagCode = tagCode << 1;
if (v > threshold) tagCode |= 1;
#ifdef DEBUG_APRIL
{
if (v > threshold)
cv::circle(dbgImage, cv::Point2f(irx, iry), 1,
cv::Scalar(0, 0, 255, 0), 2);
else
cv::circle(dbgImage, cv::Point2f(irx, iry), 1,
cv::Scalar(0, 255, 0, 0), 2);
}
#endif
}
}
if (!bad) {
TagDetection thisTagDetection;
thisTagFamily.decode(thisTagDetection, tagCode);
// compute the homography (and rotate it appropriately)
thisTagDetection.homography = quad.homography.getH();
thisTagDetection.hxy = quad.homography.getCXY();
float c = std::cos(thisTagDetection.rotation * (float)M_PI / 2);
float s = std::sin(thisTagDetection.rotation * (float)M_PI / 2);
Eigen::Matrix3d R;
R.setZero();
R(0, 0) = R(1, 1) = c;
R(0, 1) = -s;
R(1, 0) = s;
R(2, 2) = 1;
Eigen::Matrix3d tmp;
tmp = thisTagDetection.homography * R;
thisTagDetection.homography = tmp;
// Rotate points in detection according to decoded
// orientation. Thus the order of the points in the
// detection object can be used to determine the
// orientation of the target.
std::pair<float, float> bottomLeft = thisTagDetection.interpolate(-1, -1);
int bestRot = -1;
float bestDist = FLT_MAX;
for (int i = 0; i < 4; i++) {
float const dist =
AprilTags::MathUtil::distance2D(bottomLeft, quad.quadPoints[i]);
if (dist < bestDist) {
bestDist = dist;
bestRot = i;
}
}
for (int i = 0; i < 4; i++)
thisTagDetection.p[i] = quad.quadPoints[(i + bestRot) % 4];
if (thisTagDetection.good) {
thisTagDetection.cxy = quad.interpolate01(0.5f, 0.5f);
thisTagDetection.observedPerimeter = quad.observedPerimeter;
detections.push_back(thisTagDetection);
}
}
}
#ifdef DEBUG_APRIL
{ cv::imshow("debug_april", dbgImage); }
#endif
//================================================================
// Step nine: Some quads may be detected more than once, due to
// partial occlusion and our aggressive attempts to recover from
// broken lines. When two quads (with the same id) overlap, we will
// keep the one with the lowest error, and if the error is the same,
// the one with the greatest observed perimeter.
std::vector<TagDetection> goodDetections;
// NOTE: allow multiple non-overlapping detections of the same target.
for (vector<TagDetection>::const_iterator it = detections.begin();
it != detections.end(); it++) {
const TagDetection &thisTagDetection = *it;
bool newFeature = true;
for (unsigned int odidx = 0; odidx < goodDetections.size(); odidx++) {
TagDetection &otherTagDetection = goodDetections[odidx];
if (thisTagDetection.id != otherTagDetection.id ||
!thisTagDetection.overlapsTooMuch(otherTagDetection))
continue;
// There's a conflict. We must pick one to keep.
newFeature = false;
// This detection is worse than the previous one... just don't use it.
if (thisTagDetection.hammingDistance > otherTagDetection.hammingDistance)
continue;
// Otherwise, keep the new one if it either has strictly *lower* error, or
// greater perimeter.
if (thisTagDetection.hammingDistance <
otherTagDetection.hammingDistance ||
thisTagDetection.observedPerimeter >
otherTagDetection.observedPerimeter)
goodDetections[odidx] = thisTagDetection;
}
if (newFeature) goodDetections.push_back(thisTagDetection);
}
// cout << "AprilTags: edges=" << nEdges << " clusters=" << clusters.size() <<
// " segments=" << segments.size()
// << " quads=" << quads.size() << " detections=" << detections.size() <<
// " unique tags=" << goodDetections.size() << endl;
return goodDetections;
}
} // namespace

138
thirdparty/apriltag/ethz_apriltag2/src/TagFamily.cc vendored Normal file
View File

@@ -0,0 +1,138 @@
#include <iostream>
#include "apriltags/TagFamily.h"
/**
// example of instantiation of tag family:
#include "apriltags/TagFamily.h"
#include "apriltags/Tag36h11.h"
TagFamily *tag36h11 = new TagFamily(tagCodes36h11);
// available tag families:
#include "apriltags/Tag16h5.h"
#include "apriltags/Tag16h5_other.h"
#include "apriltags/Tag25h7.h"
#include "apriltags/Tag25h9.h"
#include "apriltags/Tag36h11.h"
#include "apriltags/Tag36h11_other.h"
#include "apriltags/Tag36h9.h"
*/
namespace AprilTags {
TagFamily::TagFamily(const TagCodes& tagCodes, const size_t blackBorder)
: blackBorder(blackBorder), bits(tagCodes.bits), dimension((int)std::sqrt((float)bits)),
minimumHammingDistance(tagCodes.minHammingDistance),
errorRecoveryBits(1), codes() {
if ( bits != dimension*dimension )
cerr << "Error: TagFamily constructor called with bits=" << bits << "; must be a square number!" << endl;
codes = tagCodes.codes;
}
void TagFamily::setErrorRecoveryBits(int b) {
errorRecoveryBits = b;
}
void TagFamily::setErrorRecoveryFraction(float v) {
errorRecoveryBits = (int) (((int) (minimumHammingDistance-1)/2)*v);
}
unsigned long long TagFamily::rotate90(unsigned long long w, int d) {
unsigned long long wr = 0;
const unsigned long long oneLongLong = 1;
for (int r = d-1; r>=0; r--) {
for (int c = 0; c<d; c++) {
int b = r + d*c;
wr = wr<<1;
if ((w & (oneLongLong<<b)) != 0)
wr |= 1;
}
}
return wr;
}
int TagFamily::hammingDistance(unsigned long long a, unsigned long long b) {
return popCount(a^b);
}
unsigned char TagFamily::popCountReal(unsigned long long w) {
unsigned char cnt = 0;
while (w != 0) {
w &= (w-1);
++cnt;
}
return cnt;
}
int TagFamily::popCount(unsigned long long w) {
int count = 0;
while (w != 0) {
count += popCountTable[(unsigned int) (w & (popCountTableSize-1))];
w >>= popCountTableShift;
}
return count;
}
void TagFamily::decode(TagDetection& det, unsigned long long rCode) const {
int bestId = -1;
int bestHamming = INT_MAX;
int bestRotation = 0;
unsigned long long bestCode = 0;
unsigned long long rCodes[4];
rCodes[0] = rCode;
rCodes[1] = rotate90(rCodes[0], dimension);
rCodes[2] = rotate90(rCodes[1], dimension);
rCodes[3] = rotate90(rCodes[2], dimension);
for (unsigned int id = 0; id < codes.size(); id++) {
for (unsigned int rot = 0; rot < 4; rot++) {
int thisHamming = hammingDistance(rCodes[rot], codes[id]);
if (thisHamming < bestHamming) {
bestHamming = thisHamming;
bestRotation = rot;
bestId = id;
bestCode = codes[id];
}
}
}
det.id = bestId;
det.hammingDistance = bestHamming;
det.rotation = bestRotation;
det.good = (det.hammingDistance <= errorRecoveryBits);
det.obsCode = rCode;
det.code = bestCode;
}
void TagFamily::printHammingDistances() const {
vector<int> hammings(dimension*dimension+1);
for (unsigned i = 0; i < codes.size(); i++) {
unsigned long long r0 = codes[i];
unsigned long long r1 = rotate90(r0, dimension);
unsigned long long r2 = rotate90(r1, dimension);
unsigned long long r3 = rotate90(r2, dimension);
for (unsigned int j = i+1; j < codes.size(); j++) {
int d = min(min(hammingDistance(r0, codes[j]),
hammingDistance(r1, codes[j])),
min(hammingDistance(r2, codes[j]),
hammingDistance(r3, codes[j])));
hammings[d]++;
}
}
for (unsigned int i = 0; i < hammings.size(); i++)
printf("hammings: %u = %d\n", i, hammings[i]);
}
unsigned char TagFamily::popCountTable[TagFamily::popCountTableSize];
TagFamily::TableInitializer TagFamily::initializer;
} // namespace

54
thirdparty/apriltag/ethz_apriltag2/src/UnionFindSimple.cc vendored Normal file
View File

@@ -0,0 +1,54 @@
#include "apriltags/UnionFindSimple.h"
#include <iostream>
namespace AprilTags {
int UnionFindSimple::getRepresentative(int thisId) {
// terminal case: a node is its own parent
if (data[thisId].id == thisId)
return thisId;
// otherwise, recurse...
int root = getRepresentative(data[thisId].id);
// short circuit the path
data[thisId].id = root;
return root;
}
void UnionFindSimple::printDataVector() const {
for (unsigned int i = 0; i < data.size(); i++)
std::cout << "data[" << i << "]: " << " id:" << data[i].id << " size:" << data[i].size << std::endl;
}
int UnionFindSimple::connectNodes(int aId, int bId) {
int aRoot = getRepresentative(aId);
int bRoot = getRepresentative(bId);
if (aRoot == bRoot)
return aRoot;
int asz = data[aRoot].size;
int bsz = data[bRoot].size;
if (asz > bsz) {
data[bRoot].id = aRoot;
data[aRoot].size += bsz;
return aRoot;
} else {
data[aRoot].id = bRoot;
data[bRoot].size += asz;
return bRoot;
}
}
void UnionFindSimple::init() {
for (unsigned int i = 0; i < data.size(); i++) {
// everyone is their own cluster of size 1
data[i].id = i;
data[i].size = 1;
}
}
} // namespace

9
thirdparty/apriltag/ethz_apriltag2/src/example/CMakeLists.txt vendored Normal file
View File

@@ -0,0 +1,9 @@
cmake_minimum_required (VERSION 2.6)
link_libraries(apriltags)
add_executable(apriltags_demo apriltags_demo.cpp Serial.cpp)
pods_install_executables(apriltags_demo)
add_executable(imu imu.cpp Serial.cpp)
pods_install_executables(imu)

113
thirdparty/apriltag/ethz_apriltag2/src/example/Serial.cpp vendored Normal file
View File

@@ -0,0 +1,113 @@
/**
* @file Serial.cpp
* @brief Simple serial interface, for example to talk to Arduino.
* @author: Michael Kaess
*/
#include <fcntl.h>
#include <termios.h>
#include <stdlib.h>
#include <iostream>
#include <sstream>
#include <unistd.h>
#include "Serial.h"
using namespace std;
// open a serial port connection
void Serial::open(const string& port, int rate) {
m_serialPort = ::open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
if (m_serialPort==-1) {
cout << "Unable to open serial port" << endl;
exit(1);
}
fcntl(m_serialPort, F_SETFL,0); // O_NONBLOCK might be needed for write...
struct termios port_settings; // structure to store the port settings in
tcgetattr(m_serialPort, &port_settings); // get current settings
speed_t b;
switch(rate) {
case(9600):
b = B9600;
break;
case(19200):
b = B19200;
break;
case(38400):
b = B38400;
break;
case(115200):
b = B115200;
break;
default:
cout << "Error: Unknown baud rate requested in Serial.open()" << endl;
exit(1);
}
cfsetispeed(&port_settings, b); // set baud rates
cfsetospeed(&port_settings, b);
port_settings.c_cflag &= ~PARENB; // set no parity, stop bits, 8 data bits
port_settings.c_cflag &= ~CSTOPB;
port_settings.c_cflag &= ~CSIZE;
port_settings.c_cflag |= CS8;
tcsetattr(m_serialPort, TCSANOW, &port_settings); // apply the settings to the port
}
// read a single character
int Serial::read() const {
unsigned char result;
if (::read(m_serialPort, &result, 1) == 1) {
return (int)result;
} else {
return -1;
}
}
// read until special character up to a maximum number of bytes
string Serial::readBytesUntil(unsigned char until, int max_length) {
string result(max_length, ' ');
int n = 0;
int c;
do {
c = read();
if (c<0) { // wait for more characters
usleep(100);
} else {
result[n] = (unsigned char)c;
n++;
}
} while ((c != (int)until) && (n < max_length));
result.resize(n);
return result;
}
// send a string
void Serial::print(string str) const {
int res = ::write(m_serialPort, str.c_str(), str.length());
}
// send an integer
void Serial::print(int num) const {
stringstream stream;
stream << num;
string str = stream.str();
print(str);
}
// send a double
void Serial::print(double num) const {
stringstream stream;
stream << num;
string str = stream.str();
print(str);
}
// send a float
void Serial::print(float num) const {
print((double)num);
}

41
thirdparty/apriltag/ethz_apriltag2/src/example/Serial.h vendored Normal file
View File

@@ -0,0 +1,41 @@
/**
* @file Serial.h
* @brief Simple serial interface, for example to talk to Arduino.
* @author: Michael Kaess
*/
#pragma once
#include <string>
class Serial {
int m_serialPort; // file description for the serial port
public:
Serial() : m_serialPort(-1) {}
// open a serial port connection
void open(const std::string& port, int rate = 115200);
// read a single character
int read() const;
// read until special character up to a maximum number of bytes
std::string readBytesUntil(unsigned char until, int length = 300);
// send a string
void print(std::string str) const;
// send an integer
void print(int num) const;
// send a double
void print(double num) const;
// send a float
void print(float num) const;
};

453
thirdparty/apriltag/ethz_apriltag2/src/example/apriltags_demo.cpp vendored Normal file
View File

@@ -0,0 +1,453 @@
/**
* @file april_tags.cpp
* @brief Example application for April tags library
* @author: Michael Kaess
*
* Opens the first available camera (typically a built in camera in a
* laptop) and continuously detects April tags in the incoming
* images. Detections are both visualized in the live image and shown
* in the text console. Optionally allows selecting of a specific
* camera in case multiple ones are present and specifying image
* resolution as long as supported by the camera. Also includes the
* option to send tag detections via a serial port, for example when
* running on a Raspberry Pi that is connected to an Arduino.
*/
using namespace std;
#include <iostream>
#include <cstring>
#include <vector>
#include <sys/time.h>
const string usage = "\n"
"Usage:\n"
" apriltags_demo [OPTION...] [deviceID]\n"
"\n"
"Options:\n"
" -h -? Show help options\n"
" -a Arduino (send tag ids over serial port)\n"
" -d disable graphics\n"
" -C <bbxhh> Tag family (default 36h11)\n"
" -F <fx> Focal length in pixels\n"
" -W <width> Image width (default 640, availability depends on camera)\n"
" -H <height> Image height (default 480, availability depends on camera)\n"
" -S <size> Tag size (square black frame) in meters\n"
" -E <exposure> Manually set camera exposure (default auto; range 0-10000)\n"
" -G <gain> Manually set camera gain (default auto; range 0-255)\n"
" -B <brightness> Manually set the camera brightness (default 128; range 0-255)\n"
"\n";
const string intro = "\n"
"April tags test code\n"
"(C) 2012-2013 Massachusetts Institute of Technology\n"
"Michael Kaess\n"
"\n";
#ifndef __APPLE__
#define EXPOSURE_CONTROL // only works in Linux
#endif
#ifdef EXPOSURE_CONTROL
#include <libv4l2.h>
#include <linux/videodev2.h>
#include <fcntl.h>
#include <errno.h>
#endif
// OpenCV library for easy access to USB camera and drawing of images
// on screen
#include "opencv2/opencv.hpp"
// April tags detector and various families that can be selected by command line option
#include "apriltags/TagDetector.h"
#include "apriltags/Tag16h5.h"
#include "apriltags/Tag25h7.h"
#include "apriltags/Tag25h9.h"
#include "apriltags/Tag36h9.h"
#include "apriltags/Tag36h11.h"
// Needed for getopt / command line options processing
#include <unistd.h>
extern int optind;
extern char *optarg;
// For Arduino: locally defined serial port access class
#include "Serial.h"
const char* window_name = "apriltags_demo";
// utility function to provide current system time (used below in
// determining frame rate at which images are being processed)
double tic() {
struct timeval t;
gettimeofday(&t, NULL);
return ((double)t.tv_sec + ((double)t.tv_usec)/1000000.);
}
#include <cmath>
#ifndef PI
const double PI = 3.14159265358979323846;
#endif
const double TWOPI = 2.0*PI;
/**
* Normalize angle to be within the interval [-pi,pi].
*/
inline double standardRad(double t) {
if (t >= 0.) {
t = fmod(t+PI, TWOPI) - PI;
} else {
t = fmod(t-PI, -TWOPI) + PI;
}
return t;
}
void wRo_to_euler(const Eigen::Matrix3d& wRo, double& yaw, double& pitch, double& roll) {
yaw = standardRad(atan2(wRo(1,0), wRo(0,0)));
double c = cos(yaw);
double s = sin(yaw);
pitch = standardRad(atan2(-wRo(2,0), wRo(0,0)*c + wRo(1,0)*s));
roll = standardRad(atan2(wRo(0,2)*s - wRo(1,2)*c, -wRo(0,1)*s + wRo(1,1)*c));
}
class Demo {
AprilTags::TagDetector* m_tagDetector;
AprilTags::TagCodes m_tagCodes;
bool m_draw; // draw image and April tag detections?
bool m_arduino; // send tag detections to serial port?
int m_width; // image size in pixels
int m_height;
double m_tagSize; // April tag side length in meters of square black frame
double m_fx; // camera focal length in pixels
double m_fy;
double m_px; // camera principal point
double m_py;
int m_deviceId; // camera id (in case of multiple cameras)
cv::VideoCapture m_cap;
int m_exposure;
int m_gain;
int m_brightness;
Serial m_serial;
public:
// default constructor
Demo() :
// default settings, most can be modified through command line options (see below)
m_tagDetector(NULL),
m_tagCodes(AprilTags::tagCodes36h11),
m_draw(true),
m_arduino(false),
m_width(640),
m_height(480),
m_tagSize(0.166),
m_fx(600),
m_fy(600),
m_px(m_width/2),
m_py(m_height/2),
m_exposure(-1),
m_gain(-1),
m_brightness(-1),
m_deviceId(0)
{}
// changing the tag family
void setTagCodes(string s) {
if (s=="16h5") {
m_tagCodes = AprilTags::tagCodes16h5;
} else if (s=="25h7") {
m_tagCodes = AprilTags::tagCodes25h7;
} else if (s=="25h9") {
m_tagCodes = AprilTags::tagCodes25h9;
} else if (s=="36h9") {
m_tagCodes = AprilTags::tagCodes36h9;
} else if (s=="36h11") {
m_tagCodes = AprilTags::tagCodes36h11;
} else {
cout << "Invalid tag family specified" << endl;
exit(1);
}
}
// parse command line options to change default behavior
void parseOptions(int argc, char* argv[]) {
int c;
while ((c = getopt(argc, argv, ":h?adC:F:H:S:W:E:G:B:")) != -1) {
// Each option character has to be in the string in getopt();
// the first colon changes the error character from '?' to ':';
// a colon after an option means that there is an extra
// parameter to this option; 'W' is a reserved character
switch (c) {
case 'h':
case '?':
cout << intro;
cout << usage;
exit(0);
break;
case 'a':
m_arduino = true;
break;
case 'd':
m_draw = false;
break;
case 'C':
setTagCodes(optarg);
break;
case 'F':
m_fx = atof(optarg);
m_fy = m_fx;
break;
case 'H':
m_height = atoi(optarg);
m_py = m_height/2;
break;
case 'S':
m_tagSize = atof(optarg);
break;
case 'W':
m_width = atoi(optarg);
m_px = m_width/2;
break;
case 'E':
#ifndef EXPOSURE_CONTROL
cout << "Error: Exposure option (-E) not available" << endl;
exit(1);
#endif
m_exposure = atoi(optarg);
break;
case 'G':
#ifndef EXPOSURE_CONTROL
cout << "Error: Gain option (-G) not available" << endl;
exit(1);
#endif
m_gain = atoi(optarg);
break;
case 'B':
#ifndef EXPOSURE_CONTROL
cout << "Error: Brightness option (-B) not available" << endl;
exit(1);
#endif
m_brightness = atoi(optarg);
break;
case ':': // unknown option, from getopt
cout << intro;
cout << usage;
exit(1);
break;
}
}
if (argc == optind + 1) {
m_deviceId = atoi(argv[optind]);
}
}
void setup() {
m_tagDetector = new AprilTags::TagDetector(m_tagCodes);
#ifdef EXPOSURE_CONTROL
// manually setting camera exposure settings; OpenCV/v4l1 doesn't
// support exposure control; so here we manually use v4l2 before
// opening the device via OpenCV; confirmed to work with Logitech
// C270; try exposure=20, gain=100, brightness=150
string video_str = "/dev/video0";
video_str[10] = '0' + m_deviceId;
int device = v4l2_open(video_str.c_str(), O_RDWR | O_NONBLOCK);
if (m_exposure >= 0) {
// not sure why, but v4l2_set_control() does not work for
// V4L2_CID_EXPOSURE_AUTO...
struct v4l2_control c;
c.id = V4L2_CID_EXPOSURE_AUTO;
c.value = 1; // 1=manual, 3=auto; V4L2_EXPOSURE_AUTO fails...
if (v4l2_ioctl(device, VIDIOC_S_CTRL, &c) != 0) {
cout << "Failed to set... " << strerror(errno) << endl;
}
cout << "exposure: " << m_exposure << endl;
v4l2_set_control(device, V4L2_CID_EXPOSURE_ABSOLUTE, m_exposure*6);
}
if (m_gain >= 0) {
cout << "gain: " << m_gain << endl;
v4l2_set_control(device, V4L2_CID_GAIN, m_gain*256);
}
if (m_brightness >= 0) {
cout << "brightness: " << m_brightness << endl;
v4l2_set_control(device, V4L2_CID_BRIGHTNESS, m_brightness*256);
}
v4l2_close(device);
#endif
// find and open a USB camera (built in laptop camera, web cam etc)
m_cap = cv::VideoCapture(m_deviceId);
if(!m_cap.isOpened()) {
cerr << "ERROR: Can't find video device " << m_deviceId << "\n";
exit(1);
}
m_cap.set(CV_CAP_PROP_FRAME_WIDTH, m_width);
m_cap.set(CV_CAP_PROP_FRAME_HEIGHT, m_height);
cout << "Camera successfully opened (ignore error messages above...)" << endl;
cout << "Actual resolution: "
<< m_cap.get(CV_CAP_PROP_FRAME_WIDTH) << "x"
<< m_cap.get(CV_CAP_PROP_FRAME_HEIGHT) << endl;
// prepare window for drawing the camera images
if (m_draw) {
cv::namedWindow(window_name, 1);
}
// optional: prepare serial port for communication with Arduino
if (m_arduino) {
m_serial.open("/dev/ttyACM0");
}
}
void print_detection(AprilTags::TagDetection& detection) const {
cout << " Id: " << detection.id
<< " (Hamming: " << detection.hammingDistance << ")";
// recovering the relative pose of a tag:
// NOTE: for this to be accurate, it is necessary to use the
// actual camera parameters here as well as the actual tag size
// (m_fx, m_fy, m_px, m_py, m_tagSize)
Eigen::Vector3d translation;
Eigen::Matrix3d rotation;
detection.getRelativeTranslationRotation(m_tagSize, m_fx, m_fy, m_px, m_py,
translation, rotation);
Eigen::Matrix3d F;
F <<
1, 0, 0,
0, -1, 0,
0, 0, 1;
Eigen::Matrix3d fixed_rot = F*rotation;
double yaw, pitch, roll;
wRo_to_euler(fixed_rot, yaw, pitch, roll);
cout << " distance=" << translation.norm()
<< "m, x=" << translation(0)
<< ", y=" << translation(1)
<< ", z=" << translation(2)
<< ", yaw=" << yaw
<< ", pitch=" << pitch
<< ", roll=" << roll
<< endl;
// Also note that for SLAM/multi-view application it is better to
// use reprojection error of corner points, because the noise in
// this relative pose is very non-Gaussian; see iSAM source code
// for suitable factors.
}
// The processing loop where images are retrieved, tags detected,
// and information about detections generated
void loop() {
cv::Mat image;
cv::Mat image_gray;
int frame = 0;
double last_t = tic();
while (true) {
// capture frame
m_cap >> image;
// alternative way is to grab, then retrieve; allows for
// multiple grab when processing below frame rate - v4l keeps a
// number of frames buffered, which can lead to significant lag
// m_cap.grab();
// m_cap.retrieve(image);
// detect April tags (requires a gray scale image)
cv::cvtColor(image, image_gray, CV_BGR2GRAY);
vector<AprilTags::TagDetection> detections = m_tagDetector->extractTags(image_gray);
// print out each detection
cout << detections.size() << " tags detected:" << endl;
for (int i=0; i<detections.size(); i++) {
print_detection(detections[i]);
}
// show the current image including any detections
if (m_draw) {
for (int i=0; i<detections.size(); i++) {
// also highlight in the image
detections[i].draw(image);
}
imshow(window_name, image); // OpenCV call
}
// optionally send tag information to serial port (e.g. to Arduino)
if (m_arduino) {
if (detections.size() > 0) {
// only the first detected tag is sent out for now
Eigen::Vector3d translation;
Eigen::Matrix3d rotation;
detections[0].getRelativeTranslationRotation(m_tagSize, m_fx, m_fy, m_px, m_py,
translation, rotation);
m_serial.print(detections[0].id);
m_serial.print(",");
m_serial.print(translation(0));
m_serial.print(",");
m_serial.print(translation(1));
m_serial.print(",");
m_serial.print(translation(2));
m_serial.print("\n");
} else {
// no tag detected: tag ID = -1
m_serial.print("-1,0.0,0.0,0.0\n");
}
}
// print out the frame rate at which image frames are being processed
frame++;
if (frame % 10 == 0) {
double t = tic();
cout << " " << 10./(t-last_t) << " fps" << endl;
last_t = t;
}
// exit if any key is pressed
if (cv::waitKey(1) >= 0) break;
}
}
}; // Demo
// here is were everything begins
int main(int argc, char* argv[]) {
Demo demo;
// process command line options
demo.parseOptions(argc, argv);
// setup image source, window for drawing, serial port...
demo.setup();
// the actual processing loop where tags are detected and visualized
demo.loop();
return 0;
}

40
thirdparty/apriltag/ethz_apriltag2/src/example/arduino_tags/arduino_tags.ino vendored Normal file
View File

@@ -0,0 +1,40 @@
/*
ArduinoTag
Turns on an LED whenever Raspberry Pi sees an Apriltag.
Michael Kaess 04/13
*/
// pin number for LED
int led = 9;
// information about detected tag
int tagId;
float x, y, z;
// runs once when the program starts
void setup() {
// open serial port
Serial.begin(115200);
// initialize pin as output
pinMode(led, OUTPUT);
}
// runs over and over again
void loop() {
// check if new data is available
if (Serial.available() > 0) {
tagId = Serial.parseInt();
x = Serial.parseFloat();
y = Serial.parseFloat();
z = Serial.parseFloat();
Serial.read(); // ignore newline character
if (tagId >= 0) {
digitalWrite(led, HIGH);
} else {
digitalWrite(led, LOW);
}
}
// wait for 20ms before checking again for new data
delay(20);
}

64
thirdparty/apriltag/ethz_apriltag2/src/example/imu.cpp vendored Normal file
View File

@@ -0,0 +1,64 @@
// Example of reading from ArduIMU (v3) via serial port
// Michael Kaess, May 2013
#include <iostream>
#include <string>
#include <stdio.h>
#include <math.h>
#include "Serial.h"
using namespace std;
void parse(string s) {
// note that ex (the rotation matrix) and mg? (the magnetormeter
// readings) below are optional, depending on how the ArduIMU was
// configured
float version; // ArduIMU code version
float gyroX, gyroY, gyroZ; // raw gyroscope values
float accelX, accelY, accelZ; // raw accelerometer values
// int ex[9]; // rotation matrix (scaled by 10^7)
float roll, pitch, yaw; // Euler angles
int imuh; // IMU health
// float mgx, mgy, mgz, mgh; // magnetometer readings
int tow; // GPS time
// cout << s;
// try to parse the line
int num = sscanf(s.c_str(),
//"!!!VER:%g,AN0:%g,AN1:%g,AN2:%g,AN3:%g,AN4:%g,AN5:%g,EX0:%i,EX1:%i,EX2:%i,EX3:%i,EX4:%i,EX5:%i,EX6:%i,EX7:%i,EX8:%i,RLL:%g,PCH:%g,YAW:%g,IMUH:%i,TOW:%i***",
"!!!VER:%g,AN0:%g,AN1:%g,AN2:%g,AN3:%g,AN4:%g,AN5:%g,RLL:%g,PCH:%g,YAW:%g,IMUH:%i,TOW:%i",
//"!!!VER:%g,AN0:%g,AN1:%g,AN2:%g,AN3:%g,AN4:%g,AN5:%g,RLL:%g,PCH:%g,YAW:%g,IMUH:%i,MGX:%g,MGY:%g,MGZ:%g,MGH:%g,TOW:%i",
&version, &gyroX, &gyroY, &gyroZ, &accelX, &accelY, &accelZ,
// &ex[0], &ex[1], &ex[2], &ex[3], &ex[4], &ex[5], &ex[6], &ex[7], &ex[8],
&roll, &pitch, &yaw,
&imuh,
// &mgx, &mgy, &mgz, &mgh,
&tow);
// did we read the correct number of entries, or did the line contain other information?
if (num==12 || num==16 || num==21) {
cout << "Euler angles: " << yaw << "," << pitch << "," << roll << endl;
} else {
// cout << "Could not parse string " << num << endl;
}
}
int main() {
Serial serial;
serial.open("/dev/ttyUSB0", 38400); // might need to change to your USB port
// read and parse one line at a time
while (true) {
string s = serial.readBytesUntil('\n');
parse(s);
}
return 0;
}

27
thirdparty/apriltag/include/basalt/utils/apriltag.h vendored Normal file
View File

@@ -0,0 +1,27 @@
#include <basalt/image/image.h>
#include <basalt/utils/sophus_utils.hpp>
#include <vector>
namespace basalt {
struct ApriltagDetectorData;
class ApriltagDetector {
public:
ApriltagDetector();
~ApriltagDetector();
void detectTags(basalt::ManagedImage<uint16_t>& img_raw,
Eigen::aligned_vector<Eigen::Vector2d>& corners,
std::vector<int>& ids, std::vector<double>& radii,
Eigen::aligned_vector<Eigen::Vector2d>& corners_rejected,
std::vector<int>& ids_rejected,
std::vector<double>& radii_rejected);
private:
ApriltagDetectorData* data;
};
} // namespace basalt

249
thirdparty/apriltag/src/apriltag.cpp vendored Normal file
View File

@@ -0,0 +1,249 @@
#include <basalt/utils/apriltag.h>
#include <apriltags/TagDetector.h>
#include <apriltags/Tag36h11.h>
namespace basalt {
struct ApriltagDetectorData {
ApriltagDetectorData()
: doSubpixRefinement(true),
maxSubpixDisplacement(0),
minTagsForValidObs(4),
minBorderDistance(4.0),
blackTagBorder(2),
_tagCodes(AprilTags::tagCodes36h11) {
_tagDetector =
std::make_shared<AprilTags::TagDetector>(_tagCodes, blackTagBorder);
}
bool doSubpixRefinement;
double
maxSubpixDisplacement; //!< maximum displacement for subpixel refinement.
//!< If 0, only base it on tag size.
unsigned int minTagsForValidObs;
double minBorderDistance;
unsigned int blackTagBorder;
AprilTags::TagCodes _tagCodes;
std::shared_ptr<AprilTags::TagDetector> _tagDetector;
inline int size() { return 36 * 4; }
};
ApriltagDetector::ApriltagDetector() { data = new ApriltagDetectorData; }
ApriltagDetector::~ApriltagDetector() { delete data; }
void ApriltagDetector::detectTags(
basalt::ManagedImage<uint16_t>& img_raw,
Eigen::aligned_vector<Eigen::Vector2d>& corners, std::vector<int>& ids,
std::vector<double>& radii,
Eigen::aligned_vector<Eigen::Vector2d>& corners_rejected,
std::vector<int>& ids_rejected, std::vector<double>& radii_rejected) {
corners.clear();
ids.clear();
radii.clear();
corners_rejected.clear();
ids_rejected.clear();
radii_rejected.clear();
cv::Mat image(img_raw.h, img_raw.w, CV_8U);
uint8_t* dst = image.ptr();
const uint16_t* src = img_raw.ptr;
for (size_t i = 0; i < img_raw.size(); i++) {
dst[i] = (src[i] >> 8);
}
// detect the tags
std::vector<AprilTags::TagDetection> detections =
data->_tagDetector->extractTags(image);
/* handle the case in which a tag is identified but not all tag
* corners are in the image (all data bits in image but border
* outside). tagCorners should still be okay as apriltag-lib
* extrapolates them, only the subpix refinement will fail
*/
// min. distance [px] of tag corners from image border (tag is not used if
// violated)
std::vector<AprilTags::TagDetection>::iterator iter = detections.begin();
for (iter = detections.begin(); iter != detections.end();) {
// check all four corners for violation
bool remove = false;
for (int j = 0; j < 4; j++) {
remove |= iter->p[j].first < data->minBorderDistance;
remove |= iter->p[j].first >
(float)(image.cols) - data->minBorderDistance; // width
remove |= iter->p[j].second < data->minBorderDistance;
remove |= iter->p[j].second >
(float)(image.rows) - data->minBorderDistance; // height
}
// also remove tags that are flagged as bad
if (iter->good != 1) remove |= true;
// also remove if the tag ID is out-of-range for this grid (faulty
// detection)
if (iter->id >= (int)data->size() / 4) remove |= true;
// delete flagged tags
if (remove) {
// delete the tag and advance in list
iter = detections.erase(iter);
} else {
// advance in list
++iter;
}
}
// did we find enough tags?
if (detections.size() < data->minTagsForValidObs) return;
// sort detections by tagId
std::sort(detections.begin(), detections.end(),
AprilTags::TagDetection::sortByIdCompare);
// check for duplicate tagIds (--> if found: wild Apriltags in image not
// belonging to calibration target)
// (only if we have more than 1 tag...)
if (detections.size() > 1) {
for (unsigned i = 0; i < detections.size() - 1; i++)
if (detections[i].id == detections[i + 1].id) {
std::cerr << "Wild Apriltag detected. Hide them!" << std::endl;
return;
}
}
// compute search radius for sub-pixel refinement depending on size of tag in
// image
std::vector<double> radiiRaw;
for (unsigned i = 0; i < detections.size(); i++) {
const double minimalRadius = 2.0;
const double percentOfSideLength = 7.5;
const double avgSideLength =
static_cast<double>(detections[i].observedPerimeter) / 4.0;
// use certain percentage of average side length as radius
// subtract 1.0 since this radius is for displacement threshold; Search
// region is slightly larger
radiiRaw.emplace_back(std::max(
minimalRadius, (percentOfSideLength / 100.0 * avgSideLength) - 1.0));
}
// convert corners to cv::Mat (4 consecutive corners form one tag)
/// point ordering here
/// 11-----10 15-----14
/// | TAG 2 | | TAG 3 |
/// 8-------9 12-----13
/// 3-------2 7-------6
/// y | TAG 0 | | TAG 1 |
/// ^ 0-------1 4-------5
/// |-->x
cv::Mat tagCorners(4 * detections.size(), 2, CV_32F);
for (unsigned i = 0; i < detections.size(); i++) {
for (unsigned j = 0; j < 4; j++) {
tagCorners.at<float>(4 * i + j, 0) = detections[i].p[j].first;
tagCorners.at<float>(4 * i + j, 1) = detections[i].p[j].second;
}
}
// store a copy of the corner list before subpix refinement
cv::Mat tagCornersRaw = tagCorners.clone();
// optional subpixel refinement on all tag corners (four corners each tag)
if (data->doSubpixRefinement) {
for (size_t i = 0; i < detections.size(); i++) {
cv::Mat currentCorners(4, 2, CV_32F);
for (unsigned j = 0; j < 4; j++) {
currentCorners.at<float>(j, 0) = tagCorners.at<float>(4 * i + j, 0);
currentCorners.at<float>(j, 1) = tagCorners.at<float>(4 * i + j, 1);
}
const int radius = static_cast<int>(std::ceil(radiiRaw[i] + 1.0));
cv::cornerSubPix(
image, currentCorners, cv::Size(radius, radius), cv::Size(-1, -1),
cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER,
100, 0.01));
for (unsigned j = 0; j < 4; j++) {
tagCorners.at<float>(4 * i + j, 0) = currentCorners.at<float>(j, 0);
tagCorners.at<float>(4 * i + j, 1) = currentCorners.at<float>(j, 1);
}
}
}
// insert the observed points into the correct location of the grid point
// array
/// point ordering
/// 12-----13 14-----15
/// | TAG 2 | | TAG 3 |
/// 8-------9 10-----11
/// 4-------5 6-------7
/// y | TAG 0 | | TAG 1 |
/// ^ 0-------1 2-------3
/// |-->x
for (unsigned int i = 0; i < detections.size(); i++) {
// get the tag id
int tagId = detections[i].id;
// check maximum displacement from subpixel refinement
const double radius = radiiRaw[i];
const double tagMaxDispl2 = radius * radius;
const double globalMaxDispl2 =
data->maxSubpixDisplacement * data->maxSubpixDisplacement;
const double subpixRefinementThreshold2 =
globalMaxDispl2 > 0 ? std::min(globalMaxDispl2, tagMaxDispl2)
: tagMaxDispl2;
// add four points per tag
for (int j = 0; j < 4; j++) {
int pointId = (tagId << 2) + j;
// refined corners
double corner_x = tagCorners.row(4 * i + j).at<float>(0);
double corner_y = tagCorners.row(4 * i + j).at<float>(1);
// raw corners
double cornerRaw_x = tagCornersRaw.row(4 * i + j).at<float>(0);
double cornerRaw_y = tagCornersRaw.row(4 * i + j).at<float>(1);
// only add point if the displacement in the subpixel refinement is below
// a given threshold
double subpix_displacement_squarred =
(corner_x - cornerRaw_x) * (corner_x - cornerRaw_x) +
(corner_y - cornerRaw_y) * (corner_y - cornerRaw_y);
// add all points, but only set active if the point has not moved to far
// in the subpix refinement
// TODO: We still get a few false positives here, e.g. when the whole
// search region lies on an edge but the actual corner is not included.
// Maybe what we would need to do is actually checking a "corner
// score" vs "edge score" after refinement and discard all corners
// that are not more "cornery" than "edgy". Another possible issue
// might be corners, where (due to fisheye distortion), neighboring
// corners are in the search radius. For those we should check if in
// the radius there is really a clear single maximum in the corner
// score and otherwise discard the corner.
if (subpix_displacement_squarred <= subpixRefinementThreshold2) {
corners.emplace_back(corner_x, corner_y);
ids.emplace_back(pointId);
radii.emplace_back(radius);
} else {
corners_rejected.emplace_back(corner_x, corner_y);
ids_rejected.emplace_back(pointId);
radii_rejected.emplace_back(radius);
}
}
}
}
} // namespace basalt