0,0 → 1,459 |
#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(); |
} |
} |
} |