RectangleTracker.cpp

#include "RectangleTracker.h"

static void TrackTask(RectangleTracker *t) {
    while (true) {
        if (!t->Enabled()) continue;
        if (!t->camera->IsFreshImage()) continue;

        t->GetRectangles();
        if (t->MatchesFound()) {
            t->SelectTarget();
            t->PrintTarget();
        }
    }
}

RectangleTracker::RectangleTracker(int fps):
    task("TrackTask", (FUNCPTR)TrackTask)
{
    SetDefaultDetectionOptions();
    cameraFPS = fps;
}

RectangleTracker::~RectangleTracker() {
}

void RectangleTracker::SetCamera(AxisCamera &c) {
    camera = &c;
    camera->WriteMaxFPS(cameraFPS);
    task.Start((INT32)this);
}

void RectangleTracker::Start() {
    isEnabled = true;
}

void RectangleTracker::Stop() {
    isEnabled = false;
}

bool RectangleTracker::Enabled() {
    return isEnabled;
}

void RectangleTracker::GetRectangles() {
    ColorImage *rawImage = new ColorImage(IMAQ_IMAGE_RGB);
    camera->GetImage(rawImage);
    imageWidth  = rawImage->GetWidth();
    imageHeight = rawImage->GetHeight();

    MonoImage *grayscaleImage = rawImage->GetLuminancePlane();

    int numCurrentMatches;
    RectangleMatch *temp;
    temp = imaqDetectRectangles(grayscaleImage->GetImaqImage(),
                                &rectangleDescriptor,
                                &curveOptions,
                                &shapeOptions,
                                NULL,
                                &numCurrentMatches);
    matches.erase(matches.begin(), matches.end());
    for (int i = 0; i < numCurrentMatches; i++)
        matches.push_back(temp[i]);

    imaqDispose(temp);
    delete grayscaleImage;
    delete rawImage;
}

bool RectangleTracker::MatchesFound() {
    return matches.size() > 0;
}

void RectangleTracker::SelectTarget() {
    target = matches.at(0);
}

void RectangleTracker::PrintMatch(RectangleMatch match) {
    PointFloat *p = match.corner;
    printf("(%d):\t%dx%d (%.2f ft)\n\t(%d, %d)\n\t(%d, %d)\n\t(%d, %d)\n\t(%d, %d)\n\n",
            (int)match.score,
            (int)match.width,
            (int)match.height,
            CalculateDistance(match),
            (int)p[0].x, (int)p[0].y,
            (int)p[1].x, (int)p[1].y,
            (int)p[2].x, (int)p[2].y,
            (int)p[3].x, (int)p[3].y);
}

void RectangleTracker::PrintTarget() {
    PrintMatch(target);
}

float RectangleTracker::CalculateDistance(RectangleMatch match) {
    float imageWidthInFeet = (2 * imageWidth) / match.width;
    return (imageWidthInFeet / 2) / AXIS206_TANGENT;
}

void RectangleTracker::SetDefaultDetectionOptions() {
    curveOptions.extractionMode   = IMAQ_NORMAL_IMAGE;
    curveOptions.threshold        = 190;
    curveOptions.filterSize       = IMAQ_NORMAL;
    curveOptions.minLength        = 200;
    curveOptions.rowStepSize      = 15;
    curveOptions.columnStepSize   = 15;
    curveOptions.maxEndPointGap   = 10;
    curveOptions.onlyClosed       = false;
    curveOptions.subpixelAccuracy = false;

    shapeOptions.mode = IMAQ_GEOMETRIC_MATCH_SHIFT_INVARIANT;
    shapeOptions.angleRanges         = NULL;
    shapeOptions.numAngleRanges      = 0;
    shapeOptions.scaleRange.minValue = 75;
    shapeOptions.scaleRange.maxValue = 125;
    shapeOptions.minMatchScore       = 950;

    rectangleDescriptor.minWidth  = 100;
    rectangleDescriptor.maxWidth  = 400;
    rectangleDescriptor.minHeight = 100;
    rectangleDescriptor.maxHeight = 400;
}

RectangleTracker.h

#ifndef RECTANGLE_TRACKER_H
#define RECTANGLE_TRACKER_H

#define AXIS206_ANGLE 47
#define AXIS206_TANGENT 0.434812375

#include "WPILib.h"

class RectangleTracker {
public:
    RectangleTracker(int fps);
    ~RectangleTracker();
    void SetCamera(AxisCamera &c);
    void Start();
    void Stop();
    bool Enabled();
    void GetRectangles();
    bool MatchesFound();
    void SelectTarget();
    void PrintMatch(RectangleMatch match);
    void PrintTarget();

    AxisCamera *camera;
    RectangleDescriptor rectangleDescriptor;
    ShapeDetectionOptions shapeOptions;
    CurveOptions curveOptions;

private:
    int cameraFPS;
    int imageWidth, imageHeight;
    bool isEnabled;
    Task task;
    vector <RectangleMatch> matches;
    RectangleMatch target;

    float CalculateDistance(RectangleMatch match);
    void SetDefaultDetectionOptions();
};

#endif