#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 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??
float a[3], w[3], Angle[3], h[3];
int flag_picture, duo_flag;
// Rect list;cd ~
clock_t start_, end_, mid_time;
static void on(int, void*);
//double picture();
void GetROI(Mat src, Mat& ROI);
double PID(double error1);
vector<Point2f> get_lines_fangcheng(vector<Vec4i> lines);
void Set_gpio();
Rect Obstacles(Mat img);
Rect blue(Mat img); // 识别蓝色挡板
Rect yellow(Mat img); // 识别黄色挡板
string str = "sudo cp /home/pi/.Xauthority /root/";
int flag = system(str.c_str());
#pragma endregion
//5G
Mat img, img_HSV, img_HSV_mask, img_combined, img_per, img_clone, warped, frame;
int start_flag = 1, find_blue_card_flag = 0, cross_flag = 0, music_flag = 0, avoid_flag = 0, car_blake_flag = 0;
int LAB_Bmin = 190, LAB_Bmax = 255, HSL_Lmin = 134, HSL_Lmax = 255;
int TIMECOUNT = 0; //定时器延时
int line_error = 0, line_error1 = 0, line_last_error = 0, line_last_error1 = 0;
float center_x = 0, left_x = 0, right_x = 0;
int line_y = 190;
int rail_width_pix = 56;//跑道一半宽度
Point mousePos(0, 0);
double kp = 0.28; // 0.155;//0.11?????;//0.16;//0.2;//0.3;//0.1???? 0.3????
double kd = 0.04; // 0.1;//0 ?????
double kp1 = 0.25, kd1 = 0.02;
double min_angle = 30; // 80 90 - 10
double max_angle = 30; // 100
int Angle_Z;
using namespace std;
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 << "find crossing" << endl;
return true;
}
return false;
}
void gpio_init(void)
{
if (gpioInitialise() < 0)
exit(1);
gpioSetMode(13, PI_OUTPUT);
gpioSetPWMrange(13, 40000);
gpioSetPWMfrequency(13, 200);
gpioPWM(13, 12000);
gpioDelay(1000000);
gpioSetMode(22, PI_OUTPUT);
gpioSetPWMfrequency(22, 50);
gpioSetPWMrange(22, 1000);
gpioPWM(22, 75); //
gpioSetMode(23, PI_OUTPUT);
gpioSetPWMfrequency(23, 50);
gpioSetPWMrange(23, 1000);
gpioPWM(23, 70); //大上下小
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
gpioPWM(12, value);
}
void Start_motor(void)
{
cout << "1" << endl;
gpioPWM(13, 10400);
//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_stop(void)
{
gpioPWM(13, 12600); // 刹车
gpioDelay(50000);
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 nStop)
{
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, sizeof(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);
cfsetospeed(&newtio, B2400);
break;
case 4800:
cfsetispeed(&newtio, B4800);
cfsetospeed(&newtio, B4800);
break;
case 9600:
cfsetispeed(&newtio, B9600);
cfsetospeed(&newtio, B9600);
break;
case 115200:
cfsetispeed(&newtio, B115200);
cfsetospeed(&newtio, B115200);
break;
case 460800:
cfsetispeed(&newtio, B460800);
cfsetospeed(&newtio, B460800);
break;
default:
cfsetispeed(&newtio, B9600);
cfsetospeed(&newtio, B9600);
break;
}
if (nStop == 1)
newtio.c_cflag &= ~CSTOPB;
else if (nStop == 2)
newtio.c_cflag |= CSTOPB;
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 * sizeof(unsigned char));
return length;
}
int recv_data(int fd, char* recv_buffer, int length)
{
length = read(fd, recv_buffer, length);
return length;
}
void Control_Xun(void)
{
int angle = kp * line_error + kd * (line_error - line_last_error);
line_last_error = line_error;
angle = 90 - angle;
if (angle > 90 + max_angle) // 130
angle = 90 + max_angle;
if (angle < 90 - min_angle)
angle = 90 - min_angle;
//cout <<"line_error"<<line_error<<"angle:" << angle << endl;
//cout << "循迹处理完毕" << endl;
steering(angle);
}
void Control_avoid(void)
{
const int DEAD_ZONE = 30; // 中心死区阈值(像素)
const int AVOID_ANGLE = 25; // 基础避让角度偏移量
// 修改:使用负反馈控制
int angle = 90 - (kp1 * line_error1 + kd1 * (line_error1 - line_last_error1));
line_last_error1 = line_error1;
// 中心区域强制避让(忽略距离因素)
if (abs(line_error1) < DEAD_ZONE) {
// 根据偏差方向决定避让方向
int avoid_direction = (line_error1 >= 0) ? 1 : -1;
angle = 90 + avoid_direction * AVOID_ANGLE;
}
// 保留限幅逻辑
if (angle > 105) angle = 120;
if (angle < 75) angle = 70;
cout << "avoid_angle:" << angle << endl;
steering(angle);
}
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
}
vector<Point2f> get_lines_fangcheng(vector<Vec4i> lines)
{
// 遍历概率霍夫变换检测到的直线
float k = 0; // 斜率
float b = 0; // 截距
vector<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(100, 100, 50);
Scalar Upper(120, 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);
vector<vector<Point>> contours;
findContours(binary, contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
int maxContourIndex = -1;
double maxContourArea = 0.0;
for (int i = 0; i < contours.size(); i++)
{
double area = cv::contourArea(contours[i]);
if (area > maxContourArea)
{
maxContourArea = area;
maxContourIndex = i;
}
}
if (maxContourIndex != -1) {
Rect boundRect = boundingRect(contours[maxContourIndex]);
return boundRect;
}
else {
return Rect();
}
}
//Rect Obstacles(Mat img)//识别到锥桶
//{
// Rect obstacle_object = blue(img);//识别到的蓝色物体
// if (obstacle_object.height > 5 && obstacle_object.height < 100)//对识别到的蓝色物体限幅,
// {
// rectangle(img, Point(obstacle_object.x, obstacle_object.y + frame.rows / 2), Point(obstacle_object.x + obstacle_object.width, obstacle_object.y + obstacle_object.height + frame.rows / 2), Scalar(0, 255, 0), 3);
// return obstacle_object;
// }
// return Rect();
//}
Rect Obstacles(Mat img) {
Rect obstacle_object = blue(img);
if (obstacle_object.height > 5 && obstacle_object.height < 100) {
// 计算在原图中的坐标位置
int roi_offset_y = img.rows / 2; // ROI起始y坐标
Point topLeft(obstacle_object.x, obstacle_object.y + roi_offset_y);
Point bottomRight(obstacle_object.x + obstacle_object.width,
obstacle_object.y + obstacle_object.height + roi_offset_y);
// 在原图上绘制绿色框
rectangle(img, topLeft, bottomRight, Scalar(0, 255, 0), 3);
return obstacle_object;
}
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(void)
{
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 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]);
Angle_Z = Angle[2] - 167;
printf("Z:%d \r\n", Angle_Z);
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 Get_GPS_data(void)
{
while (1)
{
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);
}
//usleep(1000);
}
void audio(void)
{
system("omxplayer /home/pi/car435/car.wav");
}
Mat HLS_Lthresh(const cv::Mat& img, int min_thresh = 220, int max_thresh = 255)
{
Mat hls;
cvtColor(img, hls, COLOR_BGR2HLS);
vector<Mat> channels;
split(hls, channels);
Mat l_channel = channels[1];
double minVal, maxVal;
minMaxLoc(l_channel, &minVal, &maxVal);
l_channel.convertTo(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);
vector<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.convertTo(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
{
vector<Point> left_points;
vector<Point> right_points;
vector<float> left_fit;
vector<float> right_fit;
vector<float> mid_fit;
vector<Rect> left_windows;
vector<Rect> right_windows;
};
Mat getHistogram(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 = 20, int minpix = 10)
{
LaneData result;
auto [leftx, rightx] = findLaneBases(getHistogram(binary_img));
//cout<<"leftx:"<<leftx<<"rightx:"<<rightx<<endl;
int window_height = binary_img.rows / nwindows;
vector<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 auto& 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;
}
vector<float> polyFit(const vector<Point>& points, int order = 2)
{
if (points.size() < order + 1)
{
cout << "data no enough for" << order << "profit" << endl; return {};
}
Mat A(points.size(), order + 1, 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_SVD);
//solve(A, B, coeff, DECOMP_QR);
return { coeff.at<float>(0), coeff.at<float>(1), coeff.at<float>(2) };
}
vector<float> calculateCenterLaneCoefficient(const vector<float>& left_fit, const vector<float>& right_fit)
{
if (left_fit.size() != 3 || right_fit.size() != 3) {
cout << "Invalid polynomial coefficients" << endl;
return {};
}
vector<float> center_fit(3);
center_fit[0] = (left_fit[0] + right_fit[0]) / 2.0f; // A 系数
center_fit[1] = (left_fit[1] + right_fit[1]) / 2.0f; // B 系数
center_fit[2] = (left_fit[2] + right_fit[2]) / 2.0f; // C 系数
//cout<<center_fit[0]<<center_fit[1]<<center_fit[2]<<endl;
return center_fit;
}
float calculateXFromPolynomial(const vector<float>& fit, float y)
{
if (fit.size() != 3) return 0.0f;
return fit[0] * y * y + fit[1] * y + fit[2];
}
void visualize(Mat& img, const LaneData& data) {
// 绘制滑动窗口
// for (const auto& win : data.left_windows)
// rectangle(img, win, Scalar(0,255,0), 2);
// for (const auto& win : data.right_windows)
// rectangle(img, win, Scalar(0,255,0), 2);
// 绘制车道线点
for (const auto& pt : data.left_points)
circle(img, pt, 3, Scalar(255, 0, 0), -1);
for (const auto& 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);
}
}
circle(img, Point(center_x, line_y), 3, Scalar(0, 255, 0), -1);
}
bool Contour_Area(vector<Point> contour1, vector<Point> contour2)
{
return contourArea(contour1) > contourArea(contour2);
}
Mat customEqualizeHist(const Mat& inputImage, float alpha)
{
Mat enhancedImage;
equalizeHist(inputImage, enhancedImage);
return alpha * enhancedImage + (1 - alpha) * inputImage;
}
void timer(void)//定时器中断
{
TIMECOUNT++;
std::cout << "timer:" << TIMECOUNT << std::endl;
}
void reset_timer(void)//定时器中断
{
TIMECOUNT = 0;
}
int timer_interrupt(void)//定时器
{
while (true)
{
//timer();
this_thread::sleep_for(chrono::milliseconds(50));
Control_Xun();
}
return 0;
}
void onTrackbar(int, void*)
{
LAB_Bmin = getTrackbarPos("LAB_Bmin", "TrackBars");
LAB_Bmax = getTrackbarPos("LAB_Bmax", "TrackBars");
HSL_Lmin = getTrackbarPos("HSL_Lmin", "TrackBars");
HSL_Lmax = getTrackbarPos("HSL_Lmax", "TrackBars");
}
void UI_init(void)
{
namedWindow("TrackBars", WINDOW_NORMAL);
resizeWindow("TrackBars", 320, 240);
moveWindow("TrackBars", 0, 50); // 设置窗口位置
createTrackbar("LAB_Bmin", "TrackBars", &LAB_Bmin, 255, onTrackbar);
setTrackbarPos("LAB_Bmin", "TrackBars", LAB_Bmin);
createTrackbar("LAB_Bmax", "TrackBars", &LAB_Bmax, 255, onTrackbar);
setTrackbarPos("LAB_Bmax", "TrackBars", LAB_Bmax);
createTrackbar("HSL_Lmin", "TrackBars", &HSL_Lmin, 255, onTrackbar);
setTrackbarPos("HSL_Lmin", "TrackBars", HSL_Lmin);
createTrackbar("HSL_Lmax", "TrackBars", &HSL_Lmax, 255, onTrackbar);
setTrackbarPos("HSL_Lmax", "TrackBars", HSL_Lmax);
}
void onMouse(int event, int x, int y, int flags, void* userdata)
{
mousePos = Point(x, y);
}
void find_blue_card(void)//检测到蓝色挡板
{
vector<vector<Point>> contours;
vector<Vec4i> hierarcy;
findContours(img_HSV_mask, contours, hierarcy, RETR_EXTERNAL, CHAIN_APPROX_NONE);
if (contours.size() > 0)
{
sort(contours.begin(), contours.end(), Contour_Area);
//cout<<cStart_motorontours.size()<<endl;
vector<vector<Point>> newContours;
for (const vector<Point>& contour : contours)
{
Point2f center;
float radius;
minEnclosingCircle(contour, center, radius);
if (center.y > 90 && center.y < 160)
{
newContours.push_back(contour);
}
}
contours = newContours;
if (contours.size() > 0)
{
if (contourArea(contours[0]) > 500)
{
cout << "find blue card" << endl;
Point2f center;
float radius;
minEnclosingCircle(contours[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;
vector<vector<Point>> contours;
vector<Vec4i> hierarcy;
findContours(img_HSV_mask, contours, hierarcy, RETR_EXTERNAL, CHAIN_APPROX_NONE);
if (contours.size() > 0)
{
sort(contours.begin(), contours.end(), Contour_Area);
vector<vector<Point>> newContours;
for (const vector<Point>& contour : contours)
{
Point2f center;
float radius;
minEnclosingCircle(contour, center, radius);
if (center.y > 90 && center.y < 160)
{
newContours.push_back(contour);
}
}
contours = newContours;
if (contours.size() == 0)
{
start_flag = 0;
cout << "blue card remove" << endl;
//Start_motor();
sleep(1);
}
}
else
{
start_flag = 0;
cout << "blue card remove" << endl;
//Start_motor();
sleep(1);
}
}
int crossroad(void)//斑马线识别
{
//gpioDelay(10000000);//延时十秒
VideoCapture capture;
capture.open(2);
if (!capture.isOpened())
{
cout << "Can not open camera" << endl;
return -1;
}
else
cout << "camera open successful" << endl;
capture.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
capture.set(cv::CAP_PROP_FPS, 30);
capture.set(cv::CAP_PROP_FRAME_WIDTH, 320);
capture.set(cv::CAP_PROP_FRAME_HEIGHT, 240);
this_thread::sleep_for(chrono::milliseconds(2000)); //让图像稳定
cout << "Program started" << endl;
Mat img_cross;
Mat blur;
while (1)
{
capture >> img_cross;
GaussianBlur(img_cross, 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,120,320,120);//shang
Rect roi(0, 260, 640, 220);//xia
Mat region = cross_mask(roi);
//imshow("combined_image", region);
//imshow("Full Mask", cross_mask); // 完整图像
imshow("ROI Region", 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 >= 16) ? 1 : 0;
cout << "stripe_count" << stripe_count << "cross_flag:" << cross_flag << endl;
if (cv::waitKey(1) == 27 || cross_flag == 1)break;
}
//gpioTerminate();
capture.release(); // 释放摄像头
destroyWindow("ROI Region"); // 只关闭自己的窗口
//uart_close(GPS);
return 0;
}
Mat per_wt(Mat& frame)
{
int img_h, img_w;
img_clone = frame.clone();
img_h = img_clone.rows; // 图像高度
img_w = img_clone.cols;
vector<cv::Point2f> src = {
Point2f(125, 198),
Point2f(230, 198),
Point2f(318, 237),
Point2f(55, 237)
};
vector<cv::Point2f> dst = {
Point2f(100,0),
Point2f(img_w - 100, 0),
Point2f(img_w - 100, img_h),
Point2f(100, img_h)
};
// 定义目标坐标(变换后的四个点)
vector<cv::Point> polygon;
for (const auto& 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);
// 计算透视变换矩阵
Mat M = getPerspectiveTransform(src, dst);
Mat Minv = getPerspectiveTransform(dst, src);
// 应用透视
warpPerspective(frame, warped, M, cv::Size(img_w, img_h), cv::INTER_LINEAR);
waitKey(5);
return warped;
}
int find_line(void) //循迹线程
{
// 初始化程序
//UI_init();
VideoCapture capture;
capture.open(0);
if (!capture.isOpened())
{
cout << "Can not open camera" << endl;
return -1;
}
else
cout << "camera open successful" << endl;
capture.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
capture.set(cv::CAP_PROP_FPS, 30);
capture.set(cv::CAP_PROP_FRAME_WIDTH, 320);
capture.set(cv::CAP_PROP_FRAME_HEIGHT, 240);
this_thread::sleep_for(chrono::milliseconds(1000)); //让图像稳定
cout << "Program started" << endl;
while (1)
{
capture >> img;
if (start_flag == 1)
{
cvtColor(img, img_HSV, COLOR_BGR2HSV);
Scalar HSV_L = Scalar(100, 43, 46);
Scalar HSV_H = Scalar(124, 255, 255);
inRange(img_HSV, HSV_L, HSV_H, img_HSV_mask);
Mat kernel = getStructuringElement(MORPH_RECT, Size(5, 5));
morphologyEx(img_HSV_mask, img_HSV_mask, MORPH_OPEN, kernel);
morphologyEx(img_HSV_mask, img_HSV_mask, MORPH_CLOSE, kernel);
if (find_blue_card_flag == 0)find_blue_card();
else find_blue_card_remove();
}
else
{
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);
if (lanes.left_fit.size() != 3 || lanes.right_fit.size() != 3)
{
if (lanes.left_fit.size() != 3)center_x = lanes.right_fit[0] * line_y * line_y + lanes.right_fit[1] * line_y + lanes.right_fit[2] - rail_width_pix;
if (lanes.right_fit.size() != 3)center_x = lanes.left_fit[0] * line_y * line_y + lanes.left_fit[1] * line_y + lanes.left_fit[2] + rail_width_pix;
//cout << center_x << endl;
}
if (lanes.left_fit.size() == 3 && lanes.right_fit.size() == 3)
{
center_x = (lanes.right_fit[0] * line_y * line_y + lanes.right_fit[1] * line_y + lanes.right_fit[2] + lanes.left_fit[0] * line_y * line_y + lanes.left_fit[1] * line_y + lanes.left_fit[2]) / 2;
//cout << center_x << endl;
}
line_error = center_x - 160;
//cout << left_x << "," << right_x << "," << center_x << "," << line_error << endl;
visualize(white_image, lanes);
//namedWindow("img", cv::WINDOW_NORMAL); // 创建可调整大小的窗口
//resizeWindow("img", 320, 240); // 设置窗口尺寸
//moveWindow("img", 320, 50); // 设置窗口位置
//imshow("img", img_clone);
//namedWindow("combined_image", cv::WINDOW_NORMAL); // 创建可调整大小的窗口
//resizeWindow("combined_image", 320, 240); // 设置窗口尺寸
//moveWindow("combined_image", 640, 50); // 设置窗口位置
imshow("combined_image", img_combined);
namedWindow("white_image", cv::WINDOW_NORMAL); // 创建可调整大小的窗口
resizeWindow("white_image", 320, 240); // 设置窗口尺寸
moveWindow("white_image", 960, 50); // 设置窗口位置
imshow("white_image", white_image);
Start_motor();
if (cross_flag == 1) {
Control_stop();//停车
audio();//语音
Start_motor();//再次发车
cross_flag = 2;
cout << "cross_flag = 2" << endl;
gpioDelay(3000000);
break;
}
}
if (cv::waitKey(1) == 27)break;
//if(car_blake_flag == 1) ;//break;//结束代码
}
gpioTerminate();
capture.release(); // 释放摄像头
destroyAllWindows(); // 关闭所有窗口
//uart_close(GPS);
return 0;
}
int Blue_cone(void)
{
VideoCapture capture;
capture.open(2);
if (!capture.isOpened()) {
cout << "Error: Camera not available" << endl;
return -1;
}
else
cout << "camera open successful" << endl;
capture.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
capture.set(cv::CAP_PROP_FPS, 30);
capture.set(cv::CAP_PROP_FRAME_WIDTH, 640);
capture.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
this_thread::sleep_for(chrono::milliseconds(1000)); //让图像稳定
cout << "Program started" << endl;
//Mat frame;
while (1)
{
capture >> frame;
if (!capture.read(frame)) { // 更健壮的帧读取检查
cout << "Frame read error!" << endl;
break;
}
Rect cone = Obstacles(frame);
if (cone.width > 0 && cone.height > 0) {
// 计算障碍物中心与画面中心的水平偏移
int obstacle_center_x = cone.x + cone.width / 2;
int frame_center_x = frame.cols / 2;
cout << obstacle_center_x << "," << frame_center_x << endl;
line_error1 = obstacle_center_x - frame_center_x; // 水平偏移量
cout << "Error: " << line_error1 << endl;
Control_avoid();
imshow("blue_cone", frame);
Start_motor();
}
if (waitKey(1) == 27) break; // ESC退出
}
gpioTerminate();
capture.release(); // 释放摄像头
destroyAllWindows(); // 关闭所有窗口
return 0;
}
class BlueSignDetector {
private:
// 优化后的蓝色标志牌HSV颜色范围
int lowH = 100, highH = 130; // 色调范围
int lowS = 120, highS = 245; // 饱和度范围
int lowV = 180, highV = 245; // 亮度范围
bool calibrating = false;
// 字母识别参数
double min_contour_area = 50; // 最小轮廓面积
double max_contour_area = 10000; // 最大轮廓面积
public:
BlueSignDetector() {
cout << "蓝色标志牌检测器初始化完成" << endl;
cout << "使用轮廓分析方法识别A/B标志" << endl;
}
// 原有的校准函数保持不变...
void toggleCalibration() {
calibrating = !calibrating;
if (calibrating) {
cout << "进入颜色校准模式" << endl;
createCalibrationWindow();
}
else {
cout << "退出颜色校准模式" << endl;
destroyWindow("颜色校准");
}
}
void createCalibrationWindow() {
namedWindow("颜色校准", WINDOW_NORMAL);
resizeWindow("颜色校准", 800, 600);
createTrackbar("Low H", "颜色校准", &lowH, 179, nullptr);
createTrackbar("High H", "颜色校准", &highH, 179, nullptr);
createTrackbar("Low S", "颜色校准", &lowS, 255, nullptr);
createTrackbar("High S", "颜色校准", &highS, 255, nullptr);
createTrackbar("Low V", "颜色校准", &lowV, 255, nullptr);
createTrackbar("High V", "颜色校准", &highV, 255, nullptr);
setTrackbarPos("Low H", "颜色校准", lowH);
setTrackbarPos("High H", "颜色校准", highH);
setTrackbarPos("Low S", "颜色校准", lowS);
setTrackbarPos("High S", "颜色校准", highS);
setTrackbarPos("Low V", "颜色校准", lowV);
setTrackbarPos("High V", "颜色校准", highV);
}
void getColorRange(Scalar& lower, Scalar& upper) {
lower = Scalar(lowH, lowS, lowV);
upper = Scalar(highH, highS, highV);
}
bool isCalibrating() {
return calibrating;
}
void showCalibration(Mat& frame) {
if (!calibrating) return;
Mat hsv, mask, result;
cvtColor(frame, hsv, COLOR_BGR2HSV);
inRange(hsv, Scalar(lowH, lowS, lowV), Scalar(highH, highS, highV), mask);
Mat display;
frame.copyTo(display);
string text = "H: " + to_string(lowH) + "-" + to_string(highH) +
" S: " + to_string(lowS) + "-" + to_string(highS) +
" V: " + to_string(lowV) + "-" + to_string(highV);
putText(display, text, Point(10, 30), FONT_HERSHEY_SIMPLEX, 0.7, Scalar(0, 255, 255), 2);
Mat combined;
cvtColor(mask, mask, COLOR_GRAY2BGR);
hconcat(display, mask, combined);
imshow("颜色校准", combined);
}
/**
* 主识别函数
*/
int detectBlueSign(Mat& frame, Mat& display_frame) {
frame.copyTo(display_frame);
// 显示调试信息
string size_info = "图像大小: " + to_string(frame.cols) + "x" + to_string(frame.rows);
putText(display_frame, size_info, Point(10, 30),
FONT_HERSHEY_SIMPLEX, 0.6, Scalar(255, 255, 255), 2);
// 预处理图像,提取蓝色区域
Mat blue_mask = extractBlueRegions(frame);
// 在显示图像上叠加蓝色掩膜
Mat blue_display;
cvtColor(blue_mask, blue_display, COLOR_GRAY2BGR);
addWeighted(display_frame, 0.7, blue_display, 0.3, 0, display_frame);
// 查找可能的标志牌区域
vector<Rect> signRegions = findSignRegions(blue_mask, frame);
cout << "找到 " << signRegions.size() << " 个候选区域" << endl;
// 对每个候选区域进行识别
for (size_t i = 0; i < signRegions.size(); i++) {
const Rect& roi = signRegions[i];
// 创建临时变量
Mat roi_region = frame(roi);
int result = recognizeBlueLetter(roi_region);
if (result != 0) {
// 在控制台输出详细信息
cout << "=== 识别到标志牌 ===" << endl;
cout << "区域 " << i << ": 位置(" << roi.x << ", " << roi.y << ")" << endl;
cout << "大小: " << roi.width << "x" << roi.height << endl;
cout << "面积: " << roi.width * roi.height << " 像素" << endl;
cout << "类型: " << (result == 1 ? "A" : "B") << endl;
cout << "===================" << endl;
// 绘制检测结果
rectangle(display_frame, roi, Scalar(0, 255, 255), 3);
string label = (result == 1) ? "A" : "B";
string text = "识别结果: " + label;
// 显示详细信息
string size_text = "大小: " + to_string(roi.width) + "x" + to_string(roi.height);
string area_text = "面积: " + to_string(roi.width * roi.height) + " 像素";
string pos_text = "位置: (" + to_string(roi.x) + "," + to_string(roi.y) + ")";
putText(display_frame, text, Point(roi.x, roi.y - 15),
FONT_HERSHEY_SIMPLEX, 0.8, Scalar(0, 255, 255), 2);
putText(display_frame, size_text, Point(roi.x, roi.y + roi.height + 25),
FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 255, 0), 2);
putText(display_frame, area_text, Point(roi.x, roi.y + roi.height + 45),
FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 255, 0), 2);
putText(display_frame, pos_text, Point(roi.x, roi.y + roi.height + 65),
FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 255, 0), 2);
// 在图像顶部显示大号结果
string result_text = "停车标志 " + label + " - " + size_text;
putText(display_frame, result_text, Point(50, 80),
FONT_HERSHEY_SIMPLEX, 1.2,
(result == 1) ? Scalar(0, 255, 0) : Scalar(255, 0, 0), 3);
return result;
}
else {
cout << "区域 " << i << ": 无法识别字母类型" << endl;
// 绘制未识别区域(红色)
rectangle(display_frame, roi, Scalar(0, 0, 255), 2);
putText(display_frame, "未识别", Point(roi.x, roi.y - 10),
FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 0, 255), 2);
}
}
if (signRegions.empty()) {
putText(display_frame, "未找到蓝色标志牌区域", Point(50, 100),
FONT_HERSHEY_SIMPLEX, 1.0, Scalar(0, 0, 255), 2);
}
else {
putText(display_frame, "找到区域但无法识别字母", Point(50, 100),
FONT_HERSHEY_SIMPLEX, 1.0, Scalar(0, 255, 255), 2);
}
return 0;
}
private:
/**
* 提取蓝色区域
*/
Mat extractBlueRegions(Mat& frame) {
Mat hsv, blue_mask;
// 转换为HSV颜色空间
cvtColor(frame, hsv, COLOR_BGR2HSV);
// 提取蓝色区域
inRange(hsv, Scalar(lowH, lowS, lowV), Scalar(highH, highS, highV), blue_mask);
// 形态学操作
Mat kernel_open = getStructuringElement(MORPH_RECT, Size(3, 3));
Mat kernel_close = getStructuringElement(MORPH_RECT, Size(7, 7));
morphologyEx(blue_mask, blue_mask, MORPH_OPEN, kernel_open);
morphologyEx(blue_mask, blue_mask, MORPH_CLOSE, kernel_close);
return blue_mask;
}
/**
* 寻找标志牌区域
*/
vector<Rect> findSignRegions(Mat& blue_mask, Mat& original) {
vector<vector<Point>> contours;
vector<Vec4i> hierarchy;
vector<Rect> candidates;
findContours(blue_mask, contours, hierarchy, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
cout << "轮廓数量: " << contours.size() << endl;
for (size_t i = 0; i < contours.size(); i++) {
// 计算轮廓面积
double area = contourArea(contours[i]);
if (area < min_contour_area || area > max_contour_area) {
cout << "轮廓 " << i << " 面积 " << area << " 超出范围,跳过" << endl;
continue;
}
// 获取边界矩形
Rect rect = boundingRect(contours[i]);
// 检查长宽比
double aspect_ratio = (double)rect.width / rect.height;
if (aspect_ratio < 0.7 || aspect_ratio > 20.0) {
cout << "轮廓 " << i << " 宽高比 " << aspect_ratio << " 不合适,跳过" << endl;
continue;
}
// 检查矩形度
double rect_area = rect.width * rect.height;
double rectness = area / rect_area;
if (rectness < 0.3) {
cout << "轮廓 " << i << " 矩形度 " << rectness << " 太低,跳过" << endl;
continue;
}
// 检查尺寸合理性
if (rect.width < 20 || rect.height < 20) {
cout << "轮廓 " << i << " 尺寸太小,跳过" << endl;
continue;
}
cout << "候选区域 " << i << ": " << rect.width << "x" << rect.height
<< " (面积: " << area << ", 矩形度: " << rectness
<< ", 宽高比: " << aspect_ratio << ")" << endl;
candidates.push_back(rect);
}
return candidates;
}
/**
* 字母识别 - 纯轮廓分析方法
*/
int recognizeBlueLetter(const Mat& roi) {
cout << "开始识别区域,大小: " << roi.cols << "x" << roi.rows << endl;
if (roi.cols < 20 || roi.rows < 20) {
cout << "区域太小,无法识别" << endl;
return 0;
}
return enhancedContourAnalysis(roi);
}
/**
* 增强的轮廓分析方法
*/
int enhancedContourAnalysis(const Mat& roi) {
// 转换为灰度图
Mat gray;
cvtColor(roi, gray, COLOR_BGR2GRAY);
// 多种二值化方法尝试
Mat binary1, binary2, binary3;
threshold(gray, binary1, 100, 255, THRESH_BINARY);
threshold(gray, binary2, 0, 255, THRESH_BINARY | THRESH_OTSU);
adaptiveThreshold(gray, binary3, 255, ADAPTIVE_THRESH_GAUSSIAN_C, THRESH_BINARY, 11, 2);
// 尝试不同的二值化结果
int result = analyzeBinaryImage(binary1, "固定阈值");
if (result != 0) return result;
result = analyzeBinaryImage(binary2, "OTSU阈值");
if (result != 0) return result;
result = analyzeBinaryImage(binary3, "自适应阈值");
return result;
}
/**
* 分析二值图像
*/
int analyzeBinaryImage(Mat& binary, const string& method_name) {
cout << "使用 " << method_name << " 进行分析..." << endl;
vector<vector<Point>> contours;
vector<Vec4i> hierarchy;
findContours(binary, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE);
if (contours.empty()) {
cout << method_name << ": 未找到轮廓" << endl;
return 0;
}
// 按面积排序轮廓
sort(contours.begin(), contours.end(),
[](const vector<Point>& a, const vector<Point>& b) {
return contourArea(a) > contourArea(b);
});
// 分析前3个最大的轮廓
for (size_t i = 0; i < min(contours.size(), size_t(3)); i++) {
double area = contourArea(contours[i]);
if (area < 20) continue;
// 计算轮廓特征
vector<Point> hull;
convexHull(contours[i], hull);
double hull_area = contourArea(hull);
double contour_area = contourArea(contours[i]);
double solidity = (hull_area > 0) ? contour_area / hull_area : 0;
Rect bound_rect = boundingRect(contours[i]);
double aspect_ratio = (double)bound_rect.width / bound_rect.height;
// 计算轮廓周长和圆形度
double perimeter = arcLength(contours[i], true);
double circularity = 0;
if (perimeter > 0) {
circularity = (4 * CV_PI * contour_area) / (perimeter * perimeter);
}
// 计算轮廓的顶点数(多边形逼近)
vector<Point> approx;
double epsilon = 0.02 * perimeter;
approxPolyDP(contours[i], approx, epsilon, true);
int vertices = approx.size();
cout << method_name << " - 轮廓 " << i << ": 面积=" << contour_area
<< ", 矩形度=" << solidity
<< ", 宽高比=" << aspect_ratio
<< ", 圆形度=" << circularity
<< ", 顶点数=" << vertices << endl;
// 改进的识别逻辑
if (analyzeShapeFeatures(contour_area, solidity, aspect_ratio, circularity, vertices)) {
cout << method_name << ": 判断为 A" << endl;
return 1;
}
else if (analyzeShapeFeaturesB(contour_area, solidity, aspect_ratio, circularity, vertices)) {
cout << method_name << ": 判断为 B" << endl;
return 2;
}
}
cout << method_name << ": 无法确定字母类型" << endl;
return 0;
}
/**
* 分析A字母的形状特征
*/
bool analyzeShapeFeatures(double area, double solidity, double aspect_ratio,
double circularity, int vertices) {
// B字母特征:通常更三角形,solidity较高,aspect_ratio接近1
bool condition1 = (solidity > 0.75 && aspect_ratio > 3 && aspect_ratio < 5.5);
// B字母可能有较少的顶点(三角形特征)
bool condition2 = (vertices >= 4 && vertices <= 5);
// B字母的圆形度通常较低(更接近三角形)
bool condition3 = (circularity < 0.4);
return condition1 && condition2 && condition3;
}
/**
* 分析B字母的形状特征
*/
bool analyzeShapeFeaturesB(double area, double solidity, double aspect_ratio,
double circularity, int vertices) {
// A字母特征:通常更圆形/椭圆形,solidity较低
bool condition1 = (solidity < 1 && 3 < aspect_ratio < 5.5);
// A字母可能有更多的顶点
bool condition2 = (vertices > 5);
// A字母的圆形度通常较高
bool condition3 = (circularity > 0.4);
return condition1 && condition2 && condition3;
}
};
// 创建显示窗口的函数
void createDisplayWindows() {
//namedWindow("原始图像", WINDOW_NORMAL);
//namedWindow("处理结果", WINDOW_NORMAL);
namedWindow("蓝色区域", WINDOW_NORMAL);
//resizeWindow("原始图像", 800, 600);
//resizeWindow("处理结果", 800, 600);
resizeWindow("蓝色区域", 800, 600);
}
int find_AB(void) {
while (1) {
cout << "等待cross_flag" << endl;
if (cross_flag == 2)
cout << "find_AB开启" << endl;
break;
}
gpioDelay(8000000);//延时8秒
VideoCapture cap(2);
if (!cap.isOpened()) {
cout << "无法打开摄像头" << endl;
return -1;
}
// 设置摄像头分辨率
cap.set(CAP_PROP_FRAME_WIDTH, 640);
cap.set(CAP_PROP_FRAME_HEIGHT, 480);
// 创建显示窗口
createDisplayWindows();
BlueSignDetector detector;
Mat frame, display_frame;
cout << "开始识别蓝色停车标志..." << endl;
cout << "红底赛道 + 蓝色标志牌" << endl;
cout << "使用轮廓分析方法" << endl;
cout << "按 'q' 退出程序" << endl;
cout << "按 'c' 进入/退出颜色校准模式" << endl;
cout << "按 's' 保存当前阈值" << endl;
// 多帧验证
int confirm_count = 0;
const int REQUIRED_CONFIRMATIONS = 5;
int last_sign = 0;
while (true) {
cap >> frame;
if (frame.empty()) break;
// 如果在校准模式,显示校准图像
detector.showCalibration(frame);
// 创建处理结果显示图像
frame.copyTo(display_frame);
// 如果不是校准模式,进行正常检测
if (!detector.isCalibrating()) {
// 进行标志牌识别
int current_sign = detector.detectBlueSign(frame, display_frame);
// 多帧验证逻辑
if (current_sign == last_sign && current_sign != 0) {
confirm_count++;
string confirm_text = "确认: " + to_string(confirm_count) + "/" +
to_string(REQUIRED_CONFIRMATIONS);
putText(display_frame, confirm_text,
Point(50, 130), FONT_HERSHEY_SIMPLEX, 0.8, Scalar(0, 255, 0), 2);
if (confirm_count >= REQUIRED_CONFIRMATIONS) {
string result_text = (current_sign == 1) ?
">>> 停入左侧A区 <<<" : ">>> 停入右侧B区 <<<";
putText(display_frame, result_text,
Point(100, 170), FONT_HERSHEY_SIMPLEX, 1.5,
Scalar(0, 0, 255), 3);
// 在控制台输出最终结果
cout << "!!! 最终确认: 停车标志 " << (current_sign == 1 ? "A" : "B") << " !!!" << endl;
}
}
else {
confirm_count = 0;
last_sign = current_sign;
}
}
else {
// 在校准模式下显示提示
putText(display_frame, "颜色校准模式 - 调节轨迹条优化蓝色检测",
Point(50, 100), FONT_HERSHEY_SIMPLEX, 0.8, Scalar(0, 255, 255), 2);
}
// 创建蓝色区域显示
Mat hsv, blue_mask, blue_mask_display;
cvtColor(frame, hsv, COLOR_BGR2HSV);
// 获取当前颜色阈值
Scalar lower_blue, upper_blue;
detector.getColorRange(lower_blue, upper_blue);
inRange(hsv, lower_blue, upper_blue, blue_mask);
cvtColor(blue_mask, blue_mask_display, COLOR_GRAY2BGR);
string blue_text = detector.isCalibrating() ? "蓝色区域检测 (校准模式)" : "蓝色区域检测";
putText(blue_mask_display, blue_text,
Point(10, 30), FONT_HERSHEY_SIMPLEX, 0.8, Scalar(255, 255, 255), 2);
// 显示所有窗口
//imshow("原始图像", frame);
//imshow("处理结果", display_frame);
imshow("蓝色区域", blue_mask_display);
// 键盘输入处理
char key = waitKey(1);
if (key == 'q') {
break;
}
else if (key == 'c') {
detector.toggleCalibration();
}
else if (key == 's') {
// 保存当前阈值
Scalar lower, upper;
detector.getColorRange(lower, upper);
cout << "当前颜色阈值已保存:" << endl;
cout << "H: " << lower[0] << "-" << upper[0] << endl;
cout << "S: " << lower[1] << "-" << upper[1] << endl;
cout << "V: " << lower[2] << "-" << upper[2] << endl;
}
}
cap.release();
destroyAllWindows();
return 0;
}
int main()
{
gpioTerminate();
gpio_init();
//GPS_init();
camera(72, 86);
steering(90);
thread thread1(find_line);//循迹线程
thread thread2(crossroad);//斑马线
thread thread3(timer_interrupt);//定时器功能
//thread thread4(find_AB);//停车线程
//thread thread5(Blue_cone);//避障线程
//thread thread6(Get_GPS_data);//test
thread1.join();
thread2.join();
thread3.join();
//thread4.join();
//thread5.join();
//thread6.join();
return 0;//GPS_init();
}
//取数据调试的main函数
//int main(void)
//{
// UI_init();
// gpioTerminate();
// gpio_init();
// camera(72,86);
// steering(90);
// // Mat img = imread("C:/Users/wtcat/CLionProjects/untitled7/test_images/225.jpg",IMREAD_COLOR_BGR);
// // if(img.empty())
// // {
// // std::cerr << "Error: Could not load the image" << std::endl;
// // return -1;
// // }
// VideoCapture capture;
// capture.open(0);
// if (!capture.isOpened())
// {
// cout << "Can not open camera!" << std::endl;
// return -1;
// }
// capture.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
// capture.set(cv::CAP_PROP_FPS, 30);
// capture.set(cv::CAP_PROP_FRAME_WIDTH, 320);
// capture.set(cv::CAP_PROP_FRAME_HEIGHT, 240);
// this_thread::sleep_for(chrono::milliseconds(1000)); //让图像稳定
// Mat img_per,img_combined;
// string windowName = "XY";
// namedWindow(windowName, WINDOW_AUTOSIZE);
// setMouseCallback(windowName, onMouse, NULL);
// while (1)
// {
// capture>>img;
// img_per=per_wt(img);
// string coordText = "X=" + to_string(mousePos.x) + " Y=" + to_string(mousePos.y);
// putText(img_clone, coordText, Point(15, 35),FONT_HERSHEY_SIMPLEX, 0.7, Scalar(0, 255, 100), 2);
// moveWindow(windowName, 320, 50);
// imshow(windowName, img_clone);
// 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);
// //img_combined=HLS_Lresult;
// 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("combined_image", cv::WINDOW_NORMAL); // 创建可调整大小的窗口
// resizeWindow("combined_image", 320, 240); // 设置窗口尺寸
// moveWindow("combined_image", 640, 50); // 设置窗口位置
// imshow("combined_image", img_combined);
// namedWindow("white_image", cv::WINDOW_NORMAL); // 创建可调整大小的窗口
// resizeWindow("white_image", 320, 240); // 设置窗口尺寸
// moveWindow("white_image", 960,50); // 设置窗口位置
// imshow("white_image", white_image);
// if (cv::waitKey(1) == 27)
// {
// break;
// }
// //if(car_blake_flag == 1) ;//break;//结束代码
// }
// capture.release(); // 释放摄像头
// destroyAllWindows(); // 关闭所有窗口
// return 0;
//}