#pragma region
#include <iostream>
#include <opencv4/opencv2/core/core.hpp>
#include <opencv4/opencv2/highgui.hpp>
#include <opencv4/opencv2/opencv.hpp>
#include <opencv4/opencv2/imgproc/types_c.h>
#include "pigpio.h"
#include <thread>
#include <cmath>
#include <chrono>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <assert.h>
#include <termios.h>
#include <string.h>
#include <sys/time.h>
#include <time.h>
#include <sys/types.h>
#include <errno.h>
#include <signal.h>
using namespace std;
using namespace cv;
#pragma endregion
#define BAUD 9600
#pragma region
int MAX_YU = 140;
int MIN_YU = 60;
// PID 各个数
值
float kk1 = 0, kk2 = 0, bb1 = 0, bb2 = 0, ll1 = 0, ll2 = 0;
float kr = 0.00000001, kl = -0.0000001, lr = 0, ll = 0, br, bl;
bool flag_avoid = false; // 躲避锥桶标志位
bool flag_zhang = false; // 未知
bool flag_voidcount = false; // 未知
bool flag_ren = false; // 人行道判断标志位
bool flag_yellow = false;
double limit_voidtime = 0.17;
bool flag_start; // 判断蓝色挡板标志位
bool flag_count; // 未知
double time_void = 100; // 未知
double angle; //
计算得到的角度
double angle_bi; // 避障角度
double angle_cover; //??????????
double speed_avoid = 10050; //???????
// double speed_delay = 10200;//?????????????????
double heigth;
double kuan;
double low = 0, high = 0;
Mat kernel_3 = Mat::ones
(cv::Size
(3, 3
), CV_8U
);
Mat kernel = getStructuringElement
(MORPH_RECT, Size
(1, 1
));
int shu = 0;
int sum_avoid = 0;
double width, x_r, x_l; //,heigth;
Mat frame, ca;
static int ret;
static int GPS;
char r_buf[1024];
FILE* fp;
double angle_x = 0; //???????y??
struct itimerval timer;
float a[3], w[3], Angle[3], h[3];
int flag_picture,duo_flag;
// Rect list;
double speed_mid = 10000; // 10800;//10600;//10200;//10400;//10450;//10600;
double speed_init = 9000; // 10800
double last_error = 0; //寻迹参数
double kp = 0.25; // 0.155;//0.11?????;//0.16;//0.2;//0.3;//0.1???? 0.3????
double kd = 0.08; // 0.1;//0 ?????
double min_angle = 30; // 80 90 - 10
double max_angle = 20; // 100
double last_error1 = 0; //避障参数
double kp1 = 0.11; // 0.18;//0.12;//0.08;//0.;//0.12;
double kd1 = 0.08; // 0.05;
double minavoid_angle = 10; // 50 = 90 - 40
double maxavoid_angle = 10; // 110 = 90 + 20
// int flagl = 0, flagr = 0;
double last_error2 = 0;
double kp2 = 0.16; // 0.155;//0.155;//0.18;//0.12;//0.08;//0.;//0.12;
double kd2 = 0.1; // 0.2;//0.05;
double minhui_angle = 30; // 50 = 90 - 40
double maxhui_angle = 20; // 110 = 90 + 20
clock_t start_, end_, mid_time;
static void on
(int, void*
);
double picture
();
void Set_duo
(int angle
);
void GetROI
(Mat src, Mat& ROI
);
void Set_dian
();
double PID
(double error1
);
vec
tor<Point2f> get_lines_fangcheng
(vec
tor<Vec4i> lines
);
void Set_gpio
();
Rect Obstacles
(Mat img
);
int car_start
(Mat img
); // 车辆启动
int s
top
(Mat img
);
Rect blue
(Mat img
); // 识别蓝色挡板
Rect yellow
(Mat img
); // 识别黄色挡板
int uart_open
(int fd, const char* pathname
); // 打开串口
int uart_set
(int fd, int nSpeed, int nBits, char nEvent, int nS
top
); // 串口设置
int uart_close
(int fd
); // 串口关闭
int send_data
(int fd, char* send_buffer, int length
); // 发送数据?未启用
int recv_data
(int fd, char* recv_buffer, int length
); // 没用
void ParseData
(char chr
);
void Init
();
void Data
(int signal
);
bool crossing
(Mat image
); // 识别人行横道
void Control
(int flag
);
void Control_avoid
(); // 避障
void Control_cover
();
void Control_s
top
(); // 停止
void Control_Xun
(double error
);
double Bi_Control
(double len
); // 锥桶避障
void Control_avoid
();
void Control_Hui
(double error1
);
string str = "sudo cp /home/pi/.Xauthority /root/";
int flag = system
(str.c_str
());
Mat img,img_combined,img_HSV_mask,img_per;
int start_flag = 1,
find_blue_card_flag = 0,cross_flag=0,music_flag = 1,avoid_flag=0,car_blake_flag=0;
int LAB_Bmin = 190, LAB_Bmax = 255, HSL_Lmin=220,HSL_Lmax=255;
#pragma endregion
void cd
(char* path
)
{
chdir
(path
);
}
bool crossing
(Mat image
)
{
Mat roi, labels, stats, centroids, hui;
GetROI
(image, roi
);
//imshow
("roi", roi
);
cvtColor
(roi, hui, COLOR_BGR2GRAY
);
Mat binaryImage;
threshold
(hui, binaryImage, 180, 255, cv::THRESH_BINARY
);
imshow
("binaryImage", binaryImage
);
int numComponents = cv::connectedComponentsWithStats
(binaryImage, labels, stats, centroids
);
int crossingnum = 0;
for
(int i = 1; i < numComponents; i++
)
{
int
area = stats.at<int>
(i, cv::CC_STAT_
AREA);
if
(area > 200
)
{
crossingnum++;
}
// cout<<
area<<endl;
}
cout << "CrossingNum:" << crossingnum << endl;
if
(crossingnum >= 4
)
{
cout << "????|l|l|" << endl;
return true;
}
return false;
}
void gpio_init
()
{
if
(gpioInitialise
() < 0
)
exit
(1
);
gpioSetMode
(13, PI_OUTPUT
);
gpioSetPWMrange
(13, 40000
);
gpioSetPWMfrequency
(13, 200
);
if
(gpioInitialise
() < 0
)
exit
(1
);
gpioSetMode
(22, PI_OUTPUT
);
gpioSetPWMfrequency
(22, 50
);
gpioSetPWMrange
(22, 1000
);
gpioPWM
(22, 75
); //大左小右
if
(gpioInitialise
() < 0
)
exit
(1
);
gpioSetMode
(23, PI_OUTPUT
);
gpioSetPWMfrequency
(23, 50
);
gpioSetPWMrange
(23, 1000
);
gpioPWM
(23, 70
); //大上下小
if
(gpioInitialise
() < 0
)
exit
(1
);
gpioSetMode
(12, PI_OUTPUT
); // 设置GPIO12为PWM输出
gpioSetPWMfrequency
(12, 50
); // 50Hz标准频率
gpioSetPWMrange
(12, 30000
); // 设置PWM范围
printf
("GPIO initial successful"
);
}
void steering
(int angle
)
{
double value =
(0.5 +
(2.0 / 180.0
) * angle
) / 20 * 30000; // 100;//20000
cout << "设置舵机" << endl;
gpioPWM
(12, value
);
}
void Start_mo
tor
()
{
cout << "1" << endl;
gpioPWM
(13, 12000
);
gpioDelay
(1000000
);
for
(int x=12000;x>=9000;x-=300
)
{
cout << x << endl;
gpioPWM
(13, x
);
gpioDelay
(500000
);
}
// 第二次启动时,可以注释掉 下面部分
/*cout << "2" << endl;
gpioPWM
(13, 12000
);
gpioDelay
(1000000
);
// 到此为止
cout << "3" << endl;
gpioPWM
(13, 11000
);
gpioDelay
(1000000
);
cout << "4" << endl;
gpioPWM
(13, 10300
);
double value = speed_init; // ???动慢???
gpioPWM
(13, value
);*/
}
void Control_s
top
()
{
gpioPWM
(13, 12600
); // 刹车
gpioDelay
(100000
);
gpioPWM
(13, 12200
); // 保护
gpioDelay
(3000000
);
//gpioPWM
(13, speed_mid
); // 冲刺
}
int uart_open
(int fd, const char* pathname
)
{
fd = open
(pathname, O_RDWR | O_NOCTTY
);
if
(-1 == fd
)
{
perror
("Can't Open Serial Port"
);
return
(-1
);
}
else
printf
("open %s success!\n", pathname
);
if
(isatty
(STDIN_FILENO
) == 0
)
printf
("standard input is not a terminal device\n"
);
else
printf
("isatty success!\n"
);
return fd;
}
int uart_set
(int fd, int nSpeed, int nBits, char nEvent, int nS
top
)
{
struct termios newtio, oldtio;
if
(tcgetattr
(fd, &oldtio
) != 0
) {
perror
("SetupSerial 1"
);
printf
("tcgetattr
( fd,&oldtio
) -> %d\n", tcgetattr
(fd, &oldtio
));
return -1;
}
bzero
(&newtio, size
of(newtio
));
newtio.c_cflag |= CLOCAL | CREAD;
newtio.c_cflag &= ~CSIZE;
switch
(nBits
)
{
case 7:
newtio.c_cflag |= CS7;
break;
case 8:
newtio.c_cflag |= CS8;
break;
}
switch
(nEvent
)
{
case 'o':
case 'O':
newtio.c_cflag |= PARENB;
newtio.c_cflag |= PARODD;
newtio.c_iflag |=
(INPCK | ISTRIP
);
break;
case 'e':
case 'E':
newtio.c_iflag |=
(INPCK | ISTRIP
);
newtio.c_cflag |= PARENB;
newtio.c_cflag &= ~PARODD;
break;
case 'n':
case 'N':
newtio.c_cflag &= ~PARENB;
break;
default:
break;
}
/*设置波特率*/
switch
(nSpeed
)
{
case 2400:
cfsetispeed
(&newtio, B2400
);
cfse
tospeed
(&newtio, B2400
);
break;
case 4800:
cfsetispeed
(&newtio, B4800
);
cfse
tospeed
(&newtio, B4800
);
break;
case 9600:
cfsetispeed
(&newtio, B9600
);
cfse
tospeed
(&newtio, B9600
);
break;
case 115200:
cfsetispeed
(&newtio, B115200
);
cfse
tospeed
(&newtio, B115200
);
break;
case 460800:
cfsetispeed
(&newtio, B460800
);
cfse
tospeed
(&newtio, B460800
);
break;
default:
cfsetispeed
(&newtio, B9600
);
cfse
tospeed
(&newtio, B9600
);
break;
}
if
(nS
top == 1
)
newtio.c_cflag &= ~CS
TOPB;
else if
(nS
top == 2
)
newtio.c_cflag |= CS
TOPB;
newtio.c_cc[VTIME] = 0;
newtio.c_cc[VMIN] = 0;
tcflush
(fd, TCIFLUSH
);
if
((tcsetattr
(fd, TCSANOW, &newtio
)) != 0
)
{
perror
("com set error"
);
return -1;
}
printf
("set done!\n"
);
return 0;
}
int uart_close
(int fd
)
{
assert
(fd
);
close
(fd
);
return 0;
}
int send_data
(int fd, char* send_buffer, int length
)
{
length = write
(fd, send_buffer, length * size
of(unsigned char
));
return length;
}
int recv_data
(int fd, char* recv_buffer, int length
)
{
length = read
(fd, recv_buffer, length
);
return length;
}
void ParseData
(char chr
)
{
static char chrBuf[100];
static unsigned char chrCnt = 0;
signed short sData[4];
unsigned char i;
time_t now;
chrBuf[chrCnt++] = chr;
if
(chrCnt < 11
) return;
if
((chrBuf[0] != 0x55
) ||
((chrBuf[1] & 0x50
) != 0x50
)) { printf
("Error:%x %x\r\n", chrBuf[0], chrBuf[1]
); memcpy
(&chrBuf[0], &chrBuf[1], 10
); chrCnt--; return; }
memcpy
(&sData[0], &chrBuf[2], 8
);
switch
(chrBuf[1]
)
{
case 0x51:
for
(i = 0; i < 3; i++
) a[i] =
(float
)sData[i] / 32768.0 * 16.0;
time
(&now
);
//printf
("\r\nT:%s a:%6.3f %6.3f %6.3f ", asctime
(localtime
(&now
)), a[0], a[1], a[2]
);
break;
case 0x52:
for
(i = 0; i < 3; i++
) w[i] =
(float
)sData[i] / 32768.0 * 2000.0;
//printf
("w:%7.3f %7.3f %7.3f ", w[0], w[1], w[2]
);
break;
case 0x53:
for
(i = 0; i < 3; i++
) Angle[i] =
(float
)sData[i] / 32768.0 * 180.0;
//printf
("A:%7.3f %7.3f %7.3f ", Angle[0], Angle[1], Angle[2]
);
printf
("Z:%7.3f \r\n",Angle[2]
);
break;
case 0x54:
for
(i = 0; i < 3; i++
) h[i] =
(float
)sData[i];
//printf
("h:%4.0f %4.0f %4.0f ", h[0], h[1], h[2]
);
break;
}
chrCnt = 0;
}
void Control_Xun
(double error1
)
{
double angle = kp * error1 + kd *
(error1 - last_error
);
last_error = error1;
angle = 100 - angle;
if
(angle > 100 + max_angle
) // 130
angle = 100 + max_angle;
if
(angle < 100 - min_angle
)
angle = 100 - min_angle;
cout << "angle:" << angle << endl;
cout << "循迹处理完毕" << endl;
steering
(angle
);
}
void Control_avoid
(double error1
)
{
double angle = kp1 * error1 + kd1 *
(error1 - last_error
); // PID 公式
last_error1 = error1;
angle = 90 - angle;
if
(angle > 90 + max_angle
) // 130
angle = 90 + max_angle;
if
(angle < 90 - min_angle
)
angle = 90 - min_angle;
cout << "avoid_angle:" << angle << endl;
steering
(angle
);
}
// opencv 截取ROI区域
void GetROI
(Mat src, Mat& ROI
)
{
// 取得源图像的长和宽
int width = src.cols;
int height = src.rows;
Rect rect
(Point
(0,
(height / 20
) * 10
), Point
(width,
(height / 20
) * 17
)); // 按比例截取
ROI = src
(rect
); // 赋
值ROI
}
double picture
()
{
Mat roi, hui, gao, binaryImage;
GetROI
(frame, roi
); // 截取ROI区域
cvtColor
(roi, hui, COLOR_BGR2GRAY
); // 转换为灰度图
GaussianBlur
(hui, gao, Size
(5, 5
), 0.5, 0.5
); // 高斯滤波
// morphologyEx
(gao, gao, MORPH_OPEN, kernel_3
); // 开运算
// threshold
(gao, binaryImage, 150, 255, cv::THRESH_BINARY
); // 二
值化
Canny
(gao, ca, MIN_YU, MAX_YU, 3
); // Canny边缘检测
imshow
("canny", ca
);
// imshow
("binaryImage", binaryImage
);
int
area = countNonZero
(ca
); //
计算非零像素点个数,即边缘点个数
// 根据边缘点个数调整MIN_YU 和 MAX_YU
// 动态调整边缘检测的阈
值
if
(area > 2500
)
{
MIN_YU += 2;
MAX_YU += 4;
}
if
(area < 2000
)
{
MIN_YU -= 2;
MAX_YU -= 4;
}
float len;
vec
tor<Vec4i> plines; // 存储霍夫变换检测到的直线
vec
tor<Point2f> a; // 存储直线的斜率和截距
HoughLinesP
(ca, plines, 1, 0.05, 50, 30, 5
); // 70 50 50 概率霍夫变换
a = get_lines_fangcheng
(plines
); // 获取直线的斜率和截距
int i = 0;
kr = 0.00000001, kl = -0.0000001, lr = 0, ll = 0, br, bl; // 初始化
int sum = 0;
// 遍历每一条直线
for
(i = 0; i < plines.size
(); i++
)
{
/*
首先，使用HoughLinesP函数对输入图像ca进行霍夫变换，检测出图像中的直线，并将这些直线的参数存储在plines向量中。然后，通过get_lines_fangcheng函数
计算每条直线的斜率和截距，存储在向量a中。
接下来，代码遍历每一条直线，通过斜率判断直线的位置。如果斜率在0.25到2之间，或者在-0.25到-2之间，那么这条直线被认为是有效的。对于斜率大于0的直线，代码会更新右侧直线的斜率kr和截距br；对于斜率小于0的直线，代码会更新左侧直线的斜率kl和截距bl
*/
// 获取直线的斜率和截距
float x1 = a[i].x;
float x2 = a[i].y + frame.rows / 2;
// 通过斜率判断直线的位置
if
((x1 > 0.25 && x1 < 2
) ||
(x1 < -0.25 && x1 > -2
))
{
if
(x1 > 0
)
{
lr++;
// cout<<"??"<<x1<<endl;
// 更新右侧直线的斜率和截距
if
(x1 > kr
)
{
kr = x1;
br = x2;
}
}
else if
(x1 < 0
)
{ // 2?? 110
ll++;
// cout<<"??"<<x1<<endl;
// 更新左侧直线的斜率和截距
if
(x1 < kl
)
{
kl = x1;
bl = x2;
}
}
}
}
float ave_x = 0;
int flagl = 0, flagr = 0;
flagl = 0, flagr = 0;
// 检查左侧和右侧是否都有有效的直线
if
(lr == 0
)
{
// ??? 0
flagr = 1;
}
if
(ll == 0
)
{
// ??? 0
flagl = 1;
}
double error;
end_ = clock
();
mid_time = double
(end_ - start_
) / CLOCKS_PER_SEC;
cout << "time" << mid_time << endl;
if
(flag_ren == true && mid_time < 20
) // 如果flag_ren为真且运行时间小于20秒
{
// 扫线
for
(i = 130; i < 230; i++
)
{
int l, r;
if
(flagl
)
l = 0;
else
l =
(i - bl
) / kl; // 设置直线参数
if
(flagr
)
r = frame.cols; // 左右
值
else
r =
(i - br
) / kr;
//
求中点
double mid =
(l + r
) / 2;
double mid_mid =
(r + mid
) / 2;
ave_x += mid_mid; //
计算平均
值
Point pa
(r, i
);
Point pb
(l, i
);
Point p
((l + r
) / 2, i
);
circle(frame, pa, 4, Scalar
(55, 25, 0
));
circle(frame, pb, 4, Scalar
(55, 25, 0
));
circle(frame, p, 1, Scalar
(255, 255, 255
));
}
ave_x = ave_x / 100; //
计算平均
值
error = ave_x - frame.cols / 2 - 0; //
计算误差
(PID关键参数
)
Control_avoid
(error
);
}
else
{
for
(i = 130; i < 230; i++
)
{
int l, r;
if
(flagl
)
l = 0;
else
l =
(i - bl
) / kl;
if
(flagr
)
r = frame.cols;
else
r =
(i - br
) / kr;
// ????е????????????
double mid =
(l + r
) / 2;
ave_x += mid;
Point pa
(r, i
);
Point pb
(l, i
);
Point p
((l + r
) / 2, i
);
circle(frame, pa, 4, Scalar
(55, 25, 0
));
circle(frame, pb, 4, Scalar
(55, 25, 0
));
circle(frame, p, 1, Scalar
(255, 255, 255
));
}
ave_x = ave_x / 100;
error = ave_x - frame.cols / 2;
gpioPWM
(13, 10600
);
Control_Xun
(error
);
}
imshow
("????", frame
);
cout << "???:" << ave_x;
return error;
}
vec
tor<Point2f> get_lines_fangcheng
(vec
tor<Vec4i> lines
)
{
// 遍历概率霍夫变换检测到的直线
float k = 0; // 斜率
float b = 0; // 截距
vec
tor<Point2f> lines_fangcheng;
for
(unsigned int i = 0; i < lines.size
(); i++
)
{
k =
(double
)(lines[i][3] - lines[i][1]
) /
(double
)(lines[i][2] - lines[i][0]
);
b =
(double
)lines[i][1] - k *
(double
)lines[i][0];
lines_fangcheng.push_back
(Point2f
(k, b
));
}
return lines_fangcheng;
}
Rect blue
(Mat img
)
{
Mat HSV, roi;
GetROI
(img, roi
);
cvtColor
(roi, HSV, COLOR_BGR2HSV
);
Scalar Lower
(90, 50, 50
);
Scalar Upper
(130, 255, 255
);
Mat mask;
inRange
(HSV, Lower, Upper, mask
);
Mat erosion_dilation;
erode
(mask, erosion_dilation, kernel_3, Point
(-1, -1
), 1, BORDER_CONSTANT, morphologyDefaultBorderValue
());
dilate
(erosion_dilation, erosion_dilation, kernel_3, Point
(-1, -1
), 1, BORDER_CONSTANT, morphologyDefaultBorderValue
());
Mat target;
bitwise_and
(roi, roi, target, erosion_dilation
);
Mat binary;
threshold
(erosion_dilation, binary, 127, 255, THRESH_BINARY
);
imshow
("blue", binary
);
vec
tor<vec
tor<Point>> con
tours;
findCon
tours
(binary, con
tours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE
);
int maxCon
tourIndex = -1;
double maxCon
tour
Area = 0.0;
for
(int i = 0; i < con
tours.size
(); i++
)
{
double
area = cv::con
tour
Area(con
tours[i]
);
if
(area > maxCon
tour
Area)
{
maxCon
tour
Area =
area;
maxCon
tourIndex = i;
}
}
if
(maxCon
tourIndex != -1
)
{
return boundingRect
(con
tours[maxCon
tourIndex]
);
}
else
{
return Rect
();
}
}
int car_start
(Mat img
)
{
Rect start_flag = blue
(img
);
// cout << start_flag.width << " " << start_flag.height << endl;
if
(start_flag.width > 50 && start_flag.height > 50
)
{
return 0; // ??????????
}
return 1; // ????????I????
}
void camera
(int angl_x,int angl_y
)
{
gpioPWM
(22, angl_x
); //大左小右
gpioPWM
(23, angl_y
); //大上下小
}
void GPS_init
()
{
char r_buf[1024];
bzero
(r_buf, 1024
);
GPS = uart_open
(GPS, "/dev/ttyUSB0"
);/*串口号/dev/ttySn,USB口号/dev/ttyUSBn */
if
(GPS == -1
)
{
fprintf
(stderr, "GPS error\n"
);
exit
(EXIT_FAILURE
);
}
if
(uart_set
(GPS, BAUD, 8, 'N', 1
) == -1
)
{
fprintf
(stderr, "GPS set failed!\n"
);
exit
(EXIT_FAILURE
);
}
}
void Get_GPS_data
()
{
ret = recv_data
(GPS, r_buf, 44
);
if
(ret == -1
)
{
fprintf
(stderr, "uart read failed!\n"
);
exit
(EXIT_FAILURE
);
}
for
(int i = 0; i < ret; i++
) ParseData
(r_buf[i]
);
//usleep
(1000
);
}
void audio
()
{
system
("omxplayer /home/pi/car435/car.wav"
);
}
Mat per_wt
(Mat &frame
)
{
Mat img_clone,warped;
int img_h,img_w;
img_clone = frame.clone
();
img_h = img_clone.rows; // 图像高度
img_w = img_clone.cols;
vec
tor<cv::Point2f> src = {
Point2f
(122, 146
),
Point2f
(210, 146
),
Point2f
(319, 170
),
Point2f
(75, 170
)
};
vec
tor<cv::Point2f> dst = {
Point2f
(100,0
),
Point2f
(img_w - 100, 0
),
Point2f
(img_w - 100, img_h
),
Point2f
(100, img_h
)
};
// 定义目标坐标（变换后的四个点）
vec
tor<cv::Point> polygon;
for
(const au
to& pt : src
)
{
polygon.push_back
(Point
(static_cast<int>
(pt.x
), static_cast<int>
(pt.y
)));
}
polylines
(img_clone, polygon, 1, Scalar
(0, 0, 255
), 3, cv::LINE_AA
);
//for
(size_t i = 0; i < src.size
(); ++i
)
//{
//
circle(img, cv::Point
(static_cast<int>
(src[i].x
), static_cast<int>
(src[i].y
)), 5, cv::Scalar
(0, 0, 255
), -1
);
//}
//
计算透视变换矩阵
Mat M = getPerspectiveTransform
(src, dst
);
Mat Minv = getPerspectiveTransform
(dst, src
);
// 应用透视
warpPerspective
(frame, warped, M, cv::Size
(img_w, img_h
), cv::INTER_LINEAR
);
//cvtColor
(warped, warped, COLOR_BGR2GRAY
);
//GaussianBlur
(warped, warped, Size
(11,11
), 0
);
//threshold
(warped, warped, 0, 255, THRESH_BINARY + THRESH_OTSU
);
namedWindow
("img_clone", cv::WINDOW_NORMAL
); // 创建可调整大小的窗口
resizeWindow
("img_clone", 320, 240
); // 设置窗口尺寸
moveWindow
("img_clone", 320, 50
); // 设置窗口位置
imshow
("img_clone", img_clone
);
//namedWindow
("warped_image", cv::WINDOW_NORMAL
); // 创建可调整大小的窗口
//resizeWindow
("warped_image", 320, 240
); // 设置窗口尺寸
//moveWindow
("warped_image", 640, 50
); // 设置窗口位置
//imshow
("warped_image", warped
);
waitKey
(5
);
return warped;
}
Mat HLS_Lthresh
(const cv::Mat& img, int min_thresh = 220, int max_thresh = 255
)
{
Mat hls;
cvtColor
(img, hls, COLOR_BGR2HLS
);
vec
tor<Mat> channels;
split
(hls, channels
);
Mat l_channel = channels[1];
double minVal, maxVal;
minMaxLoc
(l_channel, &minVal, &maxVal
);
l_channel.convert
To(l_channel, CV_8UC1, 255.0/maxVal
);
Mat binary_output = Mat::zeros
(l_channel.size
(), CV_8UC1
);
inRange
(l_channel, min_thresh, max_thresh, binary_output
);
return binary_output;
}
Mat LAB_BThreshold
(const Mat& img, int min_thresh = 190, int max_thresh = 255
)
{
Mat lab;
cvtColor
(img, lab, COLOR_BGR2Lab
);
vec
tor<Mat> lab_channels;
split
(lab, lab_channels
);
Mat b_channel = lab_channels[2];
//cout<<b_channel<<endl;
double maxVal;
minMaxLoc
(b_channel, nullptr, &maxVal
);
cout<<maxVal<<endl;
if
(maxVal > 175
) {
b_channel.convert
To(b_channel, CV_8UC1, 255.0/maxVal
);
}
Mat binary_output = Mat::zeros
(b_channel.size
(), CV_8UC1
);
inRange
(b_channel, min_thresh, max_thresh, binary_output
);
return binary_output;
}
Mat combineThresholds
(const Mat& img_LThresh, const Mat& img_BThresh
)
{
Mat combined = Mat::zeros
(img_BThresh.size
(), CV_8UC1
);
bitwise_or
(img_LThresh, img_BThresh, combined
);
return combined;
}
struct LaneData {
vec
tor<Point> left_points;
vec
tor<Point> right_points;
vec
tor<float> left_fit;
vec
tor<float> right_fit;
vec
tor<Rect> left_windows;
vec
tor<Rect> right_windows;
};
Mat getHis
togram
(const Mat& binary_img
) {
Mat hist;
reduce
(binary_img.rowRange
(binary_img.rows/2, binary_img.rows
),
hist, 0, REDUCE_SUM, CV_32S
);
return hist;
}
pair<int, int>
findLaneBases
(const Mat& hist
) {
int midpoint = hist.cols / 2;
int quarter = midpoint / 2;
Point left_base, right_base;
minMaxLoc
(hist.colRange
(quarter, midpoint
), 0, 0, 0, &left_base
);
minMaxLoc
(hist.colRange
(midpoint, midpoint+quarter
), 0, 0, 0, &right_base
);
return {left_base.x + quarter, right_base.x + midpoint};
}
LaneData slidingWindow
(const Mat& binary_img, int nwindows=9, int margin=100, int minpix=50
)
{
LaneData result;
au
to [leftx, rightx] =
findLaneBases
(getHis
togram
(binary_img
));
//cout<<"leftx:"<<leftx<<"rightx:"<<rightx<<endl;
int window_height = binary_img.rows / nwindows;
vec
tor<Point> nonzero;
findNonZero
(binary_img, nonzero
);
for
(int i = 0; i < nwindows; ++i
) {
int win_y_low = binary_img.rows -
(i+1
)*window_height;
int win_y_high = binary_img.rows - i*window_height;
Rect left_win
(leftx - margin, win_y_low, 2*margin, window_height
);
Rect right_win
(rightx - margin, win_y_low, 2*margin, window_height
);
result.left_windows.push_back
(left_win
);
result.right_windows.push_back
(right_win
);
for
(const au
to& pt : nonzero
) {
if
(left_win.contains
(pt
)) result.left_points.push_back
(pt
);
if
(right_win.contains
(pt
)) result.right_points.push_back
(pt
);
}
if
(result.left_points.size
() > minpix
)
leftx = mean
(result.left_points
)[0];
if
(result.right_points.size
() > minpix
)
rightx = mean
(result.right_points
)[0];
}
return result;
}
vec
tor<float> polyFit
(const vec
tor<Point>& points
)
{
if
(points.empty
()) return {};
Mat A
(points.size
(), 3, CV_32F
);
Mat B
(points.size
(), 1, CV_32F
);
for
(size_t i = 0; i < points.size
(); ++i
) {
float y = points[i].y;
A.at<float>
(i,0
) = y*y;
A.at<float>
(i,1
) = y;
A.at<float>
(i,2
) = 1.0f;
B.at<float>
(i,0
) = points[i].x;
}
Mat coeff;
solve
(A, B, coeff, DECOMP_QR
);
return {coeff.at<float>
(0
), coeff.at<float>
(1
), coeff.at<float>
(2
)};
}
void visualize
(Mat &img, const LaneData& data
) {
// 绘制滑动窗口
for
(const au
to& win : data.left_windows
)
rectangle
(img, win, Scalar
(0,255,0
), 2
);
for
(const au
to& win : data.right_windows
)
rectangle
(img, win, Scalar
(0,255,0
), 2
);
// 绘制车道线点
for
(const au
to& pt : data.left_points
)
circle(img, pt, 3, Scalar
(255,0,0
), -1
);
for
(const au
to& pt : data.right_points
)
circle(img, pt, 3, Scalar
(0,0,255
), -1
);
// 绘制拟合曲线
if
(!data.left_fit.empty
())
{
for
(int y = 0; y < img.rows; ++y
)
{
int x = data.left_fit[0]*y*y + data.left_fit[1]*y + data.left_fit[2];
if
(x >= 0 && x < img.cols
)
circle(img, Point
(x,y
), 2, Scalar
(0,0,255
), -1
);
}
}
if
(!data.right_fit.empty
())
{
for
(int y = 0; y < img.rows; ++y
)
{
int x = data.right_fit[0]*y*y + data.right_fit[1]*y + data.right_fit[2];
if
(x >= 0 && x < img.cols
)
circle(img, Point
(x,y
), 2, Scalar
(0,0,255
), -1
);
}
}
}
void crossroad
(Mat &frame
)//斑马线识别
{
cross_flag = 0;
Mat blur;
GaussianBlur
(frame, blur, Size
(5, 5
), 0
);
Mat hsv;
cvtColor
(blur, hsv, COLOR_BGR2HSV
);
Scalar lower_white = Scalar
(0, 0, 200
);
Scalar upper_white = Scalar
(180, 40, 255
);
Mat cross_mask;
inRange
(hsv, lower_white, upper_white, cross_mask
);
// Mat edges;
// Canny
(blur, edges, 50, 150
);
// bitwise_or
(cross_mask, edges, cross_mask
);
Mat kernel = getStructuringElement
(MORPH_RECT, Size
(7, 7
));
//dilate
(mask1, mask1, kernel
);
//erode
(mask1, mask1, kernel
);
morphologyEx
(cross_mask, cross_mask, MORPH_CLOSE, kernel
);
Rect roi
(0, 240, 640, 240
);
Mat region = cross_mask
(roi
);
imshow
("combined_image", region
);
//imshow
("combined_image", mask1_roi
);
int stripe_count = 0,transitions = 0;
for
(int i = 0; i < region.rows; i++
)
{
transitions = 0;
for
(int j = 5; j < region.cols-5; j++
) {
if
(region.at<uchar>
(i,j
) != region.at<uchar>
(i,j-1
))
transitions++;
}
//cout<<"transitions:"<<transitions<<endl;
if
(transitions >= 6
) stripe_count++;
}
cross_flag=
(stripe_count >= 20
) ? 1 : 0;
cout<<"cross_flag:"<<cross_flag<<endl;
}
bool Con
tour_
Area(vec
tor<Point> con
tour1, vec
tor<Point> con
tour2
)
{
return con
tour
Area(con
tour1
) > con
tour
Area(con
tour2
);
}
void
find_blue_card
(void
)//检测到蓝色挡板
{
vec
tor<vec
tor<Point>> con
tours;
vec
tor<Vec4i> hierarcy;
findCon
tours
(img_HSV_mask, con
tours, hierarcy, RETR_EXTERNAL, CHAIN_APPROX_NONE
);
if
(con
tours.size
() > 0
)
{
sort
(con
tours.begin
(), con
tours.end
(), Con
tour_
Area);
//cout<<con
tours.size
()<<endl;
vec
tor<vec
tor<Point>> newCon
tours;
for
(const vec
tor<Point> &con
tour : con
tours
)
{
Point2f center;
float radius;
minEnclosing
Circle(con
tour, center, radius
);
if
(center.y > 90 && center.y < 160
)
{
newCon
tours.push_back
(con
tour
);
}
}
con
tours = newCon
tours;
if
(con
tours.size
() > 0
)
{
if
(con
tour
Area(con
tours[0]
) > 500
)
{
cout << "
find biggest blue" << endl;
Point2f center;
float radius;
minEnclosing
Circle(con
tours[0], center, radius
);
circle(img, center, static_cast<int>
(radius
), Scalar
(0, 255, 0
), 2
);
find_blue_card_flag = 1;
}
else
{
cout << "not blue card" << endl;
}
}
}
else
{
cout << "not blue card" << endl;
}
}
void
find_blue_card_remove
(void
)//蓝色挡板移开
{
cout << "detecting blue card remove" << endl;
vec
tor<vec
tor<Point>> con
tours;
vec
tor<Vec4i> hierarcy;
findCon
tours
(img_HSV_mask, con
tours, hierarcy, RETR_EXTERNAL, CHAIN_APPROX_NONE
);
if
(con
tours.size
() > 0
)
{
sort
(con
tours.begin
(), con
tours.end
(), Con
tour_
Area);
vec
tor<vec
tor<Point>> newCon
tours;
for
(const vec
tor<Point> &con
tour : con
tours
)
{
Point2f center;
float radius;
minEnclosing
Circle(con
tour, center, radius
);
if
(center.y > 90 && center.y < 160
)
{
newCon
tours.push_back
(con
tour
);
}
}
con
tours = newCon
tours;
if
(con
tours.size
() == 0
)
{
start_flag = 0;
cout << "blue card remove" << endl;
sleep
(2
);
}
}
else
{
start_flag = 0;
cout << "blue card remove" << endl;
sleep
(2
);
}
}
Mat cus
tomEqualizeHist
(const Mat &inputImage, float alpha
)
{
Mat enhancedImage;
equalizeHist
(inputImage, enhancedImage
);
return alpha * enhancedImage +
(1 - alpha
) * inputImage;
}
int main
()
{
//GPS_init
();
gpio_init
(); // 打开GPIO口
steering
(90
);//大左小右
camera
(72,78
);
VideoCapture capture;
capture.open
(0
);//云台摄像头
if
(!capture.isOpened
())
{
cout << "Can not open camera" << endl;
return -1;
}
else
cout << "camera open successful" << endl;
capture.set
(CAP_PROP_FOURCC,VideoWriter::fourcc
('M','J','P','G'
));
capture.set
(CAP_PROP_FPS, 30
);
capture.set
(CAP_PROP_FRAME_WIDTH, 320
);// opencv设置摄像头参数
capture.set
(CAP_PROP_FRAME_HEIGHT,240
);//opencv设置摄像头参数
this_thread::sleep_for
(chrono::milliseconds
(1000
)); //让图像稳定
cout << "
Program started" <<endl;
while
(1
)
{
capture>>img;
//imshow
("img", img
);
img_per=per_wt
(img
);
Mat HLS_Lresult = HLS_Lthresh
(img_per,HSL_Lmin,HSL_Lmax
);
Mat LAB_Bresult = LAB_BThreshold
(img_per,LAB_Bmin,LAB_Bmax
);
img_combined=combineThresholds
(HLS_Lresult, LAB_Bresult
);
Mat white_image
(img.size
(), img.type
(), Scalar::all
(255
));
LaneData lanes = slidingWindow
(img_combined
);
lanes.left_fit = polyFit
(lanes.left_points
);
lanes.right_fit = polyFit
(lanes.right_points
);
//visualize
(white_image, lanes
);
//namedWindow
("img_combined", cv::WINDOW_NORMAL
); // 创建可调整大小的窗口
//resizeWindow
("img_combined", 320, 240
); // 设置窗口尺寸
//moveWindow
("img_combined", 0, 450
); // 设置窗口位置
//imshow
("img_combined", white_image
);
namedWindow
("combined_image", cv::WINDOW_NORMAL
); // 创建可调整大小的窗口
resizeWindow
("combined_image", 320, 240
); // 设置窗口尺寸
moveWindow
("combined_image", 960, 50
); // 设置窗口位置
imshow
("combined_image", img_combined
);
if
(waitKey
(1
) == 27
)break;
}
gpioTerminate
();
capture.release
(); // 释放摄像头
destroyAllWindows
();
uart_close
(GPS
);
return 0;
}
扫一扫
9588
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
