int g2d_polygon_rasterize(const zarray_t *poly, double y, double *x)
{
int sz = zarray_size(poly);
g2d_line_t line;
if (1) {
double p0[2] = { 0, y };
double p1[2] = { 1, y };
g2d_line_init_from_points(&line, p0, p1);
}
int xpos = 0;
for (int i = 0; i < sz; i++) {
g2d_line_segment_t seg;
double *p0, *p1;
zarray_get_volatile(poly, i, &p0);
zarray_get_volatile(poly, (i+1)%sz, &p1);
g2d_line_segment_init_from_points(&seg, p0, p1);
double q[2];
if (g2d_line_segment_intersect_line(&seg, &line, q))
x[xpos++] = q[0];
}
qsort(x, xpos, sizeof(double), double_sort_up);
return xpos;
}
void *worker_thread(void *p)
{
workerpool_t *wp = (workerpool_t*) p;
int cnt = 0;
while (1) {
struct task *task;
pthread_mutex_lock(&wp->mutex);
while (wp->taskspos == zarray_size(wp->tasks)) {
wp->end_count++;
// printf("%"PRId64" thread %d did %d\n", utime_now(), pthread_self(), cnt);
pthread_cond_broadcast(&wp->endcond);
pthread_cond_wait(&wp->startcond, &wp->mutex);
cnt = 0;
// printf("%"PRId64" thread %d awake\n", utime_now(), pthread_self());
}
zarray_get_volatile(wp->tasks, wp->taskspos, &task);
wp->taskspos++;
cnt++;
pthread_mutex_unlock(&wp->mutex);
// pthread_yield();
sched_yield();
// we've been asked to exit.
if (task->f == NULL)
return NULL;
task->f(task->p);
}
return NULL;
}
void workerpool_run_single(workerpool_t *wp)
{
for (int i = 0; i < zarray_size(wp->tasks); i++) {
struct task *task;
zarray_get_volatile(wp->tasks, i, &task);
task->f(task->p);
}
zarray_clear(wp->tasks);
}
// Find point p on the boundary of poly that is closest to q.
void g2d_polygon_closest_boundary_point(const zarray_t *poly, const double q[2], double *p)
{
int psz = zarray_size(poly);
double min_dist = HUGE;
for (int i = 0; i < psz; i++) {
double *p0, *p1;
zarray_get_volatile(poly, i, &p0);
zarray_get_volatile(poly, (i+1) % psz, &p1);
g2d_line_segment_t seg;
g2d_line_segment_init_from_points(&seg, p0, p1);
double thisp[2];
g2d_line_segment_closest_point(&seg, q, thisp);
double dist = g2d_distance(q, thisp);
if (dist < min_dist) {
memcpy(p, thisp, sizeof(double[2]));
min_dist = dist;
}
}
}
int g2d_polygon_contains_point(const zarray_t *poly, double q[2])
{
// use winding. If the point is inside the polygon, we'll wrap
// around it (accumulating 6.28 radians). If we're outside the
// polygon, we'll accumulate zero.
int psz = zarray_size(poly);
int last_quadrant;
int quad_acc = 0;
for (int i = 0; i <= psz; i++) {
double *p;
zarray_get_volatile(poly, i % psz, &p);
// p[0] < q[0] p[1] < q[1] quadrant
// 0 0 0
// 0 1 3
// 1 0 1
// 1 1 2
// p[1] < q[1] p[0] < q[0] quadrant
// 0 0 0
// 0 1 1
// 1 0 3
// 1 1 2
int quadrant;
if (p[0] < q[0])
quadrant = (p[1] < q[1]) ? 2 : 1;
else
quadrant = (p[1] < q[1]) ? 3 : 0;
if (i > 0) {
int dquadrant = quadrant - last_quadrant;
// encourage a jump table by mapping to small positive integers.
switch (dquadrant) {
case -3:
case 1:
quad_acc ++;
break;
case -1:
case 3:
quad_acc --;
break;
case 0:
break;
case -2:
case 2:
{
// get the previous point.
double *p0;
zarray_get_volatile(poly, i-1, &p0);
// Consider the points p0 and p (the points around the
//polygon that we are tracing) and the query point q.
//
// If we've moved diagonally across quadrants, we want
// to measure whether we have rotated +PI radians or
// -PI radians. We can test this by computing the dot
// product of vector (p0-q) with the vector
// perpendicular to vector (p-q)
double nx = p[1] - q[1];
double ny = -p[0] + q[0];
double dot = nx*(p0[0]-q[0]) + ny*(p0[1]-q[1]);
if (dot < 0)
quad_acc -= 2;
else
quad_acc += 2;
break;
}
}
}
last_quadrant = quadrant;
}
int v = (quad_acc >= 2) || (quad_acc <= -2);
if (0 && v != g2d_polygon_contains_point_ref(poly, q)) {
printf("FAILURE %d %d\n", v, quad_acc);
exit(-1);
}
return v;
}
// creates and returns a zarray(double[2]). The resulting polygon is
// CCW and implicitly closed. Unnecessary colinear points are omitted.
zarray_t *g2d_convex_hull(const zarray_t *points)
{
zarray_t *hull = zarray_create(sizeof(double[2]));
// gift-wrap algorithm.
// step 1: find left most point.
int insz = zarray_size(points);
double *pleft = NULL;
for (int i = 0; i < insz; i++) {
double *p;
zarray_get_volatile(points, i, &p);
if (pleft == NULL || p[0] < pleft[0])
pleft = p;
}
zarray_add(hull, pleft);
// step 2. gift wrap. Keep searching for points that make the
// smallest-angle left-hand turn. This implementation is carefully
// written to use only addition/subtraction/multiply. No division
// or sqrts. This guarantees exact results for integer-coordinate
// polygons (no rounding/precision problems).
double *p = pleft;
while (1) {
double *q = NULL;
double n0 = 0, n1 = 0; // the normal to the line (p, q) (not
// necessarily unit length).
// Search for the point q for which the line (p,q) is most "to
// the right of" the other points. (i.e., every time we find a
// point that is to the right of our current line, we change
// lines.)
for (int i = 0; i < insz; i++) {
double *thisq;
zarray_get_volatile(points, i, &thisq);
if (thisq == p)
continue;
if (q == NULL) {
q = thisq;
n0 = q[1] - p[1];
n1 = -q[0] + p[0];
} else {
// is point thisq RIGHT OF line (p, q)?
double e0 = thisq[0] - p[0], e1 = thisq[1] - p[1];
double dot = e0*n0 + e1*n1;
if (dot > 0) {
q = thisq;
n0 = q[1] - p[1];
n1 = -q[0] + p[0];
}
}
}
// loop completed?
if (q == pleft)
break;
int colinear = 0;
// is this new point colinear with the last two?
if (zarray_size(hull) > 1) {
double *o;
zarray_get_volatile(hull, zarray_size(hull) - 2, &o);
double e0 = o[0] - p[0];
double e1 = o[1] - p[1];
if (n0*e0 + n1*e1 == 0)
colinear = 1;
}
// if it is colinear, overwrite the last one.
if (colinear)
zarray_set(hull, zarray_size(hull)-1, q, NULL);
else
zarray_add(hull, q);
p = q;
}
return hull;
}
// correspondences is a list of float[4]s, consisting of the points x
// and y concatenated. We will compute a homography such that y = Hx
matd_t *homography_compute(zarray_t *correspondences, int flags)
{
// compute centroids of both sets of points (yields a better
// conditioned information matrix)
double x_cx = 0, x_cy = 0;
double y_cx = 0, y_cy = 0;
for (int i = 0; i < zarray_size(correspondences); i++) {
float *c;
zarray_get_volatile(correspondences, i, &c);
x_cx += c[0];
x_cy += c[1];
y_cx += c[2];
y_cy += c[3];
}
int sz = zarray_size(correspondences);
x_cx /= sz;
x_cy /= sz;
y_cx /= sz;
y_cy /= sz;
// NB We don't normalize scale; it seems implausible that it could
// possibly make any difference given the dynamic range of IEEE
// doubles.
matd_t *A = matd_create(9,9);
for (int i = 0; i < zarray_size(correspondences); i++) {
float *c;
zarray_get_volatile(correspondences, i, &c);
// (below world is "x", and image is "y")
double worldx = c[0] - x_cx;
double worldy = c[1] - x_cy;
double imagex = c[2] - y_cx;
double imagey = c[3] - y_cy;
double a03 = -worldx;
double a04 = -worldy;
double a05 = -1;
double a06 = worldx*imagey;
double a07 = worldy*imagey;
double a08 = imagey;
MATD_EL(A, 3, 3) += a03*a03;
MATD_EL(A, 3, 4) += a03*a04;
MATD_EL(A, 3, 5) += a03*a05;
MATD_EL(A, 3, 6) += a03*a06;
MATD_EL(A, 3, 7) += a03*a07;
MATD_EL(A, 3, 8) += a03*a08;
MATD_EL(A, 4, 4) += a04*a04;
MATD_EL(A, 4, 5) += a04*a05;
MATD_EL(A, 4, 6) += a04*a06;
MATD_EL(A, 4, 7) += a04*a07;
MATD_EL(A, 4, 8) += a04*a08;
MATD_EL(A, 5, 5) += a05*a05;
MATD_EL(A, 5, 6) += a05*a06;
MATD_EL(A, 5, 7) += a05*a07;
MATD_EL(A, 5, 8) += a05*a08;
MATD_EL(A, 6, 6) += a06*a06;
MATD_EL(A, 6, 7) += a06*a07;
MATD_EL(A, 6, 8) += a06*a08;
MATD_EL(A, 7, 7) += a07*a07;
MATD_EL(A, 7, 8) += a07*a08;
MATD_EL(A, 8, 8) += a08*a08;
double a10 = worldx;
double a11 = worldy;
double a12 = 1;
double a16 = -worldx*imagex;
double a17 = -worldy*imagex;
double a18 = -imagex;
MATD_EL(A, 0, 0) += a10*a10;
MATD_EL(A, 0, 1) += a10*a11;
MATD_EL(A, 0, 2) += a10*a12;
MATD_EL(A, 0, 6) += a10*a16;
MATD_EL(A, 0, 7) += a10*a17;
MATD_EL(A, 0, 8) += a10*a18;
MATD_EL(A, 1, 1) += a11*a11;
MATD_EL(A, 1, 2) += a11*a12;
MATD_EL(A, 1, 6) += a11*a16;
MATD_EL(A, 1, 7) += a11*a17;
MATD_EL(A, 1, 8) += a11*a18;
MATD_EL(A, 2, 2) += a12*a12;
MATD_EL(A, 2, 6) += a12*a16;
MATD_EL(A, 2, 7) += a12*a17;
MATD_EL(A, 2, 8) += a12*a18;
MATD_EL(A, 6, 6) += a16*a16;
MATD_EL(A, 6, 7) += a16*a17;
MATD_EL(A, 6, 8) += a16*a18;
MATD_EL(A, 7, 7) += a17*a17;
MATD_EL(A, 7, 8) += a17*a18;
MATD_EL(A, 8, 8) += a18*a18;
double a20 = -worldx*imagey;
double a21 = -worldy*imagey;
double a22 = -imagey;
double a23 = worldx*imagex;
double a24 = worldy*imagex;
double a25 = imagex;
MATD_EL(A, 0, 0) += a20*a20;
MATD_EL(A, 0, 1) += a20*a21;
MATD_EL(A, 0, 2) += a20*a22;
MATD_EL(A, 0, 3) += a20*a23;
MATD_EL(A, 0, 4) += a20*a24;
MATD_EL(A, 0, 5) += a20*a25;
MATD_EL(A, 1, 1) += a21*a21;
MATD_EL(A, 1, 2) += a21*a22;
MATD_EL(A, 1, 3) += a21*a23;
MATD_EL(A, 1, 4) += a21*a24;
MATD_EL(A, 1, 5) += a21*a25;
MATD_EL(A, 2, 2) += a22*a22;
MATD_EL(A, 2, 3) += a22*a23;
MATD_EL(A, 2, 4) += a22*a24;
MATD_EL(A, 2, 5) += a22*a25;
MATD_EL(A, 3, 3) += a23*a23;
MATD_EL(A, 3, 4) += a23*a24;
MATD_EL(A, 3, 5) += a23*a25;
MATD_EL(A, 4, 4) += a24*a24;
MATD_EL(A, 4, 5) += a24*a25;
MATD_EL(A, 5, 5) += a25*a25;
}
// make symmetric
for (int i = 0; i < 9; i++)
for (int j = i+1; j < 9; j++)
MATD_EL(A, j, i) = MATD_EL(A, i, j);
matd_t *H = matd_create(3,3);
if (flags & HOMOGRAPHY_COMPUTE_FLAG_INVERSE) {
// compute singular vector by (carefully) inverting the rank-deficient matrix.
if (1) {
matd_t *Ainv = matd_inverse(A);
double scale = 0;
for (int i = 0; i < 9; i++)
scale += sq(MATD_EL(Ainv, i, 0));
scale = sqrt(scale);
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0) / scale;
matd_destroy(Ainv);
} else {
matd_t *b = matd_create_data(9, 1, (double[]) { 1, 0, 0, 0, 0, 0, 0, 0, 0 });
matd_t *Ainv = NULL;
if (0) {
matd_lu_t *lu = matd_lu(A);
Ainv = matd_lu_solve(lu, b);
matd_lu_destroy(lu);
} else {
matd_chol_t *chol = matd_chol(A);
Ainv = matd_chol_solve(chol, b);
matd_chol_destroy(chol);
}
double scale = 0;
for (int i = 0; i < 9; i++)
scale += sq(MATD_EL(Ainv, i, 0));
scale = sqrt(scale);
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0) / scale;
matd_destroy(b);
matd_destroy(Ainv);
}
} else {
// correspondences is a list of float[4]s, consisting of the points x
// and y concatenated. We will compute a homography such that y = Hx
matd_t *homography_compute(zarray_t *correspondences)
{
// compute centroids of both sets of points (yields a better
// conditioned information matrix)
double x_cx = 0, x_cy = 0;
double y_cx = 0, y_cy = 0;
for (int i = 0; i < zarray_size(correspondences); i++) {
float *c;
zarray_get_volatile(correspondences, i, &c);
x_cx += c[0];
x_cy += c[1];
y_cx += c[2];
y_cy += c[3];
}
int sz = zarray_size(correspondences);
x_cx /= sz;
x_cy /= sz;
y_cx /= sz;
y_cy /= sz;
// NB We don't normalize scale; it seems implausible that it could
// possibly make any difference given the dynamic range of IEEE
// doubles.
matd_t *A = matd_create(9,9);
for (int i = 0; i < zarray_size(correspondences); i++) {
float *c;
zarray_get_volatile(correspondences, i, &c);
// (below world is "x", and image is "y")
double worldx = c[0] - x_cx;
double worldy = c[1] - x_cy;
double imagex = c[2] - y_cx;
double imagey = c[3] - y_cy;
double a03 = -worldx;
double a04 = -worldy;
double a05 = -1;
double a06 = worldx*imagey;
double a07 = worldy*imagey;
double a08 = imagey;
MATD_EL(A, 3, 3) += a03*a03;
MATD_EL(A, 3, 4) += a03*a04;
MATD_EL(A, 3, 5) += a03*a05;
MATD_EL(A, 3, 6) += a03*a06;
MATD_EL(A, 3, 7) += a03*a07;
MATD_EL(A, 3, 8) += a03*a08;
MATD_EL(A, 4, 4) += a04*a04;
MATD_EL(A, 4, 5) += a04*a05;
MATD_EL(A, 4, 6) += a04*a06;
MATD_EL(A, 4, 7) += a04*a07;
MATD_EL(A, 4, 8) += a04*a08;
MATD_EL(A, 5, 5) += a05*a05;
MATD_EL(A, 5, 6) += a05*a06;
MATD_EL(A, 5, 7) += a05*a07;
MATD_EL(A, 5, 8) += a05*a08;
MATD_EL(A, 6, 6) += a06*a06;
MATD_EL(A, 6, 7) += a06*a07;
MATD_EL(A, 6, 8) += a06*a08;
MATD_EL(A, 7, 7) += a07*a07;
MATD_EL(A, 7, 8) += a07*a08;
MATD_EL(A, 8, 8) += a08*a08;
double a10 = worldx;
double a11 = worldy;
double a12 = 1;
double a16 = -worldx*imagex;
double a17 = -worldy*imagex;
double a18 = -imagex;
MATD_EL(A, 0, 0) += a10*a10;
MATD_EL(A, 0, 1) += a10*a11;
MATD_EL(A, 0, 2) += a10*a12;
MATD_EL(A, 0, 6) += a10*a16;
MATD_EL(A, 0, 7) += a10*a17;
MATD_EL(A, 0, 8) += a10*a18;
MATD_EL(A, 1, 1) += a11*a11;
MATD_EL(A, 1, 2) += a11*a12;
MATD_EL(A, 1, 6) += a11*a16;
MATD_EL(A, 1, 7) += a11*a17;
MATD_EL(A, 1, 8) += a11*a18;
MATD_EL(A, 2, 2) += a12*a12;
MATD_EL(A, 2, 6) += a12*a16;
MATD_EL(A, 2, 7) += a12*a17;
MATD_EL(A, 2, 8) += a12*a18;
MATD_EL(A, 6, 6) += a16*a16;
MATD_EL(A, 6, 7) += a16*a17;
MATD_EL(A, 6, 8) += a16*a18;
MATD_EL(A, 7, 7) += a17*a17;
MATD_EL(A, 7, 8) += a17*a18;
MATD_EL(A, 8, 8) += a18*a18;
double a20 = -worldx*imagey;
double a21 = -worldy*imagey;
double a22 = -imagey;
double a23 = worldx*imagex;
double a24 = worldy*imagex;
double a25 = imagex;
MATD_EL(A, 0, 0) += a20*a20;
MATD_EL(A, 0, 1) += a20*a21;
MATD_EL(A, 0, 2) += a20*a22;
MATD_EL(A, 0, 3) += a20*a23;
MATD_EL(A, 0, 4) += a20*a24;
MATD_EL(A, 0, 5) += a20*a25;
MATD_EL(A, 1, 1) += a21*a21;
MATD_EL(A, 1, 2) += a21*a22;
MATD_EL(A, 1, 3) += a21*a23;
MATD_EL(A, 1, 4) += a21*a24;
MATD_EL(A, 1, 5) += a21*a25;
MATD_EL(A, 2, 2) += a22*a22;
MATD_EL(A, 2, 3) += a22*a23;
MATD_EL(A, 2, 4) += a22*a24;
MATD_EL(A, 2, 5) += a22*a25;
MATD_EL(A, 3, 3) += a23*a23;
MATD_EL(A, 3, 4) += a23*a24;
MATD_EL(A, 3, 5) += a23*a25;
MATD_EL(A, 4, 4) += a24*a24;
MATD_EL(A, 4, 5) += a24*a25;
MATD_EL(A, 5, 5) += a25*a25;
}
// make symmetric
for (int i = 0; i < 9; i++)
for (int j = i+1; j < 9; j++)
MATD_EL(A, j, i) = MATD_EL(A, i, j);
matd_svd_t svd = matd_svd(A);
matd_t *Ainv = matd_inverse(A);
double scale = 0;
for (int i = 0; i < 9; i++)
scale += sq(MATD_EL(Ainv, i, 0));
scale = sqrt(scale);
if (1) {
// compute singular vector using SVD. A bit slower, but more accurate.
matd_svd_t svd = matd_svd(A);
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
// MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0)/ scale;
MATD_EL(H, i, j) = MATD_EL(svd.U, 3*i+j, 8);
matd_destroy(svd.U);
matd_destroy(svd.S);
matd_destroy(svd.V);
} else {
// compute singular vector by (carefully) inverting the rank-deficient matrix.
matd_t *Ainv = matd_inverse(A);
double scale = 0;
for (int i = 0; i < 9; i++)
scale += sq(MATD_EL(Ainv, i, 0));
scale = sqrt(scale);
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0)/ scale;
matd_destroy(Ainv);
}
matd_t *Tx = matd_identity(3);
MATD_EL(Tx,0,2) = -x_cx;
MATD_EL(Tx,1,2) = -x_cy;
matd_t *Ty = matd_identity(3);
MATD_EL(Ty,0,2) = y_cx;
MATD_EL(Ty,1,2) = y_cy;
matd_t *H2 = matd_op("M*M*M", Ty, H, Tx);
matd_destroy(A);
matd_destroy(Tx);
matd_destroy(Ty);
matd_destroy(H);
matd_destroy(svd.U);
matd_destroy(svd.S);
matd_destroy(svd.V);
return H2;
}
// return 1 if the quad looks okay, 0 if it should be discarded
int fit_quad(apriltag_detector_t *td, image_u8_t *im, zarray_t *cluster, struct quad *quad)
{
int res = 0;
int sz = zarray_size(cluster);
if (sz < 4) // can't fit a quad to less than 4 points
return 0;
/////////////////////////////////////////////////////////////
// Step 1. Sort points so they wrap around the center of the
// quad. We will constrain our quad fit to simply partition this
// ordered set into 4 groups.
// compute a bounding box so that we can order the points
// according to their angle WRT the center.
int xmax = 0, xmin = 9999999, ymax = 0, ymin = 9999999;
for (int pidx = 0; pidx < zarray_size(cluster); pidx++) {
struct pt *p;
zarray_get_volatile(cluster, pidx, &p);
xmax = imax(xmax, p->x);
xmin = imin(xmin, p->x);
ymax = imax(ymax, p->y);
ymin = imin(ymin, p->y);
}
int cx = (xmin + xmax) / 2;
int cy = (ymin + ymax) / 2;
for (int pidx = 0; pidx < zarray_size(cluster); pidx++) {
struct pt *p;
zarray_get_volatile(cluster, pidx, &p);
p->theta = atan2f(p->y - cy, p->x - cx);
}
zarray_sort(cluster, pt_compare_theta);
// remove duplicate points. (A byproduct of our segmentation system.)
if (1) {
int outpos = 1;
struct pt *last;
zarray_get_volatile(cluster, 0, &last);
for (int i = 1; i < sz; i++) {
struct pt *p;
zarray_get_volatile(cluster, i, &p);
if (p->x != last->x || p->y != last->y) {
if (i != outpos) {
struct pt *out;
zarray_get_volatile(cluster, outpos, &out);
memcpy(out, p, sizeof(struct pt));
}
outpos++;
}
last = p;
}
cluster->size = outpos;
sz = outpos;
}
if (sz < 4)
return 0;
/////////////////////////////////////////////////////////////
// Step 2. Precompute statistics that allow line fit queries to be
// efficiently computed for any contiguous range of indices.
struct line_fit_pt *lfps = (struct line_fit_pt *)calloc(sz, sizeof(struct line_fit_pt));
for (int i = 0; i < sz; i++) {
struct pt *p;
zarray_get_volatile(cluster, i, &p);
if (i > 0) {
memcpy(&lfps[i], &lfps[i-1], sizeof(struct line_fit_pt));
}
double W = 1;
if (p->x > 0 && p->x+1 < im->width && p->y > 0 && p->y+1 < im->height) {
int grad_x = im->buf[p->y * im->stride + p->x + 1] -
im->buf[p->y * im->stride + p->x - 1];
int grad_y = im->buf[(p->y+1) * im->stride + p->x] -
im->buf[(p->y-1) * im->stride + p->x];
W = sqrtf(grad_x*grad_x + grad_y*grad_y) + 1;
}
lfps[i].Mx += W * p->x;
lfps[i].My += W * p->y;
lfps[i].Mxx += W * p->x * p->x;
lfps[i].Mxy += W * p->x * p->y;
lfps[i].Myy += W * p->y * p->y;
lfps[i].W += W;
}
int indices[4];
if (1) {
if (!quad_segment_maxima(td, cluster, lfps, indices))
goto finish;
} else {
if (!quad_segment_agg(td, cluster, lfps, indices))
goto finish;
}
// printf("%d %d %d %d\n", indices[0], indices[1], indices[2], indices[3]);
if (0) {
// no refitting here; just use those points as the vertices.
// Note, this is useful for debugging, but pretty bad in
// practice since this code path also omits several
// plausibility checks that save us tons of time in quad
// decoding.
for (int i = 0; i < 4; i++) {
struct pt *p;
zarray_get_volatile(cluster, indices[i], &p);
quad->p[i][0] = p->x;
quad->p[i][1] = p->y;
}
res = 1;
} else {
double lines[4][4];
for (int i = 0; i < 4; i++) {
int i0 = indices[i];
int i1 = indices[(i+1)&3];
if (0) {
// if there are enough points, skip the points near the corners
// (because those tend not to be very good.)
if (i1-i0 > 8) {
int t = (i1-i0)/6;
if (t < 0)
t = -t;
i0 = (i0 + t) % sz;
i1 = (i1 + sz - t) % sz;
}
}
double err;
fit_line(lfps, sz, i0, i1, lines[i], NULL, &err);
// XXX VALUE?
// printf("%f %d\n", err, i1-i0);
if (err > td->qtp.max_line_fit_mse) {
res = 0;
goto finish;
}
}
for (int i = 0; i < 4; i++) {
// solve for the intersection of lines (i) and (i+1)&3.
// p0 + lambda0*u0 = p1 + lambda1*u1, where u0 and u1
// are the line directions.
//
// lambda0*u0 - lambda1*u1 = (p1 - p0)
//
// rearrange (solve for lambdas)
//
// [u0_x -u1_x ] [lambda0] = [ p1_x - p0_x ]
// [u0_y -u1_y ] [lambda1] [ p1_y - p0_y ]
//
// remember that lines[i][0,1] = p, lines[i][2,3] = NORMAL vector.
// We want the unit vector, so we need the perpendiculars. Thus, below
// we have swapped the x and y components and flipped the y components.
double A00 = lines[i][3], A01 = -lines[(i+1)&3][3];
double A10 = -lines[i][2], A11 = lines[(i+1)&3][2];
double B0 = -lines[i][0] + lines[(i+1)&3][0];
double B1 = -lines[i][1] + lines[(i+1)&3][1];
double det = A00 * A11 - A10 * A01;
// inverse.
double W00 = A11 / det, W01 = -A01 / det;
if (fabs(det) < 0.001) {
res = 0;
goto finish;
}
// solve
double L0 = W00*B0 + W01*B1;
// compute intersection
quad->p[i][0] = lines[i][0] + L0*A00;
quad->p[i][1] = lines[i][1] + L0*A10;
if (0) {
// we should get the same intersection starting
// from point p1 and moving L1*u1.
double W10 = -A10 / det, W11 = A00 / det;
double L1 = W10*B0 + W11*B1;
double x = lines[(i+1)&3][0] - L1*A10;
double y = lines[(i+1)&3][1] - L1*A11;
assert(fabs(x - quad->p[i][0]) < 0.001 &&
fabs(y - quad->p[i][1]) < 0.001);
}
res = 1;
}
}
// reject quads with edges that are too short or long.
if (1) {
for (int i = 0; i < 3; i++) {
double dist2 = sq(quad->p[i][0] - quad->p[i+1][0]) +
sq(quad->p[i][1] - quad->p[i+1][1]);
if (dist2 < sq(6) || dist2 > sq(4096)) {
res = 0;
goto finish;
}
}
}
// reject quads whose cumulative angle change isn't equal to 2PI
if (1) {
double total = 0;
for (int i = 0; i < 4; i++) {
int i0 = i, i1 = (i+1)&3, i2 = (i+2)&3;
double theta0 = atan2f(quad->p[i0][1] - quad->p[i1][1],
quad->p[i0][0] - quad->p[i1][0]);
double theta1 = atan2f(quad->p[i2][1] - quad->p[i1][1],
quad->p[i2][0] - quad->p[i1][0]);
double dtheta = theta0 - theta1;
if (dtheta < 0)
dtheta += 2*M_PI;
if (dtheta < td->qtp.critical_rad || dtheta > (M_PI - td->qtp.critical_rad))
res = 0;
total += dtheta;
}
if (total < 6.2 || total > 6.4) {
res = 0;
goto finish;
}
}
// adjust pixel coordinates; all math up 'til now uses pixel
// coordinates in which (0,0) is the lower left corner. But each
// pixel actually spans from to [x, x+1), [y, y+1) the mean value of which
// is +.5 higher than x & y.
for (int i = 0; i < 4; i++) {
quad->p[i][0] += .5;
quad->p[i][1] += .5;
}
finish:
/*
if (res) {
static image_u8_t *im;
if (im == NULL)
im = image_u8_create(1920,1080);
for (int j = 0; j < 4; j++) {
int i0 = indices[j];
int i1 = indices[(j+1)&3];
if (i1 < i0)
i1 += sz;
for (int i = i0; i <= i1; i++) {
struct pt *p;
zarray_get_volatile(cluster, i % sz, &p);
im->buf[((int) p->y)*im->stride + ((int) p->x)] = 64 + 128*(j%2);
}
}
char name[1024];
sprintf(name, "debug_seg.pnm", utime_now());
image_u8_write_pnm(im, name);
}
*/
free(lfps);
return res;
}
zarray_t *apriltag_quad_thresh(apriltag_detector_t *td, image_u8_t *im)
{
////////////////////////////////////////////////////////
// step 1. threshold the image, creating the edge image.
int w = im->width, h = im->height, s = im->stride;
image_u8_t *threshim = threshold(td, im);
assert(threshim->stride == s);
image_u8_t *edgeim = image_u8_create(w, h);
if (1) {
image_u8_t *sumim = image_u8_create(w, h);
// apply a horizontal sum kernel of width 3
for (int y = 0; y < h; y++) {
for (int x = 1; x+1 < w; x++) {
sumim->buf[y*s + x] =
threshim->buf[y*s + x - 1] +
threshim->buf[y*s + x + 0] +
threshim->buf[y*s + x + 1];
}
}
timeprofile_stamp(td->tp, "sumim");
// deglitch
if (td->qtp.deglitch) {
for (int y = 1; y+1 < h; y++) {
for (int x = 1; x+1 < w; x++) {
// edge: black pixel next to white pixel
if (threshim->buf[y*s + x] == 0 &&
sumim->buf[y*s + x - s] + sumim->buf[y*s + x] + sumim->buf[y*s + x + s] == 8) {
threshim->buf[y*s + x] = 1;
sumim->buf[y*s + x - 1]++;
sumim->buf[y*s + x + 0]++;
sumim->buf[y*s + x + 1]++;
}
if (threshim->buf[y*s + x] == 1 &&
sumim->buf[y*s + x - s] + sumim->buf[y*s + x] + sumim->buf[y*s + x + s] == 1) {
threshim->buf[y*s + x] = 0;
sumim->buf[y*s + x - 1]--;
sumim->buf[y*s + x + 0]--;
sumim->buf[y*s + x + 1]--;
}
}
}
timeprofile_stamp(td->tp, "deglitch");
}
// apply a vertical sum kernel of width 3; check if any
// over-threshold pixels are adjacent to an under-threshold
// pixel.
//
// There are two types of edges: white pixels neighboring a
// black pixel, and black pixels neighboring a white pixel. We
// label these separately. (Values 0xc0 and 0x3f are picked
// such that they add to 255 (see below) and so that they can be
// viewed as pixel intensities for visualization purposes.)
//
// symmetry of detection. We don't want to use JUST "black
// near white" (or JUST "white near black"), because that
// biases the detection towards one side of the edge. This
// measurably reduces detection performance.
//
// On large tags, we could treat "neighbor" pixels the same
// way. But on very small tags, there may be other edges very
// near the tag edge. Since each of these edges is effectively
// two pixels thick (the white pixel near the black pixel, and
// the black pixel near the white pixel), it becomes likely
// that these two nearby edges will actually touch.
//
// A partial solution to this problem is to define edges to be
// adjacent white-near-black and black-near-white pixels.
//
for (int y = 1; y+1 < h; y++) {
for (int x = 1; x+1 < w; x++) {
if (threshim->buf[y*s + x] == 0) {
// edge: black pixel next to white pixel
if (sumim->buf[y*s + x - s] + sumim->buf[y*s + x] + sumim->buf[y*s + x + s] > 0)
edgeim->buf[y*s + x] = 0xc0;
} else {
// edge: white pixel next to black pixel when both
// edge types are on, we get less bias towards one
// side of the edge.
if (sumim->buf[y*s + x - s] + sumim->buf[y*s + x] + sumim->buf[y*s + x + s] < 9)
edgeim->buf[y*s + x] = 0x3f;
}
}
}
if (td->debug) {
for (int y = 0; y < h; y++) {
for (int x = 0; x < w; x++) {
threshim->buf[y*s + x] *= 255;
}
}
image_u8_write_pnm(threshim, "debug_threshold.pnm");
image_u8_write_pnm(edgeim, "debug_edge.pnm");
// image_u8_destroy(edgeim2);
}
image_u8_destroy(threshim);
image_u8_destroy(sumim);
}
timeprofile_stamp(td->tp, "edges");
////////////////////////////////////////////////////////
// step 2. find connected components.
unionfind_t *uf = unionfind_create(w * h);
for (int y = 1; y < h - 1; y++) {
for (int x = 1; x < w -1; x++) {
uint8_t v = edgeim->buf[y*s + x];
if (v==0)
continue;
// (dx,dy) pairs for 8 connectivity:
// (REFERENCE) (1, 0)
// (-1, 1) (0, 1) (1, 1)
//
// i.e., the minimum value of dx should be:
// y=0: 1
// y=1: -1
for (int dy = 0; dy <= 1; dy++) {
for (int dx = 1-2*dy; dx <= 1; dx++) {
if (edgeim->buf[(y+dy)*s + (x+dx)] == v) {
unionfind_connect(uf, y*w + x, (y+dy)*w + x + dx);
}
}
}
}
}
timeprofile_stamp(td->tp, "unionfind");
zhash_t *clustermap = zhash_create(sizeof(uint64_t), sizeof(zarray_t*),
zhash_uint64_hash, zhash_uint64_equals);
for (int y = 1; y < h-1; y++) {
for (int x = 1; x < w-1; x++) {
uint8_t v0 = edgeim->buf[y*s + x];
if (v0 == 0)
continue;
uint64_t rep0 = unionfind_get_representative(uf, y*w + x);
// 8 connectivity. (4 neighbors to check).
// for (int dy = 0; dy <= 1; dy++) {
// for (int dx = 1-2*dy; dx <= 1; dx++) {
// 4 connectivity. (2 neighbors to check)
for (int n = 1; n <= 2; n++) {
int dy = n & 1;
int dx = (n & 2) >> 1;
uint8_t v1 = edgeim->buf[(y+dy)*s + x + dx];
if (v0 + v1 != 255)
continue;
uint64_t rep1 = unionfind_get_representative(uf, (y+dy)*w + x+dx);
uint64_t clusterid;
if (rep0 < rep1)
clusterid = (rep1 << 32) + rep0;
else
clusterid = (rep0 << 32) + rep1;
zarray_t *cluster = NULL;
if (!zhash_get(clustermap, &clusterid, &cluster)) {
cluster = zarray_create(sizeof(struct pt));
zhash_put(clustermap, &clusterid, &cluster, NULL, NULL);
}
// NB: We will add some points multiple times to a
// given cluster. I don't know an efficient way to
// avoid that here; we remove them later on when we
// sort points by pt_compare_theta.
if (1) {
struct pt p = { .x = x, .y = y};
zarray_add(cluster, &p);
}
if (1) {
struct pt p = { .x = x+dx, .y = y+dy};
zarray_add(cluster, &p);
}
}
}
}
// make segmentation image.
if (td->debug) {
image_u8_t *d = image_u8_create(w, h);
assert(d->stride == s);
uint8_t *colors = (uint8_t*) calloc(w*h, 1);
for (int y = 0; y < h; y++) {
for (int x = 0; x < w; x++) {
uint32_t v = unionfind_get_representative(uf, y*w+x);
uint32_t sz = unionfind_get_set_size(uf, y*w+x);
if (sz < td->qtp.min_cluster_pixels)
continue;
uint8_t color = colors[v];
if (color == 0) {
const int bias = 20;
color = bias + (random() % (255-bias));
colors[v] = color;
}
float mix = 0.7;
mix = 1.0;
d->buf[y*d->stride + x] = mix*color + (1-mix)*im->buf[y*im->stride + x];
}
}
free(colors);
image_u8_write_pnm(d, "debug_segmentation.pnm");
image_u8_destroy(d);
}
timeprofile_stamp(td->tp, "make clusters");
////////////////////////////////////////////////////////
// step 3. process each connected component.
zarray_t *clusters = zhash_values(clustermap);
zhash_destroy(clustermap);
zarray_t *quads = zarray_create(sizeof(struct quad));
int sz = zarray_size(clusters);
int chunksize = 1 + sz / (APRILTAG_TASKS_PER_THREAD_TARGET * td->nthreads);
struct quad_task tasks[sz / chunksize + 1];
int ntasks = 0;
for (int i = 0; i < sz; i += chunksize) {
tasks[ntasks].td = td;
tasks[ntasks].cidx0 = i;
tasks[ntasks].cidx1 = imin(sz, i + chunksize);
tasks[ntasks].h = h;
tasks[ntasks].w = w;
tasks[ntasks].quads = quads;
tasks[ntasks].clusters = clusters;
tasks[ntasks].im = im;
workerpool_add_task(td->wp, do_quad_task, &tasks[ntasks]);
ntasks++;
}
workerpool_run(td->wp);
timeprofile_stamp(td->tp, "fit quads to clusters");
if (td->debug) {
FILE *f = fopen("debug_lines.ps", "w");
fprintf(f, "%%!PS\n\n");
image_u8_t *im2 = image_u8_copy(im);
image_u8_darken(im2);
image_u8_darken(im2);
// assume letter, which is 612x792 points.
double scale = fmin(612.0/im->width, 792.0/im2->height);
fprintf(f, "%.15f %.15f scale\n", scale, scale);
fprintf(f, "0 %d translate\n", im2->height);
fprintf(f, "1 -1 scale\n");
postscript_image(f, im);
for (int i = 0; i < zarray_size(quads); i++) {
struct quad *q;
zarray_get_volatile(quads, i, &q);
float rgb[3];
int bias = 100;
for (int i = 0; i < 3; i++)
rgb[i] = bias + (random() % (255-bias));
fprintf(f, "%f %f %f setrgbcolor\n", rgb[0]/255.0f, rgb[1]/255.0f, rgb[2]/255.0f);
fprintf(f, "%.15f %.15f moveto %.15f %.15f lineto %.15f %.15f lineto %.15f %.15f lineto %.15f %.15f lineto stroke\n",
q->p[0][0], q->p[0][1],
q->p[1][0], q->p[1][1],
q->p[2][0], q->p[2][1],
q->p[3][0], q->p[3][1],
q->p[0][0], q->p[0][1]);
}
fclose(f);
}
// printf(" %d %d %d %d\n", indices[0], indices[1], indices[2], indices[3]);
/*
if (td->debug) {
for (int i = 0; i < 4; i++) {
int i0 = indices[i];
int i1 = indices[(i+1)&3];
if (i1 < i0)
i1 += zarray_size(cluster);
for (int j = i0; j <= i1; j++) {
struct pt *p;
zarray_get_volatile(cluster, j % zarray_size(cluster), &p);
edgeim->buf[p->y*edgeim->stride + p->x] = 30+64*i;
}
}
} */
unionfind_destroy(uf);
for (int i = 0; i < zarray_size(clusters); i++) {
zarray_t *cluster;
zarray_get(clusters, i, &cluster);
zarray_destroy(cluster);
}
zarray_destroy(clusters);
image_u8_destroy(edgeim);
return quads;
}