clc,clear,close all;
% 1.指定路径地址
filelistfront = dir('D:\temp0507front\*.txt');
filelistrear = dir('D:\temp0507rear\*.txt');
k1 = size(filelistfront);
k2 = size(filelistrear);
DataPostfrontA = [];
% 2.绘制车辆轮廓
% 2.1车辆固有参数
vehicleLen = 4.4456;
vehicleWid = 1.594;
FrontEdge2Center = 3.54;
RearEdge2Center = 0.905;
SideEdge2Center = 0.9275;
vehicleBody = [FrontEdge2Center -RearEdge2Center -RearEdge2Center FrontEdge2Center FrontEdge2Center;
SideEdge2Center SideEdge2Center -SideEdge2Center -SideEdge2Center SideEdge2Center;
1 1 1 1 1];
% 2.2车辆运动学参数,车辆中心在后轴
plot(vehicleBody(1,:),vehicleBody(2,:),'LineWidth',3);
hold on;
% 3.超声探测点绘制
% 3.1 车头数据绘制
for i=1:k1
namefront = filelistfront(i).name;
full_pathfront = fullfile('D:\temp0507front\',namefront);
% 载入数据
DataPrefront = load(full_pathfront);
% 判断并添加数据到处理后文件
index = find(DataPrefront(:,1)==0);
DataPostfront1 = DataPrefront(index,2:3);
index = find(DataPrefront(:,4)==0);
DataPostfront2 = DataPrefront(index,5:6);
index = find(DataPrefront(:,7)==0);
DataPostfront3 = DataPrefront(index,8:9);
index = find(DataPrefront(:,10)==0);
DataPostfront4 = DataPrefront(index,11:12);
% 拼接提取后数据
DataPostfront = [DataPostfront1;DataPostfront2;DataPostfront3;DataPostfront4];
[m,n] = size(DataPostfront);
DataPostfrontA = xlswrite('DataPostfrontA.xlsx',DataPostfront);
% 绘图
plot(DataPostfront(:,1),DataPostfront(:,2),'o')
hold on;
end
% 3.2 车尾数据绘制
for i=1:k2
namerear = filelistrear(i).name;
full_pathrear = fullfile('D:\temp0507rear\',namerear);
% 载入数据
DataPrerear = load(full_pathrear);
% 判断并添加数据到处理后文件
index = find(DataPrerear(:,13)==0);
DataPostrear5 = DataPrerear(index,14:15);
index = find(DataPrerear(:,16)==0);
DataPostrear6 = DataPrerear(index,17:18);
index = find(DataPrerear(:,19)==0);
DataPostrear7 = DataPrerear(index,20:21);
index = find(DataPrerear(:,22)==0);
DataPostrear8 = DataPrerear(index,23:24);
% 拼接提取后数据
DataPostrear = [DataPostrear5;DataPostrear6;DataPostrear7;DataPostrear8];
% 绘图
plot(DataPostrear(:,1),DataPostrear(:,2),'o')
hold on;
end
% % 绘制超声探头位置
% UltrasonicPoint = xlsread('D:\UltrasonicPoint.xlsx');
% plot(UltrasonicPoint(:,1),UltrasonicPoint(:,2),'c','Markersize',7);
% 4.绘制0.2*0.2的网格
grid on;
set(gca,'xtick',-13:0.2:13);
set(gca,'ytick',-5.1:0.2:5.1);
hold off;