C++ opencv实现车道线识别

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
class LaneDetector 
{
private:
 double img_size;
 double img_center;
 bool left_flag = false; // Tells us if there's left boundary of lane detected
 bool right_flag = false; // Tells us if there's right boundary of lane detected
 cv::Point right_b; // Members of both line equations of the lane boundaries:
 double right_m; // y = m*x + b
 cv::Point left_b; //
 double left_m; //

public:
 cv::Mat deNoise(cv::Mat inputImage); // Apply Gaussian blurring to the input Image
 cv::Mat edgeDetector(cv::Mat img_noise); // Filter the image to obtain only edges
 cv::Mat mask(cv::Mat img_edges); // Mask the edges image to only care about ROI
 std::vector<cv::Vec4i> houghLines(cv::Mat img_mask); // Detect Hough lines in masked edges image
 std::vector<std::vector<cv::Vec4i> > lineSeparation(std::vector<cv::Vec4i> lines, cv::Mat img_edges); // Sprt detected lines by their slope into right and left lines
 std::vector<cv::Point> regression(std::vector<std::vector<cv::Vec4i> > left_right_lines, cv::Mat inputImage); // Get only one line for each side of the lane
 std::string predictTurn(); // Determine if the lane is turning or not by calculating the position of the vanishing point
 int plotLane(cv::Mat inputImage, std::vector<cv::Point> lane, std::string turn); // Plot the resultant lane and turn prediction in the frame.
};

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
*@file LaneDetector.cpp
*@author Miguel Maestre Trueba
*@brief Definition of all the function that form part of the LaneDetector class.
*@brief The class will take RGB images as inputs and will output the same RGB image but
*@brief with the plot of the detected lanes and the turn prediction.
*/
#include <string>
#include <vector>
#include <opencv2/opencv.hpp>
#include "LaneDetector.h"

// IMAGE BLURRING
/**
*@brief Apply gaussian filter to the input image to denoise it
*@param inputImage is the frame of a video in which the
*@param lane is going to be detected
*@return Blurred and denoised image
*/
cv::Mat LaneDetector::deNoise(cv::Mat inputImage) {
 cv::Mat output;

 cv::GaussianBlur(inputImage, output, cv::Size(3, 3), 0, 0);

 return output;
}

// EDGE DETECTION
/**
*@brief Detect all the edges in the blurred frame by filtering the image
*@param img_noise is the previously blurred frame
*@return Binary image with only the edges represented in white
*/
cv::Mat LaneDetector::edgeDetector(cv::Mat img_noise) {
 cv::Mat output;
 cv::Mat kernel;
 cv::Point anchor;

 // Convert image from RGB to gray
 cv::cvtColor(img_noise, output, cv::COLOR_RGB2GRAY);
 // Binarize gray image
 cv::threshold(output, output, 140, 255, cv::THRESH_BINARY);

 // Create the kernel [-1 0 1]
 // This kernel is based on the one found in the
 // Lane Departure Warning System by Mathworks
 anchor = cv::Point(-1, -1);
 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
 cv::filter2D(output, output, -1, kernel, anchor, 0, cv::BORDER_DEFAULT);
 cv::imshow("output", output);
 return output;
}

// MASK THE EDGE IMAGE
/**
*@brief Mask the image so that only the edges that form part of the lane are detected
*@param img_edges is the edges image from the previous function
*@return Binary image with only the desired edges being represented
*/
cv::Mat LaneDetector::mask(cv::Mat img_edges) {
 cv::Mat output;
 cv::Mat mask = cv::Mat::zeros(img_edges.size(), img_edges.type());
 cv::Point pts[4] = {
 cv::Point(210, 720),
 cv::Point(550, 450),
 cv::Point(717, 450),
 cv::Point(1280, 720)
 };

 // Create a binary polygon mask
 cv::fillConvexPoly(mask, pts, 4, cv::Scalar(255, 0, 0));
 // Multiply the edges image and the mask to get the output
 cv::bitwise_and(img_edges, mask, output);

 return output;
}

// HOUGH LINES
/**
*@brief Obtain all the line segments in the masked images which are going to be part of the lane boundaries
*@param img_mask is the masked binary image from the previous function
*@return Vector that contains all the detected lines in the image
*/
std::vector<cv::Vec4i> LaneDetector::houghLines(cv::Mat img_mask) {
 std::vector<cv::Vec4i> line;

 // rho and theta are selected by trial and error
 HoughLinesP(img_mask, line, 1, CV_PI / 180, 20, 20, 30);

 return line;
}

// SORT RIGHT AND LEFT LINES
/**
*@brief Sort all the detected Hough lines by slope.
*@brief The lines are classified into right or left depending
*@brief on the sign of their slope and their approximate location
*@param lines is the vector that contains all the detected lines
*@param img_edges is used for determining the image center
*@return The output is a vector(2) that contains all the classified lines
*/
std::vector<std::vector<cv::Vec4i> > LaneDetector::lineSeparation(std::vector<cv::Vec4i> lines, cv::Mat img_edges) {
 std::vector<std::vector<cv::Vec4i> > output(2);
 size_t j = 0;
 cv::Point ini;
 cv::Point fini;
 double slope_thresh = 0.3;
 std::vector<double> slopes;
 std::vector<cv::Vec4i> selected_lines;
 std::vector<cv::Vec4i> right_lines, left_lines;

 // Calculate the slope of all the detected lines
 for (auto i : lines) {
 ini = cv::Point(i[0], i[1]);
 fini = cv::Point(i[2], i[3]);

 // Basic algebra: slope = (y1 - y0)/(x1 - x0)
 double slope = (static_cast<double>(fini.y) - static_cast<double>(ini.y)) / (static_cast<double>(fini.x) - static_cast<double>(ini.x) + 0.00001);

 // If the slope is too horizontal, discard the line
 // If not, save them and their respective slope
 if (std::abs(slope) > slope_thresh) {
 slopes.push_back(slope);
 selected_lines.push_back(i);
 }
 }

 // Split the lines into right and left lines
 img_center = static_cast<double>((img_edges.cols / 2));
 while (j < selected_lines.size()) {
 ini = cv::Point(selected_lines[j][0], selected_lines[j][1]);
 fini = cv::Point(selected_lines[j][2], selected_lines[j][3]);

 // Condition to classify line as left side or right side
 if (slopes[j] > 0 && fini.x > img_center && ini.x > img_center) {
 right_lines.push_back(selected_lines[j]);
 right_flag = true;
 }
 else if (slopes[j] < 0 && fini.x < img_center && ini.x < img_center) {
 left_lines.push_back(selected_lines[j]);
 left_flag = true;
 }
 j++;
 }

 output[0] = right_lines;
 output[1] = left_lines;

 return output;
}

// REGRESSION FOR LEFT AND RIGHT LINES
/**
*@brief Regression takes all the classified line segments initial and final points and fits a new lines out of them using the method of least squares.
*@brief This is done for both sides, left and right.
*@param left_right_lines is the output of the lineSeparation function
*@param inputImage is used to select where do the lines will end
*@return output contains the initial and final points of both lane boundary lines
*/
std::vector<cv::Point> LaneDetector::regression(std::vector<std::vector<cv::Vec4i> > left_right_lines, cv::Mat inputImage) {
 std::vector<cv::Point> output(4);
 cv::Point ini;
 cv::Point fini;
 cv::Point ini2;
 cv::Point fini2;
 cv::Vec4d right_line;
 cv::Vec4d left_line;
 std::vector<cv::Point> right_pts;
 std::vector<cv::Point> left_pts;

 // If right lines are being detected, fit a line using all the init and final points of the lines
 if (right_flag == true) {
 for (auto i : left_right_lines[0]) {
 ini = cv::Point(i[0], i[1]);
 fini = cv::Point(i[2], i[3]);

 right_pts.push_back(ini);
 right_pts.push_back(fini);
 }

 if (right_pts.size() > 0) {
 // The right line is formed here
 cv::fitLine(right_pts, right_line, CV_DIST_L2, 0, 0.01, 0.01);
 right_m = right_line[1] / right_line[0];
 right_b = cv::Point(right_line[2], right_line[3]);
 }
 }

 // If left lines are being detected, fit a line using all the init and final points of the lines
 if (left_flag == true) {
 for (auto j : left_right_lines[1]) {
 ini2 = cv::Point(j[0], j[1]);
 fini2 = cv::Point(j[2], j[3]);

 left_pts.push_back(ini2);
 left_pts.push_back(fini2);
 }

 if (left_pts.size() > 0) {
 // The left line is formed here
 cv::fitLine(left_pts, left_line, CV_DIST_L2, 0, 0.01, 0.01);
 left_m = left_line[1] / left_line[0];
 left_b = cv::Point(left_line[2], left_line[3]);
 }
 }

 // One the slope and offset points have been obtained, apply the line equation to obtain the line points
 int ini_y = inputImage.rows;
 int fin_y = 470;

 double right_ini_x = ((ini_y - right_b.y) / right_m) + right_b.x;
 double right_fin_x = ((fin_y - right_b.y) / right_m) + right_b.x;

 double left_ini_x = ((ini_y - left_b.y) / left_m) + left_b.x;
 double left_fin_x = ((fin_y - left_b.y) / left_m) + left_b.x;

 output[0] = cv::Point(right_ini_x, ini_y);
 output[1] = cv::Point(right_fin_x, fin_y);
 output[2] = cv::Point(left_ini_x, ini_y);
 output[3] = cv::Point(left_fin_x, fin_y);

 return output;
}

// TURN PREDICTION
/**
*@brief Predict if the lane is turning left, right or if it is going straight
*@brief It is done by seeing where the vanishing point is with respect to the center of the image
*@return String that says if there is left or right turn or if the road is straight
*/
std::string LaneDetector::predictTurn() {
 std::string output;
 double vanish_x;
 double thr_vp = 10;

 // The vanishing point is the point where both lane boundary lines intersect
 vanish_x = static_cast<double>(((right_m*right_b.x) - (left_m*left_b.x) - right_b.y + left_b.y) / (right_m - left_m));

 // The vanishing points location determines where is the road turning
 if (vanish_x < (img_center - thr_vp))
 output = "Left Turn";
 else if (vanish_x >(img_center + thr_vp))
 output = "Right Turn";
 else if (vanish_x >= (img_center - thr_vp) && vanish_x <= (img_center + thr_vp))
 output = "Straight";

 return output;
}

// PLOT RESULTS
/**
*@brief This function plots both sides of the lane, the turn prediction message and a transparent polygon that covers the area inside the lane boundaries
*@param inputImage is the original captured frame
*@param lane is the vector containing the information of both lines
*@param turn is the output string containing the turn information
*@return The function returns a 0
*/
int LaneDetector::plotLane(cv::Mat inputImage, std::vector<cv::Point> lane, std::string turn) {
 std::vector<cv::Point> poly_points;
 cv::Mat output;

 // Create the transparent polygon for a better visualization of the lane
 inputImage.copyTo(output);
 poly_points.push_back(lane[2]);
 poly_points.push_back(lane[0]);
 poly_points.push_back(lane[1]);
 poly_points.push_back(lane[3]);
 cv::fillConvexPoly(output, poly_points, cv::Scalar(0, 0, 255), CV_AA, 0);
 cv::addWeighted(output, 0.3, inputImage, 1.0 - 0.3, 0, inputImage);

 // Plot both lines of the lane boundary
 cv::line(inputImage, lane[0], lane[1], cv::Scalar(0, 255, 255), 5, CV_AA);
 cv::line(inputImage, lane[2], lane[3], cv::Scalar(0, 255, 255), 5, CV_AA);

 // Plot the turn message
 cv::putText(inputImage, turn, cv::Point(50, 90), cv::FONT_HERSHEY_COMPLEX_SMALL, 3, cvScalar(0, 255, 0), 1, CV_AA);

 // Show the final output image
 cv::namedWindow("Lane", CV_WINDOW_AUTOSIZE);
 cv::imshow("Lane", inputImage);
 return 0;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
#include <iostream>
#include <string>
#include <vector>
#include <opencv2/opencv.hpp>
#include "LaneDetector.h"
//#include "LaneDetector.cpp"

/**
*@brief Function main that runs the main algorithm of the lane detection.
*@brief It will read a video of a car in the highway and it will output the
*@brief same video but with the plotted detected lane
*@param argv[] is a string to the full path of the demo video
*@return flag_plot tells if the demo has sucessfully finished
*/
int main() {
 
 // The input argument is the location of the video
 cv::VideoCapture cap("challenge_video.mp4");
 if (!cap.isOpened())
 return -1;

 LaneDetector lanedetector; // Create the class object
 cv::Mat frame;
 cv::Mat img_denoise;
 cv::Mat img_edges;
 cv::Mat img_mask;
 cv::Mat img_lines;
 std::vector<cv::Vec4i> lines;
 std::vector<std::vector<cv::Vec4i> > left_right_lines;
 std::vector<cv::Point> lane;
 std::string turn;
 int flag_plot = -1;
 int i = 0;

 // Main algorithm starts. Iterate through every frame of the video
 while (i < 540) {
 // Capture frame
 if (!cap.read(frame))
 break;

 // Denoise the image using a Gaussian filter
 img_denoise = lanedetector.deNoise(frame);

 // Detect edges in the image
 img_edges = lanedetector.edgeDetector(img_denoise);

 // Mask the image so that we only get the ROI
 img_mask = lanedetector.mask(img_edges);

 // Obtain Hough lines in the cropped image
 lines = lanedetector.houghLines(img_mask);

 if (!lines.empty())
 {
 // Separate lines into left and right lines
 left_right_lines = lanedetector.lineSeparation(lines, img_edges);

 // Apply regression to obtain only one line for each side of the lane
 lane = lanedetector.regression(left_right_lines, frame);

 // Predict the turn by determining the vanishing point of the the lines
 turn = lanedetector.predictTurn();

 // Plot lane detection
 flag_plot = lanedetector.plotLane(frame, lane, turn);

 i += 1;
 cv::waitKey(25);
 }
 else {
 flag_plot = -1;
 }
 }
 return flag_plot;
}