激光雷达点云分割和障碍物检测的MATLAB实现
激光雷达点云分割和障碍物检测的MATLAB实现
- MATLAB处理点云工具箱
- 直接放上我的代码
- 结果
- 交流
MATLAB处理点云工具箱
从MATLAB2015a开始就有了一些处理激光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库，遇到困难，希望能够和大佬交流！