From 75b0f7863e084e0d97062172fc21871f5c212eaa Mon Sep 17 00:00:00 2001 From: Adamczyk Piotr Date: Sat, 21 Mar 2020 11:54:41 +0100 Subject: [PATCH] Added hystheresis support --- .gitignore | 1 + VisualStudio/LaneKeeping/LaneDetection.cpp | 165 ++++++++++++++++----- VisualStudio/LaneKeeping/LaneDetection.h | 12 +- VisualStudio/LaneKeeping/main.cpp | 5 +- 4 files changed, 140 insertions(+), 43 deletions(-) diff --git a/.gitignore b/.gitignore index 0afee0b..cf45efb 100644 --- a/.gitignore +++ b/.gitignore @@ -62,3 +62,4 @@ VisualStudio/x64/Release/LaneKeeping.pdb VisualStudio/LaneKeeping/x64/Debug/LaneDetector.obj VisualStudio/LaneKeeping/x64/Release/LaneDetector.obj VisualStudio/LaneKeeping/x64/Release/LaneKeeping.tlog/link.delete.1.tlog +VisualStudio/LaneKeeping/x64/Debug/lanedetection.obj.enc diff --git a/VisualStudio/LaneKeeping/LaneDetection.cpp b/VisualStudio/LaneKeeping/LaneDetection.cpp index 2b5fd64..50b4ee9 100644 --- a/VisualStudio/LaneKeeping/LaneDetection.cpp +++ b/VisualStudio/LaneKeeping/LaneDetection.cpp @@ -3,11 +3,14 @@ cv::Mat LaneDetection::s_frame; int LaneDetection::s_frameCenter; int LaneDetection::s_maxLineHeight; +std::array, LaneDetection::s_hystheresisCount> LaneDetection::s_hystheresisArray = { {} }; +unsigned short LaneDetection::s_hystheresisArrayCounter = 0; +bool LaneDetection::s_hystheresisArrayFilled = false; cv::Mat LaneDetection::s_mask; std::vector LaneDetection::s_lines; std::vector LaneDetection::s_rightLinePoints; std::vector LaneDetection::s_leftLinePoints; -std::array LaneDetection::s_boundaries; +std::array LaneDetection::s_boundaries = {}; void LaneDetection::createMask(const cv::Size& frameSize, double frameFormat) { s_mask = cv::Mat::zeros(frameSize, frameFormat); @@ -29,31 +32,35 @@ inline void LaneDetection::applyMask() { cv::bitwise_and(s_frame, s_mask, s_frame); } +inline void LaneDetection::changeContrast() { + //cv::convertScaleAbs(s_frame, s_frame, 1.3, 0); +} + inline void LaneDetection::blur() { - cv::GaussianBlur(s_frame, s_frame, cv::Size(3, 3), 0, 0); //3x3px trial & error + cv::GaussianBlur(s_frame, s_frame, cv::Size(7, 7), 0, 0); //7x7px trial & error } inline void LaneDetection::edgeDetection() { - + //TODO: ARM NEON optimization cv::cvtColor(s_frame, s_frame, cv::COLOR_RGB2GRAY); //binarize gray image + //TODO: runtime calibration every x seconds cv::threshold(s_frame, s_frame, 140, 255, cv::THRESH_BINARY); //threshold trial & error - - /* + /* Create the kernel [-1 0 1] This kernel is based on the one found in the Lane Departure Warning System by Mathworks - */ + cv::Point anchor = cv::Point(-1, -1); cv::Mat kernel = cv::Mat(1, 3, CV_32F); kernel.at(0, 0) = -1; kernel.at(0, 1) = 0; kernel.at(0, 2) = 1; - //filter the binary image to obtain the edges //compare results to CannyEdge?? + //filter the binary image to obtain the edges cv::filter2D(s_frame, s_frame, -1, kernel, anchor, 0, cv::BORDER_DEFAULT); } @@ -93,12 +100,20 @@ void LaneDetection::classifyLines() { void LaneDetection::leastSquaresRegression() { - /* - if left/right lane is not detected previous value is reused - issue: if at least 1 lane is not detected in 1st frame => garbage output + std::array xPositions = { 0.f, 0.f, 0.f, 0.f }; + + //fit left lane + if (!s_leftLinePoints.empty()) { + cv::Vec4d left_line; + + cv::fitLine(s_leftLinePoints , left_line, cv::DIST_L2, 0, 0.01, 0.01); + float left_m = left_line[1] / left_line[0]; + cv::Point left_b = cv::Point(left_line[2], left_line[3]); + + xPositions[0] = (static_cast(s_frame.rows - left_b.y) / left_m) + left_b.x; //lower + xPositions[1] = (static_cast(s_maxLineHeight - left_b.y) / left_m) + left_b.x; //upper + } - TODO: averaging over few frames - */ //fit right lane if (!s_rightLinePoints.empty()) { @@ -108,25 +123,103 @@ void LaneDetection::leastSquaresRegression() { float right_m = right_line[1] / right_line[0]; cv::Point right_b = cv::Point(right_line[2], right_line[3]); // y = m*x + b - float right_ini_x = (static_cast(s_frame.rows - right_b.y) / right_m) + right_b.x; - float right_fin_x = (static_cast(s_maxLineHeight - right_b.y) / right_m) + right_b.x; - s_boundaries[0] = cv::Point(right_ini_x, s_frame.rows); - s_boundaries[1] = cv::Point(right_fin_x, s_maxLineHeight); + xPositions[2] = (static_cast(s_frame.rows - right_b.y) / right_m) + right_b.x; //lower + xPositions[3] = (static_cast(s_maxLineHeight - right_b.y) / right_m) + right_b.x; //upper } - - //fit left lane - if (!s_leftLinePoints.empty()) { - cv::Vec4d left_line; - cv::fitLine(s_leftLinePoints , left_line, cv::DIST_L2, 0, 0.01, 0.01); - float left_m = left_line[1] / left_line[0]; - cv::Point left_b = cv::Point(left_line[2], left_line[3]); - float left_ini_x = (static_cast(s_frame.rows - left_b.y) / left_m) + left_b.x; - float left_fin_x = (static_cast(s_maxLineHeight - left_b.y) / left_m) + left_b.x; - s_boundaries[2] = cv::Point(left_ini_x, s_frame.rows); - s_boundaries[3] = cv::Point(left_fin_x, s_maxLineHeight); + hystheresis(xPositions); +} + +inline void LaneDetection::hystheresis(std::array xPositions) { + + s_hystheresisArray[s_hystheresisArrayCounter][0] = cv::Point(xPositions[0], s_frame.rows); + s_hystheresisArray[s_hystheresisArrayCounter][1] = cv::Point(xPositions[1], s_maxLineHeight); + s_hystheresisArray[s_hystheresisArrayCounter][2] = cv::Point(xPositions[2], s_frame.rows); + s_hystheresisArray[s_hystheresisArrayCounter][3] = cv::Point(xPositions[3], s_maxLineHeight); + + if (s_hystheresisArrayFilled) { + + std::array previousRow; + if (s_hystheresisArrayCounter == 0) { + previousRow = s_hystheresisArray[s_hystheresisCount - 1]; + } else { + previousRow = s_hystheresisArray[s_hystheresisArrayCounter - 1]; + } + + const int maxLowerDiff = 0.01f * s_frame.cols; + const int maxUpperDiff = 0.004f * s_frame.cols; + + std::array maxDiff = { maxLowerDiff, maxUpperDiff, maxLowerDiff, maxUpperDiff }; + std::array avgXPositions = {}; + + //unsigned short attempts = 0; + + //average over whole array (excluding 0-values) + for (unsigned short i = 0; i < 4; i++) { + unsigned short skipped = 0; + int avg = 0; + + for (unsigned short j = 0; j < s_hystheresisCount; j++) { + + if (s_hystheresisArray[j][i].x == 0) skipped++; + avg += s_hystheresisArray[j][i].x; + } + + avgXPositions[i] = avg; + if (s_hystheresisCount == skipped) { + errorHanlder(); + return; + } + avgXPositions[i] /= (s_hystheresisCount - skipped); + /* + bool reCalculate = false; + for (unsigned short j = 0; j < s_hystheresisCount; j++) { + + if (s_hystheresisArray[j][i].x != 0 && s_hystheresisArray[j][i].x * 5 < avg) { + reCalculate = true; + break; + } + } + + if (reCalculate && attempts < 5) { + std::cout << "Recalculating...\n"; + i--; + attempts++; + continue; + } else { + attempts = 0; + } + */ + int diff = avgXPositions[i] - previousRow[i].x; + if (diff > maxDiff[i]) { + avgXPositions[i] = previousRow[i].x + maxDiff[i]; + } else if (diff < -1 * maxDiff[i]) { + avgXPositions[i] = previousRow[i].x - maxDiff[i]; + } + } + + s_hystheresisArray[s_hystheresisArrayCounter][0] = cv::Point(avgXPositions[0], s_frame.rows); + s_hystheresisArray[s_hystheresisArrayCounter][1] = cv::Point(avgXPositions[1], s_maxLineHeight); + s_hystheresisArray[s_hystheresisArrayCounter][2] = cv::Point(avgXPositions[2], s_frame.rows); + s_hystheresisArray[s_hystheresisArrayCounter][3] = cv::Point(avgXPositions[3], s_maxLineHeight); } + + s_boundaries[0] = s_hystheresisArray[s_hystheresisArrayCounter][0]; + s_boundaries[1] = s_hystheresisArray[s_hystheresisArrayCounter][1]; + s_boundaries[3] = s_hystheresisArray[s_hystheresisArrayCounter][2]; + s_boundaries[2] = s_hystheresisArray[s_hystheresisArrayCounter][3]; + + + s_hystheresisArrayCounter++; + if (s_hystheresisArrayCounter == s_hystheresisCount) { + s_hystheresisArrayCounter = 0; + s_hystheresisArrayFilled = true; + } +} + +void LaneDetection::errorHanlder() { + std::cerr << "An error has occured!\n"; } void LaneDetection::prepare(const cv::Size& frameSize, double frameFormat) { @@ -141,19 +234,23 @@ void LaneDetection::setFrame(const cv::Mat& frame) { void LaneDetection::process(cv::Mat& frame) { setFrame(frame); - + changeContrast(); blur(); //remove noise by blurring image edgeDetection(); //detect edges - applyMask(); //crop ROI houghLines(); // use HoughLinesP + + cv::imshow("asd",s_frame); + cv::waitKey(1); if (!s_lines.empty()) { classifyLines(); //Classify which lines are for left or right lane leastSquaresRegression(); //calculate lane regression - display(frame); + } else { + errorHanlder(); } + } void LaneDetection::display(cv::Mat& frame) { @@ -162,13 +259,7 @@ void LaneDetection::display(cv::Mat& frame) { frame.copyTo(output); //create semi-transparent trapezoid - std::array poly_points; - poly_points[0] = s_boundaries[2]; - poly_points[1] = s_boundaries[0]; - poly_points[2] = s_boundaries[1]; - poly_points[3] = s_boundaries[3]; - - cv::fillConvexPoly(output, poly_points.data(), 4, cv::Scalar(255, 255, 255), cv::LINE_AA, 0); + cv::fillConvexPoly(output, s_boundaries.data(), 4, cv::Scalar(255, 255, 255), cv::LINE_AA, 0); cv::addWeighted(output, 0.4, frame, 0.6, 0, frame); //draw left & right lane diff --git a/VisualStudio/LaneKeeping/LaneDetection.h b/VisualStudio/LaneKeeping/LaneDetection.h index 7e3161d..8da3496 100644 --- a/VisualStudio/LaneKeeping/LaneDetection.h +++ b/VisualStudio/LaneKeeping/LaneDetection.h @@ -5,10 +5,15 @@ class LaneDetection { - static cv::Mat s_frame; //Current frame + static cv::Mat s_frame; static int s_frameCenter; static int s_maxLineHeight; + static const unsigned short s_hystheresisCount = 6; + static std::array, s_hystheresisCount> s_hystheresisArray; + static unsigned short s_hystheresisArrayCounter; + static bool s_hystheresisArrayFilled; + static cv::Mat s_mask; static std::vector s_lines; @@ -18,19 +23,20 @@ class LaneDetection { static void createMask(const cv::Size& frameSize, double frameFormat); static inline void applyMask(); + static inline void changeContrast(); static inline void blur(); static inline void edgeDetection(); static inline void houghLines(); static void classifyLines(); static void leastSquaresRegression(); + static inline void hystheresis(std::array xPositions); + static void errorHanlder(); public: static void prepare(const cv::Size& frameSize, double frameFormat); static void setFrame(const cv::Mat& frame); static void process(cv::Mat& frame); static void display(cv::Mat& frame); - - }; diff --git a/VisualStudio/LaneKeeping/main.cpp b/VisualStudio/LaneKeeping/main.cpp index eb5ea8e..fea5ec5 100644 --- a/VisualStudio/LaneKeeping/main.cpp +++ b/VisualStudio/LaneKeeping/main.cpp @@ -16,16 +16,15 @@ int main(int argc, const char** argv) { if (!cap.isOpened()) return -1; //Set up lane detection parameters - cv::Size frameSize = cv::Size(cap.get(cv::CAP_PROP_FRAME_WIDTH), cap.get(cv::CAP_PROP_FRAME_HEIGHT)); + cv::Size frameSize = cv::Size( static_cast( cap.get(cv::CAP_PROP_FRAME_WIDTH) ), static_cast( cap.get(cv::CAP_PROP_FRAME_HEIGHT) ) ); double frameFormat = cap.get(cv::CAP_PROP_FORMAT); LaneDetection::prepare(frameSize, frameFormat); cv::Mat frame; - auto globalTimer = new Timer("Loop", cap.get(cv::CAP_PROP_FRAME_COUNT)); + auto globalTimer = new Timer("Loop", static_cast( cap.get(cv::CAP_PROP_FRAME_COUNT) ) ); while (cap.read(frame)) { - auto t = new Timer("Loop"); LaneDetection::process(frame);