Exemplo n.º 1
0
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;
}
Exemplo n.º 2
0
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;
}
Exemplo n.º 3
0
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);
}
Exemplo n.º 4
0
// 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;
        }
    }
}
Exemplo n.º 5
0
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;
}
Exemplo n.º 6
0
// 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;
}
Exemplo n.º 7
0
// 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 {
Exemplo n.º 8
0
// 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;
}
Exemplo n.º 9
0
// 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;
}
Exemplo n.º 10
0
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;
}