PCL：点云数据分割，分离地面

展示了如何在三维激光雷达数据中检测地平面和发现附近的障碍物。

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

结果如下图：

1. 绿色部分为使用RANSAC(随机采样一致)算法检测和匹配到的地面平面。
2. 红色部分为半径15米内的障碍物
3. 蓝色为未标记的点

去除蓝色和绿色部分如图：