直线追踪

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

代码使用的是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, vy
	Mat 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;
}

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

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