Skip to content

Commit

Permalink
Added hystheresis support
Browse files Browse the repository at this point in the history
  • Loading branch information
Adamczyk Piotr committed Mar 21, 2020
1 parent ffd8d03 commit 75b0f78
Show file tree
Hide file tree
Showing 4 changed files with 140 additions and 43 deletions.
1 change: 1 addition & 0 deletions .gitignore
View file
Expand Up @@ -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
165 changes: 128 additions & 37 deletions VisualStudio/LaneKeeping/LaneDetection.cpp
View file
Expand Up @@ -3,11 +3,14 @@
cv::Mat LaneDetection::s_frame;
int LaneDetection::s_frameCenter;
int LaneDetection::s_maxLineHeight;
std::array<std::array<cv::Point, 4>, LaneDetection::s_hystheresisCount> LaneDetection::s_hystheresisArray = { {} };
unsigned short LaneDetection::s_hystheresisArrayCounter = 0;
bool LaneDetection::s_hystheresisArrayFilled = false;
cv::Mat LaneDetection::s_mask;
std::vector<cv::Vec4i> LaneDetection::s_lines;
std::vector<cv::Point> LaneDetection::s_rightLinePoints;
std::vector<cv::Point> LaneDetection::s_leftLinePoints;
std::array<cv::Point, 4> LaneDetection::s_boundaries;
std::array<cv::Point, 4> LaneDetection::s_boundaries = {};

void LaneDetection::createMask(const cv::Size& frameSize, double frameFormat) {
s_mask = cv::Mat::zeros(frameSize, frameFormat);
Expand All @@ -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<float>(0, 0) = -1;
kernel.at<float>(0, 1) = 0;
kernel.at<float>(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);
}

Expand Down Expand Up @@ -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<float, 4> 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<float>(s_frame.rows - left_b.y) / left_m) + left_b.x; //lower
xPositions[1] = (static_cast<float>(s_maxLineHeight - left_b.y) / left_m) + left_b.x; //upper
}

TODO: averaging over few frames
*/

//fit right lane
if (!s_rightLinePoints.empty()) {
Expand All @@ -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<float>(s_frame.rows - right_b.y) / right_m) + right_b.x;
float right_fin_x = (static_cast<float>(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<float>(s_frame.rows - right_b.y) / right_m) + right_b.x; //lower
xPositions[3] = (static_cast<float>(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<float>(s_frame.rows - left_b.y) / left_m) + left_b.x;
float left_fin_x = (static_cast<float>(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<float, 4> 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<cv::Point, 4> 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<int, 4> maxDiff = { maxLowerDiff, maxUpperDiff, maxLowerDiff, maxUpperDiff };
std::array<int, 4> 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) {
Expand All @@ -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) {
Expand All @@ -162,13 +259,7 @@ void LaneDetection::display(cv::Mat& frame) {
frame.copyTo(output);

//create semi-transparent trapezoid
std::array<cv::Point, 4> 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
Expand Down
12 changes: 9 additions & 3 deletions VisualStudio/LaneKeeping/LaneDetection.h
View file
Expand Up @@ -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<std::array<cv::Point, 4>, s_hystheresisCount> s_hystheresisArray;
static unsigned short s_hystheresisArrayCounter;
static bool s_hystheresisArrayFilled;

static cv::Mat s_mask;
static std::vector<cv::Vec4i> s_lines;

Expand All @@ -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<float, 4> 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);


};

5 changes: 2 additions & 3 deletions VisualStudio/LaneKeeping/main.cpp
View file
Expand Up @@ -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<int>( cap.get(cv::CAP_PROP_FRAME_WIDTH) ), static_cast<int>( 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<long long>( cap.get(cv::CAP_PROP_FRAME_COUNT) ) );

while (cap.read(frame)) {

auto t = new Timer("Loop");

LaneDetection::process(frame);
Expand Down

0 comments on commit 75b0f78

Please sign in to comment.