1.初始帧RANSAC
2.跟踪帧降低感兴趣区域,使用最小二乘。
3.如果跟踪失效,重新进入初始帧。
%% 初始化代码
%-----FLAGL=1;FLAGR=1;在跟踪状态下未检测到车道线,以上一帧的位置打印
clc;close all;
clear all
pic_name='Tsd76.gif';
VideoFile = VideoReader('E:\MATLAB\遍历图片保存为mat\shangqi\lane_detection\test.avi');
nFrames =VideoFile.NumberOfFrames; %得到帧数
%% 初始化calman参数
%X=[rho,theta,dr,dtheta]选取的初值分别为距离rho、角度加入量测
%速度dr、角速度的平均值vb
X_left=[-1 100 0.2 5];
R=[(X_left(1)*0.2)^2 0;0 (X_left(2)*0.1)^2];%测量噪声协方差矩阵 已知
Q=[(X_left(1)*0.2)^2 0 0 0;0 (X_left(2)*0.1)^2 0 0;0 0 (X_left(3)*0.2)^2 0;0 0 0 (X_left(4)*0.1)^2]; %过程噪声协方差矩阵 已知
A=[1 0 1 0;0 1 0 1;0 0 1 0;0 0 0 1];%系统矩阵 已知
H=[1 0 0 0;0 1 0 0]; %观测状态转移矩阵
P_left=[10 0 0 0; 0 25 0 0; 0 0 4 0; 0 0 0 4];%误差协方差矩阵 任意给的初值 待定
P_right=[10 0 0 0; 0 25 0 0; 0 0 4 0; 0 0 0 4];%误差协方差矩阵 任意给的初值 待定
xL=zeros(10,4);%用于存储中间状态值,按行存储,用时得转置变为列向量
xR=zeros(10,4);%用于存储中间状态值,按行存储,用时得转置变为列向量
countL=5;
countR=5;%车道线初始帧的标志
I_H=480;
I_W=360;
offset=10;
flag=1;
for times=1:nFrames
Frame= read(VideoFile,times); %从VideoFile视频文件中读取视频帧
I=imresize( Frame ,[360 60]);
tic;
gray= rgb2gray(I);
%% ROI点的确定与ROI的提取
%------------------------从边框中提取感兴趣区域-----------------------
if countL==5 | countR==5 %roi更新
% m=[1,480,480,1];n=[220,220,360,360]
load roi;
roi= roipoly( gray,m,n);%原始roi
else
roi_l = roipoly( gray,roi_Pl(:,1) ,roi_Pl(:,2)); %感兴趣区域为白色
roi_r= roipoly( gray,roi_Pr(:,1) ,roi_Pr(:,2)); %感兴趣区域为白色
roi=bitor( roi_l , roi_r );
end
gray(:,1:70)=0;
gray(180:240,360:480)=0;
gz = medfilt2( gray); %对躁声中值滤波
SE = strel('square',20);
Fs=imtophat(gz,SE);%图像
F=im2bw(Fs,graythresh(Fs(180:360,:))); %大津法局部二值化
% figure;imshow(F(141:end,:));
f=bitand(F,roi);%位操作,取出车道线
%figure;imshow(f);
% subplot 121;imshow(Fs)
% subplot 122;imshow(f);
SE = strel('square',5);
f=imclose(f,SE);%图像膨胀后腐蚀处理,消除空洞
% figure,imshow(f);
if flag==1
[B,L1] = bwboundaries(f,'noholes');
width=30;%车道宽度极大值
for i=1:length(B)
X=[];
X=unique(B{i}(:,1));%所有的不重复X坐标
Y=mean(B{i}(:,2));
if abs(Y-240)>155 &&mean(X)<200%检测平均X是否距中轴线(80/240)
for j=1:length(X)
IndexY=find(B{i}(:,1)==X(j));
f(X(j),min(B{i}(IndexY,2)):max(B{i}(IndexY,2)))=0;
end
else
for j=1:length(X)
IndexY=find(B{i}(:,1)==X(j));
wide=max(B{i}(IndexY,2))-min(B{i}(IndexY,2));
if wide>width
f(X(j),min(B{i}(IndexY,2)):max(B{i}(IndexY,2)))=0;
end
end
end
end
end
imshow(f)
L = bwlabeln(f,8);
S = regionprops(L, 'Area');
area=sort([S.Area],'descend');
G= ismember(L, find([S.Area] >=20));
imshow(G(171:end,:));
%% 检测是否为初始帧
%加入扫描起始点,图像并非严格的以中轴线分为左右两区域
if flag==1
middle=240;
else
middle=floor(0.5*roi_Pr(1,1)+0.5*roi_Pl(1,1));
if middle >350|middle<100
middle=240;
end
end
[X,X1,Y,Y1]=lineGet(G,240);%
data=[X;Y];
data1=[X1;Y1];
% 系统状态判断,flag=1进入初始帧,flag=2进入跟踪。flag=3正常跟踪
if countL==5 | countR==5
flag=1;
elseif countL==0 & countR==0
flag=2;
else
flag=3
end
if flag==1
if length(data) > 10
[inliers1, bestParameter1, bestParameter2]=ransac(data);
countL=0;%countL作为车道线提取的标志
slopeL = bestParameter1;
xAxis1 = min(inliers1):max(inliers1);
yAxis1 = bestParameter1*xAxis1 + bestParameter2;
bL= bestParameter2;
FLAG=1;%初始帧标志
xL(times,1:2)=[slopeL,bL];%初始帧的测量值作为输入
else
countL=5;
end
if length(data1) > 10
[inliers2, bestParameter3, bestParameter4]=ransac(data1);
countR=0; slopeR = bestParameter3;
bR= bestParameter4;
xR(times,1:2)=[slopeR,bR];%以上一帧的校正值作为输入。
xAxis2 = min(inliers2):max(inliers2);
yAxis2 = bestParameter3*xAxis2 + bestParameter4;
else
countR=5;
end
else%kalman有效则进行最小二乘拟合
if length(data) > 20
pL=polyfit(X,Y,1);
slopeL = pL(1);
bL= pL(2);
flag=3;
FLAGL=1;
else%未检测到车道线
slopeL=0;
bL=0;
FLAGL=0;
end
if length(data1) > 20
pR=polyfit(X1,Y1,1);
slopeR=pR(1)
bR=pR(2);
flag=3;
FLAGR=1;
else
slopeR=0
bR=0;
FLAGR=0;
end
FLAG=0;
end
% 初始帧拟合结果检测 flag=1进入初始帧,flag=2进入跟踪。flag=3正常跟踪
if countL==5 | countR==5
flag=1;
elseif countL==0 & countR==0
flag=2;
else
flag=3;
end
% imshow(G)
% hold on
% plot(xAxis1,yAxis1,xAxis2,yAxis2,'b-','LineWidth',4)
% plot([X,X1],[Y,Y1],'.')
% tic calman程序 时间已过 0.017254 秒。
%% 进入卡尔曼
if flag~=1
xp_left=A*xL(times,:)'; %先验状态估计方程
PP_left = A*P_left*A' + Q ; %PP 先验误差协方差矩阵 P是上一时刻的误差协方差阵 Q 过程噪声协方差矩阵 A、Q已知
K_left = PP_left*H'*inv(H*PP_left*H'+R); %K增益矩阵 PP先验误差协方差阵 R 测量噪声协方差阵 H 观测状态转移矩阵 H、R已知
xL(times+1,:) = (xp_left + K_left*([slopeL,bL]' - H*xp_left))';%k时刻的状态估计值 xp是上一时刻的先验状态估计
P_left = (eye(4)-K_left*H)*PP_left ;%k时刻的误差协方差阵
%卡尔曼预测值和测量值作比较,如果符合误差,则测量值有效。在theta1-5量测值有效。超过5则使用预测值,则认为参数突变,系统初始化
if abs(atan(xL(times,1))-atan(slopeL)) < 3*3.14/180 &slopeL~=0
xL(times+1,1)=slopeL;
xL(times+1,2)= bL;
% disp('测量值有效')
elseif abs(atan(xL(times,1))-atan(slopeL)) < 7*3.14/180
slopeL= xL(times+1,1);
bL=xL(times+1,2);
disp('修正值有效')
elseif slopeL==0
% 若未检测到车道线,则以上一帧的输出,作为当前帧的车道参数。并更改卡尔曼滤波器本次的预测值
slopeL= xL(times,1);%修正当前帧
bL=xL(times,2);
xL(times+1,1)=slopeL;%更改预测
xL(times+1,2)= bL;
countL=countL+1;%未检测出车道线,标志位加1
end
end
if flag~=1
%% right卡尔曼观测
xp_right=A*xR(times,:)'; %先验状态估计方程
PP_right = A*P_right*A' + Q ; %PP 先验误差协方差矩阵 P是上一时刻的误差协方差阵 Q 过程噪声协方差矩阵 A、Q已知
K_right = PP_right*H'*inv(H*PP_right*H'+R); %K增益矩阵 PP先验误差协方差阵 R 测量噪声协方差阵 H 观测状态转移矩阵 H、R已知
xR(times+1,:) = (xp_right + K_right *([slopeR,bR]' - H*xp_right))';%k时刻的状态估计值 xp是上一时刻的先验状态估计
P_right = (eye(4)-K_right*H)*PP_right ;%k时刻的误差协方差阵
if abs(atan(xR(times,1))-atan(slopeR)) < 3*3.14/180 &slopeR~=0
xR(times+1,1)=slopeR;
xR(times+1,2)= bR;
% disp('测量值有效')
elseif abs(atan(xR(times,1))-atan(slopeR)) < 5*3.14/180 &slopeR~=0
slopeR= xR(times+1,1);
bR=xR(times+1,2);
disp('修正值有效')
elseif slopeR==0
% 若未检测到车道线,则以上一帧的输出,作为当前帧的车道参数。并更改卡尔曼滤波器本次的预测值
slopeR= xR(times,1);%修正当前帧
bR=xR(times,2);
xR(times+1,1)=slopeR;%更改预测
xR(times+1,2)= bR;
countR=countR+1
end
end
% 系统状态判断,flag=1进入初始帧,flag=2进入跟踪。flag=3正常跟踪
if countL==5 | countR==5
flag=1
countR
elseif countL==0 & countR==0
flag=2;
else
flag=3;
end
%----------------------calman跟踪车道线--------------------------------
%%left卡尔曼观测,如果参数突变,或者连续5帧没有车道线。则认为跟踪无效。
% theta变化超过阈值,则上一帧的预测值作为输入,
%% 5次连续没有检测到车道线,calman系数初始化
if flag==1 | (abs(atan(xL(times,1))-atan(slopeL)) >10*3.14/180 & slopeL~=0 )| (abs(atan(xR(times,1))-atan(slopeR)) > 10*3.14/180 & slopeR~=0 )
R=[(X_left(1)*0.2)^2 0;0 (X_left(2)*0.1)^2];%测量噪声协方差矩阵
Q=[(X_left(1)*0.2)^2 0 0 0;0 (X_left(2)*0.1)^2 0 0;0 0 (X_left(3)*0.2)^2 0;0 0 0 (X_left(4)*0.1)^2]; %过程噪声协方差矩阵 已知
P_left=[10 0 0 0; 0 25 0 0; 0 0 4 0; 0 0 0 4];%误差协方差矩阵 任意给的初值
P_right=[10 0 0 0; 0 25 0 0; 0 0 4 0; 0 0 0 4];%误差协方差矩阵 任意给的初值
sprintf('第%dcalman帧照片初始化',times+1)
if flag==1
disp('参数突变')
end
countL=5;countR=5;%进入初始化
end
t(times)=toc;
%-------------------------更新roi区域--------------------------------
if flag~=1
roi_Pl(1,:)= [(I_W/2- bL)/slopeL-1.5*offset,I_W/2];
roi_Pl(2,:)= [(I_W/2- bL)/slopeL+1.5*offset,I_W/2];
roi_Pl(3,:)= [(I_W- bL)/slopeL+1.5*offset,I_W];
roi_Pl(4,:)= [(I_W- bL)/slopeL-1.5*offset,I_W];
roi_Pr(1,:)=[(I_W/2- bR)/slopeR-1.5*offset,I_W/2];
roi_Pr(2,:)=[(I_W/2- bR)/slopeR+1.5*offset,I_W/2];
roi_Pr(3,:)= [(I_W- bR)/slopeR+1.5*offset,I_W];
roi_Pr(4,:)= [(I_W- bR)/slopeR-1.5*offset,I_W];
end
%% 在图像上画出所有的东西
%------------------在每个框架上绘制外推线、梯形图和方向图-----------------
%tic
% figure(times)
imshow(G);
hold on
% plot(roi_Pl([1:4,1],1) ,roi_Pl([1:4,1],2),roi_Pr([1:4,1],1) ,roi_Pr([1:4,1],2), 'LineWidth',2,'Color','red')
%检测是否为初始帧,初始帧为蓝色标记。
if ~FLAG
%如果这一帧左侧的车道线为空,则打印上组信息
if FLAGL
xAxis1 =min(X):max(X);
yAxis1 = slopeL*xAxis1+bL;
end
if FLAGR
xAxis2 =min(X1):max(X1);
yAxis2 =slopeR*xAxis2+bR;
end
plot(xAxis1,yAxis1,xAxis2,yAxis2, 'LineWidth',3,'Color','R');
else
plot(xAxis1,yAxis1,xAxis2,yAxis2, 'LineWidth',5,'Color','B');
end
% plot(xAxis1,yAxis1,xAxis2,yAxis2, 'LineWidth',3,'Color','red');
% imshow(strain_image,'border','tight','initialmagnification','fit');
% set (gcf,'Position',[0,0,500,500]);
axis normal;
% title(awarn);
% saveas(gca,['test' num2str(times) '.bmp']);
frame = getframe(gcf);
% writeVideo(writerObj,frame);
hold off
% %%转换为可以直接输出的格式(这会使图像丢失)
% %如果要制作彩色的图像,你只能把生成的彩色图像单独制作(使用其他软件)
% im=frame2im(frame);
% [I,map]=rgb2ind(im,256);
% if times==1
% imwrite(I,map,pic_name,'gif','loopcount',inf)
% else
% imwrite(I,map,pic_name,'gif','writemode','append')
% end
end
%close(writerObj);