143 lines
4.2 KiB
C++
143 lines
4.2 KiB
C++
#include "lfr.h"
|
|
|
|
#define right false
|
|
#define left true
|
|
|
|
LFR::LFR(int videoHeight, int videoWidth, int thresholdBinary, int gaussKernelSize, ExceptionCallback cb):
|
|
stop(false),
|
|
input(videoHeight, videoWidth),
|
|
processing(),
|
|
controlModule(),
|
|
interpreter(),
|
|
intersectionHandler(),
|
|
cb(cb)
|
|
{
|
|
this->thresholdBinary = thresholdBinary;
|
|
this->gaussKernelSize = gaussKernelSize;
|
|
}
|
|
|
|
LFR::~LFR()
|
|
{
|
|
endLoop();
|
|
thread->join();
|
|
}
|
|
|
|
void LFR::removeListener(LFR::ListenerKey key)
|
|
{
|
|
std::lock_guard<std::mutex> lock(mutex);
|
|
auto it = std::find_if(std::begin(listeners), std::end(listeners), [&](auto const &val){
|
|
return val.first == key;
|
|
});
|
|
if(it != std::end(listeners))
|
|
{
|
|
listeners.erase(it);
|
|
}
|
|
}
|
|
|
|
void LFR::addListener(LFR::ListenerCallback cb, LFR::ListenerKey key)
|
|
{
|
|
std::lock_guard<std::mutex> lock(mutex);
|
|
listeners.emplace_back(key, std::move(cb));
|
|
}
|
|
|
|
void LFR::setStop(bool val)
|
|
{
|
|
std::lock_guard<std::mutex> lock(mutex);
|
|
stop = val;
|
|
}
|
|
|
|
void LFR::createThread()
|
|
{
|
|
thread = std::make_unique<std::thread>([this](){
|
|
while(true)
|
|
{
|
|
LFR_Result result;
|
|
if(!stop && !listeners.empty())
|
|
{
|
|
try
|
|
{
|
|
std::lock_guard<std::mutex> lock(mutex);
|
|
Mat originalImage = input.readWebcam();
|
|
|
|
Point roiOrigin(0, int(originalImage.rows*(3.25/6.0)));
|
|
Rect roi(roiOrigin.x, roiOrigin.y, originalImage.cols, originalImage.rows/6);
|
|
Mat processedImage = originalImage;
|
|
|
|
processing.processImage(processedImage, this->thresholdBinary, this->gaussKernelSize);
|
|
FrameData data = processing.calculateLineSegments(processedImage, roi);
|
|
processing.filterReflections(data);
|
|
processing.calcAngles(data, originalImage.cols, originalImage.rows, left);
|
|
|
|
processedImage = provideOutput(originalImage, processedImage, data, roi);
|
|
|
|
result.rawImage = originalImage;
|
|
result.processedImage = processedImage;
|
|
result.data = data;
|
|
}
|
|
catch(std::exception const &ex)
|
|
{
|
|
if(!cb(ex))
|
|
{
|
|
//callback returned false -> exception not handled -> exit
|
|
exit(EXIT_FAILURE);
|
|
}
|
|
}
|
|
|
|
//Invoke the callback method (ListenerPair second -> ListenerCallback)
|
|
for(auto &val : listeners)
|
|
{
|
|
val.second(result);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
});
|
|
}
|
|
|
|
void LFR::startLoop()
|
|
{
|
|
if(thread)
|
|
{
|
|
//Restart thread if it is running
|
|
setStop(true);
|
|
thread->join();
|
|
setStop(false);
|
|
}
|
|
createThread();
|
|
}
|
|
|
|
void LFR::endLoop()
|
|
{
|
|
setStop(true);
|
|
}
|
|
|
|
cv::Mat LFR::provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi)
|
|
{
|
|
for(int i = 0; i < frameData.contours.size(); i++)
|
|
{
|
|
drawContours(originalImage, frameData.contours, i, Scalar(0,255,255), 1, 8, noArray(), 0, Point(roi.x, roi.y));
|
|
|
|
rectangle(originalImage, frameData.boundingBoxes[i], Scalar(0,255,0));
|
|
Rect center(Point(frameData.middlePoints[i].x-2, frameData.middlePoints[i].y-2), Point(frameData.middlePoints[i].x+2, frameData.middlePoints[i].y+2));
|
|
rectangle(originalImage, center, Scalar(0,0,255));
|
|
Rect leftRect(Point(frameData.leftEdges[i].x-2, frameData.leftEdges[i].y-2), Point(frameData.leftEdges[i].x+2, frameData.leftEdges[i].y+2));
|
|
rectangle(originalImage, leftRect, Scalar(0,0,255));
|
|
}
|
|
|
|
if(frameData.contours.size() > 0)
|
|
{
|
|
//Draw the Arrow for the check of the angle
|
|
int length = 100;
|
|
Point P1 = frameData.middlePoints[frameData.index];
|
|
Point P2;
|
|
|
|
P2.x = (int)round(P1.x + length * cos(frameData.angle * CV_PI / 180.0));
|
|
P2.y = (int)round(P1.y + length * sin(frameData.angle * CV_PI / 180.0));
|
|
cv::arrowedLine(originalImage, P1, P2, Scalar(0,0,255), 2, 8);
|
|
}
|
|
return originalImage;
|
|
}
|