非铺装路面点云模型仿真生成方法

基本原理

非铺装路面点云往往十分粗糙，及时是较为平坦的路面，由于沙砾等，点云存在较多噪声，因此在仿真生成非铺装路面点云时，以随机噪声来表达路面的粗糙。
通过在点云的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;

** 实现效果 **

posted @ 2022-06-02 17:16 嶽过山丘阅读(154) 评论(0) 收藏举报

刷新页面返回顶部

tsytian

非铺装路面点云模型仿真生成方法

基本原理

代码实现

公告