Subversion Repositories Code-Repo

#include "DepthProcessor.h"

// Limit the processing buffer to X frames
QSemaphore processorBuffer(3);

DepthProcessor::DepthProcessor(QObject *parent)
: QThread(parent) {

        moveToThread(this);

        topLeftImage = NULL;
        topRightImage = NULL;
        botLeftImage = NULL;
        botRightImage = NULL;

        rawDepthImage = QImage(X_RES, Y_RES, QImage::Format_ARGB32);
        lastValidData16 = cv::Mat(Y_RES, X_RES, CV_16UC1, cv::Scalar(0));
        lastValidDepthImage = QImage(X_RES, Y_RES, QImage::Format_ARGB32);
        lastValidProcessed = QImage(X_RES, Y_RES, QImage::Format_ARGB32);

        fgMaskMOG = cv::Mat(Y_RES, X_RES, CV_8UC1);
        movementMaskImage = QImage(X_RES, Y_RES, QImage::Format_ARGB32);
        fgMaskTmp = cv::Mat(Y_RES, X_RES, CV_32FC1);
        fgMaskRaw = cv::Mat(Y_RES, X_RES, CV_8UC1);
        movementMaskRawImage = QImage(X_RES, Y_RES, QImage::Format_ARGB32);
        fgMaskAverage = cv::Mat(Y_RES, X_RES, CV_8UC1);
        movementMaskAverageImage = QImage(X_RES, Y_RES, QImage::Format_ARGB32);
        pMOG = new cv::BackgroundSubtractorMOG2(BACKGROUND_SUBTRACTOR_HISTORY, BACKGROUND_SUBTRACTOR_NMIXTURES, false);

        rawHorizonImage = QImage(X_RES, Y_RES, QImage::Format_ARGB32);
        lastValidHorizonImage = QImage(X_RES, Y_RES, QImage::Format_ARGB32);
        overlayHorizonImage = QImage(X_RES, Y_RES, QImage::Format_ARGB32);

        depthHorizon = QVector<float>(X_RES);
        rawDepthHorizon = QVector<float>(X_RES);

        movementMaskHorizon = QVector<int>(X_RES);

        movementPointsImage = QImage(X_RES, Y_RES, QImage::Format_ARGB32);
        movementPointsMat = cv::Mat(Y_RES, X_RES, CV_8UC3);
        //params.thresholdStep = 20;
        //params.minThreshold = 50;
        //params.minThreshold = 500;
        params.minDistBetweenBlobs = BLOB_MIN_DISTANCE;
        params.minArea = BLOB_MIN_AREA;
        params.maxArea = BLOB_MAX_AREA;
        params.filterByColor = false;
        params.filterByCircularity = false;
        params.filterByConvexity = false;
        params.filterByInertia = false;
        params.filterByArea = true;
        blobDetector = new cv::SimpleBlobDetector(params);
}

DepthProcessor::~DepthProcessor() {

}

void DepthProcessor::setFOV(float width, float height) {
        fovWidth = width;
        fovHeight = height;
}

/**
 * Change the image to display on the main GUI
 */
void DepthProcessor::setDisplayImage(const int pane, const QString &image) {
        switch (pane) {
        case 0:
                if (image == "Raw Depth")
                        topLeftImage = &rawDepthImage;
                else if (image == "Last Valid Depth")
                        topLeftImage = &lastValidDepthImage;
                else if (image == "Movement Mask Raw Depth")
                        topLeftImage = &movementMaskRawImage;
                else if (image == "Movement Mask Average Depth")
                        topLeftImage = &movementMaskAverageImage;
                else if (image == "Processed Depth")
                        topLeftImage = &lastValidProcessed;
                else if (image == "Raw Depth Horizon")
                        topLeftImage = &rawHorizonImage;
                else if (image == "Last Valid Horizon")
                        topLeftImage = &lastValidHorizonImage;
                else if (image == "Overlay Horizon")
                        topLeftImage = &overlayHorizonImage;
                else if (image == "Movement Map")
                        topLeftImage = &movementPointsImage;
                break;
        case 1:
                if (image == "Raw Depth")
                        topRightImage = &rawDepthImage;
                else if (image == "Last Valid Depth")
                        topRightImage = &lastValidDepthImage;
                else if (image == "Movement Mask Raw Depth")
                        topRightImage = &movementMaskRawImage;
                else if (image == "Movement Mask Average Depth")
                        topRightImage = &movementMaskAverageImage;
                else if (image == "Processed Depth")
                        topRightImage = &lastValidProcessed;
                else if (image == "Raw Depth Horizon")
                        topRightImage = &rawHorizonImage;
                else if (image == "Last Valid Horizon")
                        topRightImage = &lastValidHorizonImage;
                else if (image == "Overlay Horizon")
                        topRightImage = &overlayHorizonImage;
                else if (image == "Movement Map")
                        topRightImage = &movementPointsImage;
                break;
        case 2:
                if (image == "Raw Depth")
                        botLeftImage = &rawDepthImage;
                else if (image == "Last Valid Depth")
                        botLeftImage = &lastValidDepthImage;
                else if (image == "Movement Mask Raw Depth")
                        botLeftImage = &movementMaskRawImage;
                else if (image == "Movement Mask Average Depth")
                        botLeftImage = &movementMaskAverageImage;
                else if (image == "Processed Depth")
                        botLeftImage = &lastValidProcessed;
                else if (image == "Raw Depth Horizon")
                        botLeftImage = &rawHorizonImage;
                else if (image == "Last Valid Horizon")
                        botLeftImage = &lastValidHorizonImage;
                else if (image == "Overlay Horizon")
                        botLeftImage = &overlayHorizonImage;
                else if (image == "Movement Map")
                        botLeftImage = &movementPointsImage;
                break;
        case 3:
                if (image == "Raw Depth")
                        botRightImage = &rawDepthImage;
                else if (image == "Last Valid Depth")
                        botRightImage = &lastValidDepthImage;
                else if (image == "Movement Mask Raw Depth")
                        botRightImage = &movementMaskRawImage;
                else if (image == "Movement Mask Average Depth")
                        botRightImage = &movementMaskAverageImage;
                else if (image == "Processed Depth")
                        botRightImage = &lastValidProcessed;
                else if (image == "Raw Depth Horizon")
                        botRightImage = &rawHorizonImage;
                else if (image == "Last Valid Horizon")
                        botRightImage = &lastValidHorizonImage;
                else if (image == "Overlay Horizon")
                        botRightImage = &overlayHorizonImage;
                else if (image == "Movement Map")
                        botRightImage = &movementPointsImage;
                break;
        default:
                break;
        }
}

/**
 * Updates the main GUI
 */
void DepthProcessor::updateImages() {
        emit setImageTopLeft(*topLeftImage);
        emit setImageTopRight(*topRightImage);
        emit setImageBotLeft(*botLeftImage);
        emit setImageBotRight(*botRightImage);
}

/**
 * Here we process the raw data from the sensor
 */
void DepthProcessor::processDepthData(const cv::Mat &data) {
        // The 16-bit raw image is passed in via a pointer
        rawData16 = data;

        // Save a pixel as valid data if it is != 0
        for (int y = 0; y < Y_RES; y++) {
                for (int x = 0; x < X_RES; x++) {
                        if (rawData16.ptr<ushort>(y)[x] != 0) {
                                lastValidData16.ptr<ushort>(y)[x] = rawData16.ptr<ushort>(y)[x];
                        }
                }
        }

        // Apply a 5-pixel wide median filter to the data for noise removal
        //cv::medianBlur(lastValidData16, lastValidData16, 5);

        // Execute a background subtraction to obtain moving objects
        pMOG->operator()(lastValidData16, fgMaskMOG, -1);

        fgMaskMOG.copyTo(fgMaskRaw);

        // Erode then dilate the mask to remove noise
        //cv::erode(fgMaskMOG, fgMaskMOG, cv::Mat());
        //cv::dilate(fgMaskMOG, fgMaskMOG, cv::Mat());
        // Alternative:
        int kernelSize = 9;
        cv::Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(kernelSize, kernelSize));
        // Morphological opening (remove small objects from foreground)
        cv::morphologyEx(fgMaskMOG, fgMaskMOG, cv::MORPH_OPEN, kernel);
        // Morphological closing (fill small holes in the foreground)
        cv::morphologyEx(fgMaskMOG, fgMaskMOG, cv::MORPH_CLOSE, kernel);

        // Average the moving mask's values and shrink it by a bit to remove edges
        cv::accumulateWeighted(fgMaskMOG, fgMaskTmp, 0.5);
        cv::convertScaleAbs(fgMaskTmp, fgMaskAverage);
        cv::erode(fgMaskAverage, fgMaskAverage, kernel);

        // Get the closest distance in the specified range that is not 0 and convert to inches
        for (int x = 0; x < X_RES; x++) {
                ushort min = 9999;
                ushort rawMin = 9999;
                for (int y = VFOV_MIN; y < VFOV_MAX; y++) {
                        if (lastValidData16.ptr<ushort>(y)[x] != 0)
                                min = qMin(min, lastValidData16.ptr<ushort>(y)[x]);
                        rawMin = qMin(rawMin, rawData16.ptr<ushort>(y)[x]);
                }

                // Convert the raw distance values to distance in inches
                // Distance (inches) = (raw distance - 13.072) / 25.089;
                depthHorizon[x] = (min - 13.072) / 25.089;
                rawDepthHorizon[x] = (rawMin - 13.072) / 25.089;
        }

        // Mark the points of detected movements in the movement mask if the threshold is exceeded
        for (int x = 0; x < X_RES; x++) {
                int moved = 0;
                for (int y = VFOV_MIN; y < VFOV_MAX; y++) {
                        if (fgMaskAverage.ptr<uchar>(y)[x] >= FG_MASK_THRESHOLD)
                                moved = 1;
                }
                movementMaskHorizon[x] = moved;
        }

        // Draw all images
        drawDepthImages();
        drawFOVImages();

        // Update GUI with selected image
        updateImages();

        processorBuffer.release(1);
}

/**
 * Generate a visualization of the depth data
 */
void DepthProcessor::drawDepthImages() {
        // Convert raw data to images to be displayed
        for (int y = 0; y < Y_RES; ++y) {
                for (int x = 0; x < X_RES; ++x) {
                        // rawDepthImage
                        rawDepthImage.setPixel(x, y, qRgb(rawData16.ptr<ushort>(y)[x] / (SCALE_DIVISOR / 2),
                                rawData16.ptr<ushort>(y)[x] / SCALE_DIVISOR, rawData16.ptr<ushort>(y)[x] / (SCALE_DIVISOR * 2)));

                        // lastValidDepthImage
                        lastValidDepthImage.setPixel(x, y, qRgb(lastValidData16.ptr<ushort>(y)[x] / (SCALE_DIVISOR / 2),
                                lastValidData16.ptr<ushort>(y)[x] / SCALE_DIVISOR, lastValidData16.ptr<ushort>(y)[x] / (SCALE_DIVISOR * 2)));

                        // lastValidProcessed
                        if (fgMaskMOG.ptr<uchar>(y)[x] == 0) {
                                lastValidProcessed.setPixel(x, y, qRgba(lastValidData16.ptr<ushort>(y)[x] / (SCALE_DIVISOR / 2),
                                        lastValidData16.ptr<ushort>(y)[x] / SCALE_DIVISOR, lastValidData16.ptr<ushort>(y)[x] / (SCALE_DIVISOR * 2), 150));
                        } else {
                                lastValidProcessed.setPixel(x, y, qRgb(lastValidData16.ptr<ushort>(y)[x] / (SCALE_DIVISOR / 2),
                                        lastValidData16.ptr<ushort>(y)[x] / SCALE_DIVISOR, lastValidData16.ptr<ushort>(y)[x] / (SCALE_DIVISOR * 2)));
                        }

                        // movementMaskImage
                        movementMaskRawImage.setPixel(x, y, qRgb(fgMaskRaw.ptr<uchar>(y)[x], fgMaskRaw.ptr<uchar>(y)[x], fgMaskRaw.ptr<uchar>(y)[x]));

                        // movementMaskAverageImage
                        movementMaskAverageImage.setPixel(x, y, qRgb(fgMaskAverage.ptr<uchar>(y)[x], fgMaskAverage.ptr<uchar>(y)[x], fgMaskAverage.ptr<uchar>(y)[x]));
                }
        }

        // Draw lines indicating the FOV zones
        QPainter imagePainter;

        imagePainter.begin(&rawDepthImage);
        imagePainter.setPen(QPen(COLOR_DEPTH_FOV, 1));
        imagePainter.drawLine(0, VFOV_MIN, X_RES, VFOV_MIN);
        imagePainter.drawLine(0, VFOV_MAX, X_RES, VFOV_MAX);
        imagePainter.end();

        imagePainter.begin(&lastValidDepthImage);
        imagePainter.setPen(QPen(COLOR_DEPTH_FOV, 1));
        imagePainter.drawLine(0, VFOV_MIN, X_RES, VFOV_MIN);
        imagePainter.drawLine(0, VFOV_MAX, X_RES, VFOV_MAX);
        imagePainter.end();

        imagePainter.begin(&lastValidProcessed);
        imagePainter.setPen(QPen(COLOR_DEPTH_FOV, 1));
        imagePainter.setBrush(QBrush(COLOR_DEPTH_FOV_FILL));
        imagePainter.drawLine(0, VFOV_MIN, X_RES, VFOV_MIN);
        imagePainter.drawLine(0, VFOV_MAX, X_RES, VFOV_MAX);
        imagePainter.drawRect(0, 0, X_RES, VFOV_MIN);
        imagePainter.drawRect(0, VFOV_MAX, X_RES, Y_RES);
        imagePainter.end();
}

/**
 * Draws the given vector of points onto an image (projected across an arc)
 */
void DepthProcessor::drawDistanceFOV(QImage &image, QVector<float> &data) {
        QPainter painter;
        painter.begin(&image);

        // Draw the FOV for the raw data
        painter.translate(X_RES / 2, Y_RES);
        // Rotate the canvas, draw all distances, then restore original coordinates
        painter.rotate(-90 - qRadiansToDegrees(fovWidth / 2));
        for (int x = 0; x < X_RES; x++) {
                painter.rotate(qRadiansToDegrees(fovWidth / X_RES));
                painter.setPen(QPen(COLOR_DEPTH_POINT, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
                painter.drawPoint(data[x], 0);
                painter.setPen(QPen(COLOR_DEPTH_BACKGROUND, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
                painter.drawLine(QPoint(data[x], 0), QPoint(400, 0));
        }

        painter.end();
}

/**
* Draws the sensor's FOV onto the image
*/
void DepthProcessor::drawSensorFOV(QImage &image) {
        QPainter painter;
        painter.begin(&image);

        // Draw the sensor's FOV
        painter.translate(X_RES / 2, Y_RES);
        painter.setPen(QPen(COLOR_FOV, 2, Qt::DashLine));
        painter.rotate(-90 - qRadiansToDegrees(fovWidth / 2));
        painter.drawLine(0, 0, X_RES, 0);
        painter.rotate(qRadiansToDegrees(fovWidth));
        painter.drawLine(0, 0, X_RES, 0);

        painter.end();
}

/**
* Generate a horizontal visualization of the depth data
*/
void DepthProcessor::drawFOVImages() {
        // Draw the raw FOV
        rawHorizonImage.fill(Qt::white);
        drawDistanceFOV(rawHorizonImage, rawDepthHorizon);
        drawSensorFOV(rawHorizonImage);

        // Draw the last valid data FOV
        lastValidHorizonImage.fill(Qt::white);
        drawDistanceFOV(lastValidHorizonImage, depthHorizon);
        drawSensorFOV(lastValidHorizonImage);

        // Draw only the movement points along with results of blob detection
        movementPointsImage.fill(Qt::white);
        drawMovementZones(movementPointsImage, depthHorizon);
        convertQImageToMat3C(movementPointsImage, movementPointsMat);
        blobDetector->detect(movementPointsMat, blobKeypoints);
        std::vector<cv::Point2f> points;
        for (int i = 0; i < blobKeypoints.size(); i++) {
                points.push_back(cv::Point2f(blobKeypoints[i].pt.x, blobKeypoints[i].pt.y));
        }
        movementObjects = movementTracker.update(points);
        drawKeyPoints(movementPointsImage, blobKeypoints);
        drawMovingObjects(movementPointsImage, movementObjects);
        drawSensorFOV(movementPointsImage);

        // Draw the overlay of movements onto static objects
        overlayHorizonImage.fill(Qt::white);
        drawDistanceFOV(overlayHorizonImage, depthHorizon);
        drawMovementZones(overlayHorizonImage, depthHorizon);
        drawKeyPoints(overlayHorizonImage, blobKeypoints);
        drawMovingObjects(overlayHorizonImage, movementObjects);
        drawSensorFOV(overlayHorizonImage);
}

/**
* Draws the zones of detected movement on the map
*/
void DepthProcessor::drawMovementZones(QImage &image, QVector<float> &data) {
        QPainter painter;
        painter.begin(&image);

        // Draw the FOV for the raw data
        painter.translate(X_RES / 2, Y_RES);
        // Rotate the canvas, draw all distances, then restore original coordinates
        painter.rotate(-90 - qRadiansToDegrees(fovWidth / 2));
        painter.setPen(QPen(COLOR_MOVEMENT_ZONE, 20, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
        for (int x = 0; x < X_RES; x++) {
                painter.rotate(qRadiansToDegrees(fovWidth / X_RES));
                if (movementMaskHorizon[x] == 1)
                        painter.drawPoint(data[x], 0);
        }

        painter.end();
}

/**
* Draws the set of keypoints on the image
*/
void DepthProcessor::drawKeyPoints(QImage &image, std::vector<cv::KeyPoint> &points) {
        QPainter painter;
        painter.begin(&image);

        painter.setPen(QPen(COLOR_KEYPOINT, 6, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
        for (int i = 0; i < points.size(); i++) {
                painter.drawPoint(points[i].pt.x, points[i].pt.y);
        }

        painter.end();
}

/**
* Draws the moving objects along with its ID, velocity, and angle of predicted movement
*/
void DepthProcessor::drawMovingObjects(QImage &image, std::vector<MOVING_OBJECT> &objects) {
        QPainter painter;
        painter.begin(&image);

        for (int i = 0; i < objects.size(); i++) {
                QPoint initPoint = QPoint(objects[i].predicted_pt.x, objects[i].predicted_pt.y);
                // Calculate the line to draw to indicate object movement velocity and angle
                float velocity_x = initPoint.x() + (objects[i].velocity * cos(objects[i].angle)) * VELOCITY_MULTIPLIER;
                float velocity_y = initPoint.y() + (objects[i].velocity * sin(objects[i].angle)) * VELOCITY_MULTIPLIER;
                QPointF predPoint = QPointF(velocity_x, velocity_y);
                // Draw the object's estimated position
                painter.setPen(QPen(COLOR_EST_POSITION, 6, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
                painter.drawPoint(initPoint);
                // Draw the object's ID
                painter.drawText(initPoint.x() + 3, initPoint.y() - 3, QString::number(objects[i].ID));
                // Draw the line indicating object's movement velocity and angle
                painter.setPen(QPen(COLOR_EST_POSITION, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
                painter.drawLine(initPoint, predPoint);
                // Draw the object's running average
                painter.setPen(QPen(COLOR_EST_AVGERAGE, 6, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
                painter.drawPoint(objects[i].historyAvg.x, objects[i].historyAvg.y);
        }

        painter.end();
}

void DepthProcessor::convertMatToQImage3C(cv::Mat &mat, QImage &image) {
        uchar *ptr;
        for (int y = 0; y < Y_RES; y++) {
                ptr = mat.ptr<uchar>(y);
                for (int x = 0; x < X_RES; x++) {
                        image.setPixel(x, y, qRgb(ptr[x], ptr[x], ptr[x]));
                }
        }
}

void DepthProcessor::convertQImageToMat3C(QImage &image, cv::Mat &mat) {
        for (int y = 0; y < Y_RES; y++) {
                for (int x = 0; x < X_RES; x++) {
                        cv::Vec3b &pixel = mat.at<cv::Vec3b>(y, x);
                        QColor pixColor(image.pixel(x, y));
                        pixel.val[0] = pixColor.blue();
                        pixel.val[1] = pixColor.green();
                        pixel.val[2] = pixColor.red();
                }
        }
}