展示了如何在三维激光雷达数据中检测地平面和发现附近的障碍物。
clear;clc;
%%
% for img_idx = 181:446
% fid = fopen(sprintf('D:/KITTI/data_set/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);
% pcloud(img_idx-180).ptCloud = a;
% fclose(fid);
% end
%%
load pcloud.mat
%%选择要显示的点云
% 为了突出周围的环境, 车辆, 集中在一个地区的利益, 横跨20米左右的车辆, 40 米的前面和后面的车辆。
pc = pcloud(60).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);
view(player, pc)
%% 分割地平面和附近障碍物
% 找到地面平面并移除地面平面点。使用RANSAC算法检测和匹配地面平面。
% 平面的法线方向应大致沿 Z 轴向上指向。所有 inlier 点必须在地面平面的20厘米以内。
maxDistance = 0.2; % in meters
referenceVector = [0, 0, 1];
[~, inPlanePointIndices, outliers] = pcfitplane(pc, maxDistance, referenceVector);
%%
% 将颜色标签附加到点云中的每个点。使用绿色显示地面平面和红色的障碍, 在10米的激光雷达传感器。
labelSize = [pc.Count, 1];
colorLabels = zeros(labelSize, 'single');
% 设置用于标记点的颜色表。
colors = [0 0 1; ... % 蓝色为未标记的点(0 0 1); 指定为[R，G，B]
0 1 0; ... % 绿色的为地面平面点(0 1 0)
1 0 0; ... % 红色的为障碍点(1 0 0)
0 0 0]; % 黑色的为自我车辆点(0 0 0)
blueIdx = 0; % 整个点云最初是蓝色的
greenIdx = 1;
redIdx = 2;
blackIdx = 3;
% 标出地平面点。
colorLabels(inPlanePointIndices) = greenIdx;
% 选择不属于地平面一部分的点。
pcWithoutGround = select(pc, outliers);
%% 检索半径在15米以内的点, 并将它们标记为障碍物。
sensorLocation = [0,0,0]; % 将激光雷达传感器放在坐标系的中心
radius = 15; % in meters
nearIndices = findNeighborsInRadius(pcWithoutGround, sensorLocation, radius);
nearPointIndices = outliers(nearIndices);
% 标记障碍物点
colorLabels(nearPointIndices) = redIdx;
%% 分割自我车辆
% 激光雷达通常安装在车辆的顶部, 数据可能包含车辆本身的点, 如屋顶和引擎盖。
% 这些都不是障碍物，但是最接近激光雷达传感器。在包含车辆的小半径内检索点。
% 使用这些点来形成一个轴对齐的边界立方体来表示自我车辆。
radius = 2; % in meters
nearIndices = findNeighborsInRadius(pcWithoutGround, sensorLocation, radius);
vehiclePointIndices = outliers(nearIndices);
pcVehicle = select(pcWithoutGround, nearIndices);
%% 形成一个包围立方体和标签的自我车辆点。
delta = 0.1;
selfCube = [pcVehicle.XLimits(1)-delta, pcVehicle.XLimits(2)+delta ...
pcVehicle.YLimits(1)-delta, pcVehicle.YLimits(2)+delta ...
pcVehicle.ZLimits(1)-delta, pcVehicle.ZLimits(2)+delta];
colorLabels(vehiclePointIndices) = blackIdx;
%% 将所有标记的点绘制到点云播放器中。使用前面设置的数字颜色标签。
colormap(player.Axes, colors)
points1 = pc.Location;
% points1(inPlanePointIndices, :) = [];
view(player, points1, colorLabels);
title(player.Axes, 'Segmented Point Cloud');
%% 处理点云序列
% for k = 2:length(pcloud)
% pc = pcloud(k).ptCloud;
%
% % Crop the data to ROI.
% indices = find(pc.Location(:, 2) >= -yBound ...
% & pc.Location(:, 2) <= yBound ...
% & pc.Location(:, 1) >= -xBound ...
% & pc.Location(:, 1) <= xBound);
% pc = select(pc, indices);
%
% colorLabels = zeros(pc.Count, 1, 'single'); % create label array
%
% % Fit the ground plane.
% [~, inPlanePointIndices, outliers] = pcfitplane(pc, maxDistance, referenceVector);
% colorLabels(inPlanePointIndices) = greenIdx;
%
% pcWithoutGround = select(pc, outliers);
%
% % Find the points corresponding to obstacles
% radius = 10; % in meters
% nearIndices = findNeighborsInRadius(pcWithoutGround, sensorLocation, radius);
% nearPointIndices = outliers(nearIndices);
%
% colorLabels(nearPointIndices) = redIdx;
%
% % Find the ego-vehicle points.
% nearIndices = findPointsInROI(pcWithoutGround, selfCube);
% vehiclePointIndices = outliers(nearIndices);
%
% colorLabels(vehiclePointIndices) = blackIdx;
%
% % Plot the results.
% view(player, pc.Location, colorLabels);
% end结果如下图：
- 绿色部分为使用RANSAC(随机采样一致)算法检测和匹配到的地面平面。
- 红色部分为半径15米内的障碍物
- 蓝色为未标记的点
去除蓝色和绿色部分如图：
