激光雷达点云分割和障碍物检测的MATLAB实现

MATLAB处理点云工具箱

从MATLAB2019a开始就有了一些处理激光3D点云的函数比如pcfitplane()拟合地面等。在官网中可以查看其具体的使用方法，https://www.mathworks.com/help/vision/ref/pcfitplane.html?action=changeCountry&s_tid=gn_loc_drop。
随着无人驾驶技术的发展，更多优秀的方法贡献出来供研究使用。从MATLAB2019a添加了视觉处理工具箱，丰富了激光点云的处理。详见
https://www.ilovematlab.cn/thread-567484-1-1.html
https://ww2.mathworks.cn/products/computer-vision.html
包括了点云降采样，点云分割为簇等高级方法。详见
https://ww2.mathworks.cn/help/vision/segment-downsample-and-denoise-point-clouds.html

直接放上我的代码

在我的研究中也参考了很多前人的方法，其中这位作者的方法对我帮助较大，促成我的方法产生，详见https://blog.csdn.net/qq_33801763/article/details/79152376。

clear;clc;close all;
% 主要目的:分割出地面，并对障碍物聚类
%
%% 
for img_idx = 181:446
    fid = fopen(sprintf('E:/work/数据/KITTI/2011_09_26_drive_0009_sync/2011_09_26/2011_09_26_drive_0009_sync/velodyne_points/data/%010d.bin',img_idx),'rb');
    velo = fread(fid,[4 inf],'single')';
    velo = velo(:,1:3); % 取前三列x y z
%     a = pointCloud(velo);
    points = transcsv(velo);
    a = pointCloud(points);
    pcloud(img_idx-180).ptCloud = a;
    fclose(fid);
end
%% 

%%选择要显示的点云
% 为了突出周围的环境, 车辆, 集中在一个地区的利益, 横跨20米左右的车辆, 40 米的前面和后面的车辆。
pc = pcloud(1).ptCloud;

% 设置感兴趣区域(单位米)
xBound  = 40; 
yBound  = 20; 
xlimits = [-xBound, xBound];
ylimits = [-yBound, yBound];
zlimits = pc.ZLimits;

player = pcplayer(xlimits, ylimits, zlimits);

% 裁剪指定范围内的点云
indices = find(pc.Location(:, :, 2) >= -yBound ...
             & pc.Location(:, :, 2) <=  yBound ...
             & pc.Location(:, :, 1) >= -xBound ...
             & pc.Location(:, :, 1) <=  xBound);


% 将裁剪到的点显示出来
pc = select(pc, indices,'OutputSize','full');
view(player, pc);


%% 分割地平面和附近障碍物
% 找到地面平面并移除地面平面点。使用RANSAC算法检测和匹配地面平面。
% 平面的法线方向应大致沿 Z 轴向上指向。所有 inlier 点必须在地面平面的20厘米以内。

maxDistance = 0.3; % in meters
referenceVector = [0, 0, 1];
[mode, inPlanePointIndices, outliers] = pcfitplane(pc, maxDistance, referenceVector);

%%
% 标出地平面点。
pcGround = select(pc, inPlanePointIndices);

% 选择不属于地平面一部分的点。
pcWithoutGround = select(pc, outliers);

%% 检索半径在20米以内的点, 并将它们标记为障碍物。
sensorLocation   = [0,0,0]; % 将激光雷达传感器放在坐标系的中心
radius           = 40;      % in meters

nearIndices  = findNeighborsInRadius(pcWithoutGround, sensorLocation, radius);    
nearPointIndices = outliers(nearIndices);

% 标记障碍物点
pcObstacle = select(pc,nearPointIndices,'OutputSize','full');

% Cluster the points,ignoring the ground plane points.
distThreshold = 0.5;
[labels,numClusters] = pcsegdist(pcObstacle, distThreshold);

% Add an additional label for the ground plane.
numClusters = numClusters + 1;
labels(inPlanePointIndices) = numClusters;

% %% 将所有标记的点绘制到点云播放器中。使用前面设置的数字颜色标签。
labelColorIndex = labels+1;
pcshow(pc.Location,labelColorIndex)
colormap([hsv(numClusters);[0 0 0]])
title('Point Cloud Clusters')

colormap(player.Axes, [hsv(numClusters);[0 0 0]])
points1 = pc.Location;
view(player, points1, labelColorIndex);
title(player.Axes, 'Segmented Point Cloud');

结果

原始点云

地面和障碍区分割

交流

本人是刚入门，有错误之处望各位大佬纠正，并不吝赐教！
目前正在学习使用pcl库，遇到困难，希望能够和大佬交流！