imgproc.c

#include <stdio.h>
#include <core/core_c.h>
#include <highgui/highgui_c.h>
#include <imgproc/imgproc_c.h>
#include <assert.h>
#include <pthread.h>
#include "nstream.h"
#include "imgproc.h"

#define MAX_CONTOURS 1024

struct contour_t;
struct contour_t {
    CvSeq *ctr;
    double area;
    struct contour_t *parent;
};

struct bases_t {
    struct contour_t **bases;
    int count;
    struct contour_t **ctrs;
    int ncontours;

    CvMemStorage *storage;      /* */
};

void destroytargets(struct target_t *in)
{
    struct target_t *target = NULL;
    struct target_t *last = NULL;

    for (target = in; target != NULL; target = target->next) {
        free(last);
        last = target;
    }
    free(last);

    return;
}

int
checkparallel(CvPoint p0, CvPoint p1, CvPoint p2, CvPoint p3, float tolerance)
{
    float slope0denom;
    float slope1denom;
    float slope0neum;
    float slope1neum;
    float slope0;
    float slope1;

    slope0neum = ((float) p1.y - (float) p0.y);
    slope0denom = ((float) p1.x - (float) p0.x);
    slope1neum = ((float) p3.y - (float) p2.y);
    slope1denom = ((float) p3.x - (float) p2.x);

    slope0neum = fabs(slope0neum);
    slope0denom = fabs(slope0denom);
    slope1neum = fabs(slope1neum);
    slope1denom = fabs(slope0denom);

    slope0 = slope0neum / slope0denom;
    slope1 = slope1neum / slope1denom;

    /* 
       if(slope0 > slope1 + (tolerance * slope1) || slope0 < slope1 -
       (tolerance * slope0)) return 0; */

    if (fabs(slope0 / slope1) > tolerance)
        return 0;

    return 1;
}

float distfrm(CvPoint p0, CvPoint p1)
{
    float term0;
    float term1;

    term0 = ((float) p1.x - (float) p0.x);
    term0 *= term0;

    term1 = ((float) p1.y - (float) p0.y);
    term1 *= term1;

    return sqrtf(term0 + term1);
}

float RawDepthToMeters(unsigned short depthValue)
{
    if (depthValue < 2047) {
        return (float) (1.0 /
                        (double) ((depthValue) * -0.0030711016 + 3.3309495161));
    }
    return 0.0;
}

CvPoint getboxmidpoint(CvPoint p0, CvPoint p1, CvPoint p2, CvPoint p3)
{
    CvPoint midpoint;
    midpoint.x = (p0.x + p1.x + p2.x + p3.x) / 4;
    midpoint.y = (p0.y + p1.y + p2.y + p3.y) / 4;

    return midpoint;

}

/* TODO: use all four points to find the midpoint of the polygon */
float
getDistanceFromPoints(CvPoint p0, CvPoint p1, CvPoint p2, CvPoint p3,
                      struct nstream_t *grey)
{
    unsigned short depthval;
    CvPoint midpoint;

    /* get the midpoint */
    midpoint = getboxmidpoint(p0, p1, p2, p3);

    /* get the raw depth pixel */

    pthread_mutex_lock(&grey->mutex);
    /* depthval = (unsigned short) *((unsigned short *)
       ((grey->buf+(grey->imgwidth*((int)midpoint.y))+((int)midpoint.x))*2)); */

    depthval =
        (unsigned short)
        *((unsigned short *) (grey->buf +
                              ((grey->imgwidth * midpoint.y) +
                               (midpoint.x)) * 2));

    pthread_mutex_unlock(&grey->mutex);

    return RawDepthToMeters(depthval);

}

struct target_t *findtargets(struct bases_t *bases, IplImage * grey,
                             struct nstream_t *depthimage)
{
    int i;
    int b;
    struct contour_t *ctr;
    CvPoint *point;

    CvPoint parentpoint[4];
    CvPoint childpoint[4];

    CvMemStorage *storage = cvCreateMemStorage(0);
    CvSeq *childptseq;
    CvSeq *parentptseq;
    CvSeq *childhull;
    CvSeq *parenthull;

    float distpts[4];
    float distavg;
    int avgcount;

    float top;
    float btm;
    float right;
    float left;
    float targetwidth;
    float targetheight;

    float ratio;

    float mpx;
    float mpy;

    int pxval;

    struct target_t *start = NULL;
    struct target_t *previous = NULL;
    struct target_t *tmp = NULL;

    /* const int partol = 5; */
    const float disttol = 25;
    const float tratio = 0.742063582;   /* 1.36920786; *//* 2.92875051; */
    const float ratiotol = 15;

    childptseq =
        cvCreateSeq(CV_SEQ_KIND_GENERIC | CV_32SC2, sizeof(CvContour),
                    sizeof(CvPoint), storage);
    parentptseq =
        cvCreateSeq(CV_SEQ_KIND_GENERIC | CV_32SC2, sizeof(CvContour),
                    sizeof(CvPoint), storage);

    /* Check each base to see if there's a target in it */
    for (i = 0; i < bases->count; i++) {
        /* We need to ensure this isn't going to be a dupe. This is the worst
           possible way to find targets. */
        /* ... */

        for (ctr = bases->bases[i]; ctr->parent != NULL; ctr = ctr->parent) {
            cvClearSeq(childptseq);
            cvClearSeq(parentptseq);
            for (b = 0; b < 4; b++) {
                point = CV_GET_SEQ_ELEM(CvPoint, ctr->ctr, b);
                cvSeqPush(childptseq, point);
                /* childpoint[b] = *point; */

                point = CV_GET_SEQ_ELEM(CvPoint, ctr->parent->ctr, b);
                cvSeqPush(parentptseq, point);
                /* parentpoint[b] = *point; */
            }
            /* Sort the points */
            childhull = cvConvexHull2(childptseq, 0, CV_CLOCKWISE, 0);
            parenthull = cvConvexHull2(parentptseq, 0, CV_CLOCKWISE, 0);

            for (b = 0; b < 4; b++) {
                childpoint[b] = **CV_GET_SEQ_ELEM(CvPoint *, childhull, b);
                parentpoint[b] = **CV_GET_SEQ_ELEM(CvPoint *, parenthull, b);
            }

            /* There are now 8 points, 4 child, 4 parent. Check for parallel
               lines (all sides of polygon). Make sure that the ratios are
               correct. Make sure that there is white in between the two
               contours. */
            /* 
               if(!checkparallel(childpoint[0], childpoint[1], parentpoint[0],
               parentpoint[1], partol)) continue;
               if(!checkparallel(childpoint[1], childpoint[2], parentpoint[1],
               parentpoint[2], partol)) continue;
               if(!checkparallel(childpoint[2], childpoint[3], parentpoint[2],
               parentpoint[3], partol)) continue;
               if(!checkparallel(childpoint[3], childpoint[0], parentpoint[3],
               parentpoint[0], partol)) continue; */

            /* Make sure that the distance between corners is equal for all
               four corners */
            distavg = 0;
            avgcount = 0;
            for (b = 0; b < 4; b++) {
                distpts[b] = distfrm(childpoint[b], parentpoint[b]);
                distavg += distpts[b];
                avgcount++;
            }
            distavg /= avgcount;

            for (b = 0; b < 4; b++) {
                if (distpts[b] > distavg + ((disttol / 100.0) * distavg)
                    || distpts[b] < distavg - ((disttol / 100.0) * distavg))
                    break;
            }
            if (b != 4)
                continue;

            /* Check the ratio of the sides */
            /* ((top+btm)/2) / ((left+right)/2) == target */
            top = distfrm(parentpoint[0], parentpoint[1]);
            btm = distfrm(parentpoint[2], parentpoint[3]);
            right = distfrm(parentpoint[1], parentpoint[2]);
            left = distfrm(parentpoint[3], parentpoint[0]);

            targetwidth = (top + btm) / 2;
            targetheight = (left + right) / 2;

            if (targetheight > targetwidth) {
                /* it's sideways */
                top = targetwidth;
                targetwidth = targetheight;
                targetheight = top;
            }

            /* Test the ratio */
            ratio = (targetheight / targetwidth);
            if (ratio > tratio + ((ratiotol / 100.0) * tratio)
                || ratio < tratio - ((ratiotol / 100.0) * tratio)) {
                ratio = 1.0 / ratio;
                if (ratio > tratio + ((ratiotol / 100.0) * tratio)
                    || ratio < tratio - ((ratiotol / 100.0) * tratio)) {
                    continue;
                }
            }

            mpx = ((float) parentpoint[0].x + (float) childpoint[0].x) / 2;
            mpy = ((float) parentpoint[0].y + (float) childpoint[0].y) / 2;
            /* s = cvGet2D(grey, (int)mpx, (int)mpy); pxval = s.val[0]; */

            pxval =
                (char) *(grey->imageData + (grey->widthStep * ((int) mpy)) +
                         ((int) mpx));

            /* cvSaveImage("contours.png", grey, 0); */
            if (pxval == 0x00)
                continue;

            /* It's a target... */
            /* printf("(%d, %d), (%d, %d), (%d, %d), (%d, %d)\n",
               parentpoint[0].x, parentpoint[0].y, parentpoint[1].x,
               parentpoint[1].y, parentpoint[2].x, parentpoint[2].y,
               parentpoint[3].x, parentpoint[3].y); */

            /* Add the target to the linked list */
            tmp = malloc(sizeof(struct target_t));
            if (start == NULL) {
                start = tmp;
            }
            tmp->next = NULL;
            tmp->prev = previous;
            if (previous != NULL) {
                previous->next = tmp;
            }
            previous = tmp;
            tmp->p0 = childpoint[0];
            tmp->p1 = childpoint[1];
            tmp->p2 = childpoint[2];
            tmp->p3 = childpoint[3];

            tmp->width = targetwidth;
            tmp->height = targetheight;

            if (depthimage != NULL) {
                tmp->distance =
                    getDistanceFromPoints(tmp->p0, tmp->p1, tmp->p2, tmp->p3,
                                          depthimage);
            } else {
                tmp->distance = 0;
            }

            break;

        }
    }

    cvClearMemStorage(storage);
    cvReleaseMemStorage(&storage);

    return start;
}

int iscontourchild(struct contour_t *parent, struct contour_t *child)
{
    int i;
    float x;
    float y;
    CvPoint *point;

    /* Check that all of child's verticies are within parent */
    for (i = 0; i < child->ctr->total; i++) {
        point = CV_GET_SEQ_ELEM(CvPoint, child->ctr, i);
        x = (float) point->x;
        y = (float) point->y;
        if (cvPointPolygonTest(parent->ctr, cvPoint2D32f(x, y), 0) < 0)
            return 0;
    }

    return 1;
}

struct bases_t *mkcontourtree(struct contour_t **ctrs, int ncontours)
{
    char *exclusionlist;
    int i;
    int b;
    struct contour_t **bases;
    int nbases = 0;
    struct bases_t *out;

    exclusionlist = malloc(ncontours);
    memset(exclusionlist, 0x00, ncontours);

    /* For all of the contours */
    for (i = 0; i < ncontours; i++) {
        /* find it's immediate parent */
        /* for(b = 0; b < ncontours; b++){ *//* for an unsorted list */
        for (b = i + 1; b < ncontours; b++) {
            /* Check if it's a parent */
            if (iscontourchild(ctrs[b], ctrs[i])) {
                ctrs[i]->parent = ctrs[b];
                exclusionlist[b] = 1;
                break;
            }
        }
        /* If we didn't break out of the loop, the contour doesn't have a
           parent. */
    }

    for (i = 0; i < ncontours; i++) {
        if (!exclusionlist[i] == 1) {
            nbases++;
        }
    }

    /* Make a list of the base contours */
    bases = malloc(sizeof(struct contour_t *) * nbases);
    b = 0;
    for (i = 0; i < ncontours; i++) {
        if (!exclusionlist[i]) {
            assert(b <= (nbases - 1));
            bases[b] = (struct contour_t *) ctrs[i];
            b++;
        }
    }

    out = malloc(sizeof(struct bases_t));
    out->count = nbases;
    out->bases = bases;
    out->ctrs = ctrs;
    out->ncontours = ncontours;

    free(exclusionlist);

    return out;
}

static int contourcompare(const void *p1, const void *p2)
{
    const struct contour_t *c1 = *(const void **) p1;
    const struct contour_t *c2 = *(const void **) p2;

    /* reverse sort */
    return (c1->area - c2->area);
    /* 
       if(c1->area > c2->area) return 1; if(c2->area > c1->area) return -1;

       return 0; */
}

void destroybases(struct bases_t *bases)
{
    int i;

    /* Clean up contours */
    for (i = 0; i < bases->ncontours; i++) {
        free(bases->ctrs[i]);
    }

    /* Clean up bases */
    /* 
       for(i = 0; i < bases->count; i++){ free(bases->bases[i]); } */

    cvClearMemStorage(bases->storage);
    cvReleaseMemStorage(&bases->storage);

    free(bases->ctrs);
    free(bases->bases);
    free(bases);

    return;
}

struct bases_t *procContours(IplImage * in)
{
    CvMemStorage *storage = cvCreateMemStorage(0);
    CvContourScanner scanner;
    CvSeq *ctr;
    int ncontours;
    struct contour_t *ctrt;
    struct contour_t **ctrs;
    struct bases_t *out;

    /* Create contours array */
    ncontours = 0;
    ctrs = malloc(sizeof(struct contour_t *) * MAX_CONTOURS);

    /* Contours image */
    /* 
       ci = cvCreateImage(cvGetSize(in), IPL_DEPTH_8U, 1); assert(ci != NULL); */

    /* Find countours */
    scanner =
        cvStartFindContours(in, storage, sizeof(CvContour), CV_RETR_LIST,
                            CV_CHAIN_APPROX_SIMPLE, cvPoint(0, 0));

    ctr = cvFindNextContour(scanner);
    while (ctr != NULL) {
        ctr =
            cvApproxPoly(ctr, sizeof(CvContour), storage, CV_POLY_APPROX_DP, 20,
                         0);
        /* Make a list of 4-sided polygons */
        if (ctr->total == 4) {
            assert(ncontours <= MAX_CONTOURS);
            ctrt = malloc(sizeof(struct contour_t));
            ctrt->ctr = ctr;
            ctrt->area = fabs(cvContourArea(ctr, CV_WHOLE_SEQ, 0));
            ctrt->parent = NULL;
            ctrs[ncontours] = ctrt;
            ncontours++;

            /* cvDrawContours(ci, ctr, CV_RGB(0xff, 0xff, 0xff), CV_RGB(0x00,
               0x00, 0x00), 255, CV_FILLED, 8, cvPoint(0, 0)); */
        }
        ctr = cvFindNextContour(scanner);
    }

    /* Sort contours */
    qsort(ctrs, ncontours, sizeof(struct contour_t *), contourcompare);
    /* Make a tree of the contours */
    out = mkcontourtree(ctrs, ncontours);

    /* cvSaveImage("contours.png", ci, 0); */

    /* Clean up */
    /* 
       for(i = 0; i < ncontours; i++){ free(ctrs[i]); } */

    /* free(ctrs); */
    /* cvReleaseImage(&ci); */
    cvEndFindContours(&scanner);

    out->storage = storage;

    return out;
}

struct target_t *procimg(struct nstream_t *stream, struct nstream_t *depthimage)
{
    IplImage *in;
    IplImage *grey;
    IplImage *bw;
    CvSize isize;
    char *imgdata;
    struct bases_t *bases;
    struct target_t *target;
    /* struct target_t *ctarget; */

    /* Load image */
    isize.width = stream->imgwidth;
    isize.height = stream->imgheight;
    in = cvCreateImageHeader(isize, 8, stream->imgdepth);
    assert(in != NULL);

    /* lock, copy release buffer */
    imgdata = malloc(stream->bufsize);
    assert(imgdata);
    in->imageData = imgdata;

    pthread_mutex_lock(&stream->mutex);
    memcpy(in->imageData, stream->buf, stream->bufsize);
    pthread_mutex_unlock(&stream->mutex);

    /* Grey image */
    grey = cvCreateImage(cvGetSize(in), IPL_DEPTH_8U, 1);
    assert(grey != NULL);

    /* BW image */
    bw = cvCreateImage(cvGetSize(in), IPL_DEPTH_8U, 1);
    assert(bw != NULL);

    /* Convert the image to greyscale */
    cvCvtColor(in, grey, CV_RGB2GRAY);

    cvDilate(grey, bw, NULL, 3);
    cvCopy(bw, grey, NULL);

    /* Adaptive Threshold */
    cvAdaptiveThreshold(grey, bw, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, 31 /* 17 */ , 3     /* 5 
                                                                                                         */ );

    /* Dilate */
    /* cvDilate(bw, grey, NULL, 1); */
    cvCopy(bw, grey, NULL);
    /* cvSaveImage("out.png", grey, 0); */

    /* Make a copy of the image for later */
    cvCopy(grey, bw, NULL);

    bases = procContours(grey);
    target = findtargets(bases, bw, depthimage);

    /* 
       for(ctarget = target; ctarget != NULL; ctarget = ctarget->next){
       cvLine(in, ctarget->p0, ctarget->p1, CV_RGB(0xff, 0xff, 0xff), 1, 8, 0);
       cvLine(in, ctarget->p1, ctarget->p2, CV_RGB(0xff, 0xff, 0xff), 1, 8, 0);
       cvLine(in, ctarget->p2, ctarget->p3, CV_RGB(0xff, 0xff, 0xff), 1, 8, 0);
       cvLine(in, ctarget->p3, ctarget->p0, CV_RGB(0xff, 0xff, 0xff), 1, 8, 0);
       } */

    /* 
       cvSaveImage("out.png", bw, 0); cvSaveImage("rgbout.png", in, 0); */

    /* destroytargets(target); */
    destroybases(bases);
    cvReleaseImage(&bw);
    cvReleaseImage(&grey);

    cvReleaseImageHeader(&in);
    free(imgdata);

    return target;
}