直线追踪-编程知识

直线追踪

由于项目的需要，最近在做一个直线追踪的东西，但是网上的代码关于车道线或者别的什么之类的直线追踪的代码只是提了一下，相关的代码并不是公开的，所以自己写了一些直线追踪的代码。

代码使用的是kalman滤波进行直线追踪，当然这个代码是实验代码，现在已经可以调试成功了，进行一下记录。

kalman_lanes.h：我这里只写了一个函数，直接将将这个函数进行一个引入便可以

#pragma once
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <vector>using namespace cv;
using namespace std;void kalman_line(VideoCapture video, Mat frame, vector<Vec2f> lines, float slope, float intercept);

kalman_lanes.cpp:函数的实现

#include "kalman_lines.h"
#include <vector>using namespace std;void kalman_line(VideoCapture video, Mat frame, vector<Vec2f> lines, float slope, float intercept)
{// 创建Kalman滤波器对象KalmanFilter kf(4, 2, 0);Mat state(4, 1, CV_32F);  // x, y, vx, vyMat measurement = Mat::zeros(2, 1, CV_32F);  // mx, my// 设置Kalman滤波器初始状态kf.transitionMatrix = (Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);setIdentity(kf.measurementMatrix);setIdentity(kf.processNoiseCov, Scalar::all(1e-4));setIdentity(kf.measurementNoiseCov, Scalar::all(1e-1));setIdentity(kf.errorCovPost, Scalar::all(1));// 选择特定斜率与截距的直线进行追踪for (size_t i = 0; i < lines.size(); i++){float rho = lines[i][0];float theta = lines[i][1];if (abs(theta) < CV_PI / 4 || abs(theta - CV_PI / 2) < CV_PI / 4){float a = cos(theta);float b = sin(theta);float x0 = a * rho;float y0 = b * rho;Point pt1(cvRound(x0 + 1000 * (-b)), cvRound(y0 + 1000 * (a)));Point pt2(cvRound(x0 - 1000 * (-b)), cvRound(y0 - 1000 * (a)));float current_slope = (pt2.y - pt1.y) / (float)(pt2.x - pt1.x);float current_intercept = pt1.y - current_slope * pt1.x;if (abs(current_slope - slope) < 0.2 && abs(current_intercept - intercept) < 100){slope = current_slope;intercept = current_intercept;// 更新Kalman滤波器初始状态state.at<float>(0) = intercept;state.at<float>(1) = slope;kf.statePre.at<float>(0) = intercept;kf.statePre.at<float>(1) = slope;}}}Mat gray, edges;// 视频帧循环处理while (true){// 读取下一帧图像video >> frame;// 检查是否到达视频末尾if (frame.empty())break;// 预测下一状态Mat prediction = kf.predict();// 提取当前帧直线cvtColor(frame, gray, COLOR_BGR2GRAY);Canny(gray, edges, 50, 150, 3);std::vector<Vec2f> lines;HoughLines(edges, lines, 1, CV_PI / 180, 100);// 选择最接近预测斜率与截距的直线进行追踪float min_distance = FLT_MAX;float closest_slope = 0.0;float closest_intercept = 0.0;for (size_t i = 0; i < lines.size(); i++){float rho = lines[i][0];float theta = lines[i][1];if (abs(theta) < CV_PI / 4 || abs(theta - CV_PI / 2) < CV_PI / 4){float a = cos(theta);float b = sin(theta);float x0 = a * rho;float y0 = b * rho;Point pt1(cvRound(x0 + 1000 * (-b)), cvRound(y0 + 1000 * (a)));Point pt2(cvRound(x0 - 1000 * (-b)), cvRound(y0 - 1000 * (a)));float current_slope = (pt2.y - pt1.y) / (float)(pt2.x - pt1.x);float current_intercept = pt1.y - current_slope * pt1.x;float distance = sqrt(pow(current_slope - prediction.at<float>(1), 2) + pow(current_intercept - prediction.at<float>(0), 2));if (distance < min_distance){min_distance = distance;closest_slope = current_slope;closest_intercept = current_intercept;}}}// 更新Kalman滤波器状态Mat measurement(2, 1, CV_32F);measurement.at<float>(0) = closest_intercept;measurement.at<float>(1) = closest_slope;kf.correct(measurement);// 绘制预测线和测量线float slope_pred = kf.statePost.at<float>(1);float intercept_pred = kf.statePost.at<float>(0);float slope_meas = closest_slope;float intercept_meas = closest_intercept;Point pt1_pred(0, intercept_pred);Point pt2_pred(frame.cols, slope_pred * frame.cols + intercept_pred);Point pt1_meas(0, intercept_meas);Point pt2_meas(frame.cols, slope_meas * frame.cols + intercept_meas);line(frame, pt1_pred, pt2_pred, Scalar(0, 255, 0), 2);line(frame, pt1_meas, pt2_meas, Scalar(0, 0, 255), 2);imshow("Video", frame);// 检测键盘按下if (waitKey(30) == 27){break;}}
}

main主函数：

#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>#include "kalman_lines.h"using namespace cv;int main()
{// 打开视频文件VideoCapture video("2.mp4");// 检查视频是否成功打开if (!video.isOpened()){std::cout << "无法打开视频文件" << std::endl;return -1;}// 创建窗口，用于显示视频帧namedWindow("Video", WINDOW_NORMAL);// 获取第一帧图像Mat frame;video >> frame;// 提取第一帧的直线Mat gray, edges;cvtColor(frame, gray, COLOR_BGR2GRAY);Canny(gray, edges, 50, 150, 3);std::vector<Vec2f> lines;HoughLines(edges, lines, 1, CV_PI / 180, 100);kalman_line(video, frame,lines,1.0,0.0);return 0;
}

上述代码之中我根据直线的斜率和截距进行追踪。

我随便画了一条线，然后拍摄了一个视频大概是这样的情况（上传不了视频，将就着看一下）：