非铺装路面点云模型仿真生成方法
基本原理
- 非铺装路面点云往往十分粗糙，及时是较为平坦的路面，由于沙砾等，点云存在较多噪声，因此在仿真生成非铺装路面点云时，以随机噪声来表达路面的粗糙。
- 通过在点云的Z,Y方向上添加一个随机白噪声，来表示较平坦路面
rand_y{i}=0.05*rand(length(x),1);
rand_z{i}=0.05*rand(length(x),1);
- 此外，非铺装路面存在较多的凹槽，隆起，它们在实际道路的位置是随机分布的，且凹槽或隆起的深度/高度，平缓程度，范围大小都各不相同，为合理方便的表达这些特征，引入正态分布函数（高斯函数）来进行表达。
\(f(x)=\frac{1}{\sqrt{2\cdot\pi}} \cdot e^{-{\frac{x-\mu}{2\sigma}}}\)
图像关于\(x=\mu\)对称
在\(x=\mu\)处有最大值，可方便的表征凹槽或隆起的深度/高度
在\(x= \mu \pm \sigma\)为拐点，可方便的表征凹槽或隆起的范围大小
固定\(\sigma\),改变\(\mu\),图像沿x轴平移而不改变形状，可方便的表征凹槽或隆起的位置
固定\(\mu\),改变\(\sigma\)，改变图像形状,可方便的表征凹槽或隆起的位置平缓程度
A=5
B=4
C=1
f = @(x,y) A*exp(-(x-B).^2/C.^2)-y
data_f=fimplicit(f,[-2.5,10,0,10.5]);
代码实现
- 创建用于生成道路点云\(xyz\)，法向量\(nx,ny,nz\)，以及各点对应标签\(lables\)的函数\(road\_point\_lables\)
- 函数输入为生成路面宽度范围，长度范围:\([x\_min,x\_max,y\_min,y\_max]\);步长：\(step\);特征数量：\(num\);最大幅值：\(A\).
- 函数输出为包含道路点云\(xyz\)，法向量\(nx,ny,nz\)，以及各点对应标签\(lables\)的路面点云
function [outputArg1] = road_point_lables(x_min,x_max,y_min,y_max,step,num,A)
% 幅值A 正整数
% X坐标范围
% Y坐标范围
% 采样步长
% 特征数量num
x=x_min:step:x_max;
y=y_min:step:y_max;
% 添加随机白噪声
len_y=length(y);
for i=1:len_y
rand_y{i}=0.05*rand(length(x),1);
rand_z{i}=0.05*rand(length(x),1);
Y{i}=y(i)*ones(length(x),1)+rand_y{i};
labels{i}=zeros(length(x),1);
end
randIndex_A = -A+2*A*rand(num,1);% 凸起顶点幅值A
randIndex_y = randperm(len_y-20,num)+10;%凸起顶点Y位置
randIndex_rangx = 0.2*(rand(1,num))+0.3;%凸起X方向范围C
randIndex_placex = x_min+0.8+5*rand(num,1);%凸起X方向幅值位置B
randIndex_rangy_min = fix(randIndex_y-(randIndex_rangx)/step);
randIndex_rangy_max = fix(randIndex_y+(randIndex_rangx)/step);
% 将随机选择的数据通过高斯函数进行表示
for i=1:length(randIndex_y)
rand_z{randIndex_y(i)}=rand_z{i}+(randIndex_A(i).*...
exp(-(x-randIndex_placex(i)).*(x-randIndex_placex(i))/(randIndex_rangx(i)^2)))';
end
% 插值
num_1 = randIndex_y-randIndex_rangy_min;%插值个数
num_2 = randIndex_rangy_max-randIndex_y;%插值个数
for i=1:num
xq_1{i}= 0:1/num_1(i):1;%变小比例
xq_2{i}= 0:1/num_2(i):1;%变小比例
end
% 随机选定线的区域数据
for i=1:num
for j=1:num_1(i)
size=length(xq_1{i});
rand_z{randIndex_y(i)-j}=rand_z{randIndex_y(i)-j}+xq_1{i}(1,size-j)* rand_z{randIndex_y(i)};
end
end
for i=1:num
for j=1:num_2(i)
size=length(xq_2{i});
rand_z{randIndex_y(i)+j}=rand_z{randIndex_y(i)+j}+xq_2{i}(1,size-j)* rand_z{randIndex_y(i)};
end
end
% 打标签
k1=0.8;
k2=1.2;
for i=1:num
for j=1:num_1(i)
if randIndex_A(i)>=0.05&& randIndex_A(i)<0.15
min_a = fix(k1*(randIndex_placex(i)-randIndex_rangx(i))/step);
max_a = fix(k2*(randIndex_placex(i)+randIndex_rangx(i))/step);
labels{randIndex_y(i)-j+1}(min_a:max_a,1) = 1;
elseif randIndex_A(i)>=0.15&& randIndex_A(i)<0.25
min_a = fix(k1*(randIndex_placex(i)-randIndex_rangx(i))/step);
max_a = fix(k2*(randIndex_placex(i)+randIndex_rangx(i))/step);
labels{randIndex_y(i)-j+1}(min_a:max_a,1) = 2;
elseif randIndex_A(i)>=0.25&& randIndex_A(i)<0.35
min_a = fix(k1*(randIndex_placex(i)-randIndex_rangx(i))/step);
max_a = fix(k2*(randIndex_placex(i)+randIndex_rangx(i))/step);
labels{randIndex_y(i)-j+1}(min_a:max_a,1) = 3;
elseif randIndex_A(i)>=0.35
min_a = fix(k1*(randIndex_placex(i)-randIndex_rangx(i))/step);
max_a = fix(k2*(randIndex_placex(i)+randIndex_rangx(i))/step);
labels{randIndex_y(i)-j+1}(min_a:max_a,1) = 4;
elseif randIndex_A(i)>=-0.15&& randIndex_A(i)<-0.05
min_a = fix(k1*(randIndex_placex(i)-randIndex_rangx(i))/step);
max_a = fix(k2*(randIndex_placex(i)+randIndex_rangx(i))/step);
labels{randIndex_y(i)-j+1}(min_a:max_a,1) = 5;
elseif randIndex_A(i)>=-0.25&& randIndex_A(i)<-0.15
min_a = fix(k1*(randIndex_placex(i)-randIndex_rangx(i))/step);
max_a = fix(k2*(randIndex_placex(i)+randIndex_rangx(i))/step);
labels{randIndex_y(i)-j+1}(min_a:max_a,1) = 6;
elseif randIndex_A(i)>=-0.35&& randIndex_A(i)<-0.25
min_a = fix(k1*(randIndex_placex(i)-randIndex_rangx(i))/step);
max_a = fix(k2*(randIndex_placex(i)+randIndex_rangx(i))/step);
labels{randIndex_y(i)-j+1}(min_a:max_a,1) = 7;
elseif randIndex_A(i)<-0.35
min_a = fix(k1*(randIndex_placex(i)-randIndex_rangx(i))/step);
max_a = fix(k2*(randIndex_placex(i)+randIndex_rangx(i))/step);
labels{randIndex_y(i)-j+1}(min_a:max_a,1) = 8;
end
end
for j=1:num_2(i)
if randIndex_A(i)>=0.05&& randIndex_A(i)<0.15
min_a = fix(k1*(randIndex_placex(i)-randIndex_rangx(i))/step);
max_a = fix(k2*(randIndex_placex(i)+randIndex_rangx(i))/step);
labels{randIndex_y(i)+j-1}(min_a:max_a,1) = 1;
elseif randIndex_A(i)>=0.15&& randIndex_A(i)<0.25
min_a = fix(k1*(randIndex_placex(i)-randIndex_rangx(i))/step);
max_a = fix(k2*(randIndex_placex(i)+randIndex_rangx(i))/step);
labels{randIndex_y(i)+j-1}(min_a:max_a,1) = 2;
elseif randIndex_A(i)>=0.25&& randIndex_A(i)<0.35
min_a = fix(k1*(randIndex_placex(i)-randIndex_rangx(i))/step);
max_a = fix(k2*(randIndex_placex(i)+randIndex_rangx(i))/step);
labels{randIndex_y(i)+j-1}(min_a:max_a,1) = 3;
elseif randIndex_A(i)>=0.35
min_a = fix(k1*(randIndex_placex(i)-randIndex_rangx(i))/step);
max_a = fix(k2*(randIndex_placex(i)+randIndex_rangx(i))/step);
labels{randIndex_y(i)+j-1}(min_a:max_a,1) = 4;
elseif randIndex_A(i)>=-0.15&& randIndex_A(i)<-0.05
min_a = fix(k1*(randIndex_placex(i)-randIndex_rangx(i))/step);
max_a = fix(k2*(randIndex_placex(i)+randIndex_rangx(i))/step);
labels{randIndex_y(i)+j-1}(min_a:max_a,1) = 5;
elseif randIndex_A(i)>=-0.25&& randIndex_A(i)<-0.15
min_a = fix(k1*(randIndex_placex(i)-randIndex_rangx(i))/step);
max_a = fix(k2*(randIndex_placex(i)+randIndex_rangx(i))/step);
labels{randIndex_y(i)+j-1}(min_a:max_a,1) = 6;
elseif randIndex_A(i)>=-0.35&& randIndex_A(i)<-0.25
min_a = fix(k1*(randIndex_placex(i)-randIndex_rangx(i))/step);
max_a = fix(k2*(randIndex_placex(i)+randIndex_rangx(i))/step);
labels{randIndex_y(i)+j-1}(min_a:max_a,1) = 7;
elseif randIndex_A(i)<-0.35
min_a = fix(k1*(randIndex_placex(i)-randIndex_rangx(i))/step);
max_a = fix(k2*(randIndex_placex(i)+randIndex_rangx(i))/step);
labels{randIndex_y(i)+j-1}(min_a:max_a,1) = 8;
end
end
end
Data=[];
for i=1:len_y
Data=[Data;x',Y{i},rand_z{i}];
end
Label=[];
for i=1:len_y
Label=[Label;labels{i}];
end
ptcloud=pointCloud(Data(:,1:3));
normals = pcnormals(ptcloud);
Data=[Data,normals,Label];
rowrank = randperm(length(Data)); % size获得a的行数，randperm打乱各行的顺序
Data = Data(rowrank,:); % 按照rowrank重新排列各行，注意rowrank的位置
outputArg1=Data;
end
- 点云由于其易于平移，旋转等操作，选择通过生成小范围的点云，再通过点云平移来拼接较大的路面
clc
clear
close
%% 设置路面参数
% 幅值A:正整数 % X坐标范围
% Y坐标范围 % 采样步长：step
% 特征数量: num
x_min=0;
x_max=8;
y_min=0;
y_max=2;
step=0.05;
num=1;
A=0.45;
%% 生成[x_min x_max y_min y_max]范围内点云
K=10;
num_size=K*1000;
%
theta = 0;%旋转角度设为0
R = [cosd(theta) sind(theta) 0;
-sind(theta) cosd(theta) 0;
0 0 1];
move_y=[2 4 6 8 10 12 14 16 18];
for i=1:K-1
t{i} = [0 move_y(i) 0];
T{i} = rigid3d(R,t{i});
end
%
tic;
bar = waitbar(0,'开始生成路面点云数据'); % waitbar显示进度条
for i=1:num_size
str=['😊😊😊路面点云数据生成中：',num2str(100*i/num_size),'%'];
waitbar(i/num_size,bar,str) % 更新进度条bar，配合bar使用
% 通过创建的road_point_lables函数，得到[x_min x_max y_min y_max]
% 带有点云xyz坐标，法向量，分类标签的数据
[ptcloud_Data] = road_point_lables(x_min,x_max,y_min,y_max,step,num,A);
ptcloud_0 = pointCloud(ptcloud_Data(:,1:3));%输出点云xyz数据
inf = mod(i,K);
if inf==1
Ptcloud_1 = pctransform(ptcloud_0,T{inf});
Ptcloud_1 = [Ptcloud_1.Location,ptcloud_Data(:,4:7)];
elseif inf==2
Ptcloud_2 = pctransform(ptcloud_0,T{inf});
Ptcloud_2 = [Ptcloud_2.Location,ptcloud_Data(:,4:7)];
elseif inf==3
Ptcloud_3 = pctransform(ptcloud_0,T{inf});
Ptcloud_3 = [Ptcloud_3.Location,ptcloud_Data(:,4:7)];
elseif inf==4
Ptcloud_4 = pctransform(ptcloud_0,T{inf});
Ptcloud_4 = [Ptcloud_4.Location,ptcloud_Data(:,4:7)];
elseif inf==5
Ptcloud_5 = pctransform(ptcloud_0,T{inf});
Ptcloud_5 = [Ptcloud_5.Location,ptcloud_Data(:,4:7)];
elseif inf==6
Ptcloud_6 = pctransform(ptcloud_0,T{inf});
Ptcloud_6 = [Ptcloud_6.Location,ptcloud_Data(:,4:7)];
elseif inf==7
Ptcloud_7 = pctransform(ptcloud_0,T{inf});
Ptcloud_7 = [Ptcloud_7.Location,ptcloud_Data(:,4:7)];
elseif inf==8
Ptcloud_8 = pctransform(ptcloud_0,T{inf});
Ptcloud_8 = [Ptcloud_8.Location,ptcloud_Data(:,4:7)];
elseif inf==9
Ptcloud_9 = pctransform(ptcloud_0,T{inf});
Ptcloud_9 = [Ptcloud_9.Location,ptcloud_Data(:,4:7)];
elseif inf==0
Ptcloud_0 = ptcloud_Data;
Ptcloud = [Ptcloud_0;Ptcloud_1;Ptcloud_2;Ptcloud_3;Ptcloud_4;
Ptcloud_5;Ptcloud_6;Ptcloud_7;Ptcloud_8;Ptcloud_9;];
rowrank = randperm(length(Ptcloud)); % size获得a的行数，randperm打乱各行的顺序
Ptcloud = Ptcloud(rowrank,:); % 按照rowrank重新排列各行，注意rowrank的位置
path='D:\tsy\数据集生成\datagenerate_0602\txtdata_1';
name=['lidar_',num2str(i/K),'.txt'];
path_name=fullfile(path,name);
save (path_name,'Ptcloud','-ascii')
end
end
toc;
** 实现效果 **
