MATLAB 实现点云累计-坐标系转换-目标范围点云提取（附代码与代码注释）

Posted 2023-02-10 tsytian

1. 主要流程介绍：由于单帧点云数据稀疏，通过点云累计方法，可将多帧点云数据配准为一帧；由于在本次实验时将激光雷达倾斜安装，激光雷达坐标系发生变化，为将其转换至倾斜前坐标，需进行坐标系的转换；最后将感兴趣区域内数据进行提取。

2. 点云累计

原始pcap点云数据：

本次实验读取文件为VLP-16激光雷达采集的pcap文件，在matlab中可通过工具箱函veloReader = velodyneFileReader(filename,"VLP16")进行文件读取，再设定一个感兴趣区域roi，只对区域内的数据进行保存，减小计算量；通过indices_11,i = findPointsInROI(ptCloud_original1,i,roi);
ptCloud_interest1,i=select(ptCloud_original1,i,indices_11,i);对roi区域内的点云进行逐帧选择并保存至ptCloud_interest,将第一帧点云设置为参考点云，第二帧为待处理点云;对点云进行下采样处理，定义下采样网格大小与采样比例，通过fixed = pcdownsample(ptCloudRef, \'random\', percentage);moving = pcdownsample(ptCloudCurrent, \'random\', percentage); % 下采样，实现；通过matlab工具箱中的ICP函数tform=pcregistericp(moving, fixed, \'Metric\',\'pointToPlane\',\'Extrapolate\', true); % matlab自带ICP算法，得到moving点云至fixed点云之间的坐标转换矩阵tformptCloudAligned = pctransform(ptCloudCurrent,tform); % 将第2帧点云通过求得的坐标转换矩阵进行转换实现点云配准，得到待处理点云与参考点云之间的坐标转换矩阵，再通过坐标转换函数实现坐标转换。最后通过点云拼接函数将两帧点云进行拼接，则完成了第一帧点云与第二帧点云的累计。后续同样原理，只要求得后续每帧点云与第1帧点云的坐标转换关系，如第三帧点云与第一帧点云坐标转换矩阵为第三帧点云与第二帧点云转换矩阵乘以第二帧点云与第一帧点云转换矩阵。

点云累计前：

点云累计后：

filename= \'D:\\leidashuji\\LIDAR_0515\\01.pcap\'; %文件位置    
veloReader = velodyneFileReader(filename,"VLP16"); % 读取.pcap文件
ptCloud_original=cell(1,veloReader.NumberOfFrames); % 定义储存原始点云数据的元组
roi = [-20 20 -20 40 -4 1]; % roi区域筛选
num=60;
for i=1:num%veloReader.NumberOfFrames
ptCloud_original1,i=readFrame(veloReader,i); % 依次读取每次采样周期的数据，将其保存为pointCloud格式
k=1;
% 选择感兴趣区域
indices_11,i = findPointsInROI(ptCloud_original1,i,roi);
ptCloud_interest1,i=select(ptCloud_original1,i,indices_11,i);
end
ptCloudRef = ptCloud_interest1; % 将第1帧点云定义为参考点云
ptCloudCurrent = ptCloud_interest2; % 将第2帧定义为待处理点云
figure
pcshow(ptCloudRef)
%% 下采样
gridSize = 0.5; %定义下采样网格大小
percentage=0.5;
fixed = pcdownsample(ptCloudRef, \'random\', percentage);
moving = pcdownsample(ptCloudCurrent, \'random\', percentage); % 下采样
tform = pcregistericp(moving, fixed, \'Metric\',\'pointToPlane\',\'Extrapolate\', true); % matlab自带ICP算法，得到moving点云至fixed点云之间的坐标转换矩阵tform
ptCloudAligned = pctransform(ptCloudCurrent,tform); % 将第2帧点云通过求得的坐标转换矩阵进行转换
mergeSize = 0.025;
ptCloudScene = pcmerge(ptCloudRef, ptCloudAligned, mergeSize);
accumTform = tform; 
figure
hAxes = pcshow(ptCloudScene);
title(\'Updated world scene\')
%% 设置轴属性以更快地渲染
hAxes.CameraViewAngleMode = \'auto\';
hScatter = hAxes.Children; 
    for i = 3:num%length(ptCloud_original) % 依次检索没帧点云
        ptCloudCurrent = ptCloud_interesti;% 将第i帧数据赋值给待处理点云 ptCloudCurrent
        fixed = moving; % 将前一帧的移动点云作为后一帧点云的参考点云
        moving = pcdownsample(ptCloudCurrent, \'gridAverage\', gridSize);% 将待处理点云作为移动点云
        % 应用CIP算法得到moving到fixed的坐标转换矩阵
        tform = pcregistericp(moving, fixed, \'Metric\',\'pointToPlane\',\'Extrapolate\', true);
        % 通过当前转换矩阵乘以前面累积的转换矩阵，得到当前帧转换至第一帧的坐标转换矩阵
        accumTform = affine3d(tform.T * accumTform.T);
        ptCloudAligned = pctransform(ptCloudCurrent, accumTform);
        % 更新全局累积的点云数据
        ptCloudScene = pcmerge(ptCloudScene, ptCloudAligned, mergeSize);
        hScatter.XData = ptCloudScene.Location(:,1);
        hScatter.YData = ptCloudScene.Location(:,2);
        hScatter.ZData = ptCloudScene.Location(:,3);
    end
figure
pcshow(ptCloudScene) 

3. 坐标转换：本次实验测得thetax= 23.46°，thetay =2°，thetaz = 0，代码中需转换为弧度格式

坐标转换后：

thetax = 23.46*pi/180;%
thetay =2*pi/180;
thetaz = 0;
rotx = [1 0 0; ...
      0 cos(thetax) -sin(thetax); ...
      0 sin(thetax)  cos(thetax)];
rotz=[cos(thetaz)  sin(thetaz) 0; ...
      -sin(thetaz) cos(thetaz) 0; ...
               0          0  1];
roty=[cos(thetay) 0 sin(thetay);
          0      1     0;
    -sin(thetay) 0 cos(thetay)];
trans = [0, 0, 0];
tform = rigid3d(roty*rotx*rotz,trans);
ptcloud_zuobiao=pctransform(ptCloudScene,tform);
figure
pcshow(ptcloud_zuobiao) 

4. 路面范围提取:路面的roi区域为[-1 0.7 2 4 -2 -0.5]，选择区域内数据进行保存，并将其ptCloud_road.Location（xyz坐标）写入lidar_15.csv文件中进行保存。

路面范围提取后：

roi = [-1 0.7 2 4 -2 -0.5]; % roi区域筛选
indices_2 = findPointsInROI(ptcloud_zuobiao,roi);
ptCloud_road=select(ptcloud_zuobiao,indices_2);
figure
pcshow(ptCloud_road)
csvwrite(\'lidar_15.csv\',ptCloud_road.Location )