基于matlab点云工具箱对点云进行处理四：对点云进行欧式聚类，并获得包围点云簇的外接凹多边形

基于matlab点云工具箱对点云进行处理四：对点云进行欧式聚类，并获得包围点云簇的外接凹多边形

步骤：

1. 读取velodyne数据包pcap文件内的点云数据
2. 使用pcdownsample函数对点云数据进行体素化采样，减少点云数量
3. 使用find函数对点云进行筛选
4. 使用pcdnoise去除点云内的噪声
5. 使用pcsegdist进行欧式聚类
6. 使用boundary获得外包顶点
7. 对顶点进行整理，输出

相关程序代码点这里https://download.youkuaiyun.com/download/rmrgjxeivt/59558971

基于matlab点云工具箱对点云进行处理一：去除地面，保留剩下的点https://blog.youkuaiyun.com/rmrgjxeivt/article/details/121830344
基于matlab点云工具箱对点云进行处理二：对点云进行欧式聚类，获得聚类后点云簇的外接矩形https://blog.youkuaiyun.com/rmrgjxeivt/article/details/121830919
基于matlab点云工具箱对点云进行处理三：对点云进行欧式聚类，使用三角剖分处理后获取点云簇的外接凸多边形https://blog.youkuaiyun.com/rmrgjxeivt/article/details/121831507
基于matlab点云工具箱对点云进行处理四：对点云进行欧式聚类，并获得包围点云簇的外接凹多边形https://blog.youkuaiyun.com/rmrgjxeivt/article/details/121831934

% 读取激光的PCAP文件
% 筛选感兴趣区域
% 播放筛选后的点云

veloReader = velodyneFileReader('2021-11-23-12-49-43_Velodyne-HDL-32-Data.pcap','VLP32c');


%% 设置感兴趣区域

vehPara.length = 5.5;
vehPara.width = 2.2;
vehPara.d = 2.3; % 轴距
vehPara.rearOverhang = 1; % 前悬
vehPara.rearOverhang = 1; % 后悬
vehPara.CG2Rear = 1.45; % 质心到后轴


insRegion = [-20 50 -10 10 0 2]; % 感兴趣区域[minX maxX minY maxY]
groundRegion = [-1, 0.2]; % 地面区域，z轴方向

xLimits = [insRegion(1), insRegion(2)];
yLimits = [insRegion(3), insRegion(4)];
zLimits = [insRegion(5), insRegion(6)]; % 原点在后轴中心，因此此处相对于轮芯高度

player = pcplayer(xLimits,yLimits,zLimits);

xlabel(player.Axes,'X (m)');
ylabel(player.Axes,'Y (m)');
zlabel(player.Axes,'Z (m)');

veloReader.CurrentTime = veloReader.StartTime + seconds(0.3);

disp(['frame数量',num2str(veloReader.NumberOfFrames)])

pause(2)

frameID = 1000;


while(hasFrame(veloReader) && player.isOpen() && (veloReader.CurrentTime < veloReader.EndTime))
ptCloudObj = readFrame(veloReader,frameID);
frameID

tic
lidarLo = [3.5 0 1.1 0 0 0];

% 取出XYZ
xTemp = ptCloudObj.Location(:,:,2)+lidarLo(1);
yTemp = -ptCloudObj.Location(:,:,1)+lidarLo(2);
zTemp = ptCloudObj.Location(:,:,3)+lidarLo(3);







pc = [xTemp(:) yTemp(:) zTemp(:) single(ptCloudObj.Intensity(:))];









% max(pc(:,1))
% min(pc(:,1))
% max(pc(:,2))

% 对地面的点进行范围筛选
zMin = groundRegion(1);
zMax = groundRegion(2);

pcObj = pointCloud(pc(:,1:3));
pcObj.Intensity = pc(:,4);

pcOutNum = 30000; % 输出的点云数量

objPointVeh = zeros(pcOutNum,4,'single');
objPointVeh(:,1) = single(insRegion(2));
objPointVeh(:,2) = single(insRegion(4));
objPointVeh(:,3) = single(insRegion(6));
objPointVeh(:,4) = single(0);


% tic
%% 降低点云密度 coder会报错
% gridStep = 0.05;
% pcObj_downSample = pcdownsample(pcObj,'gridAverage',gridStep); % 降低点云密度

maxNumPoints = 6;
pcObj_downSample = pcdownsample(pcObj,'nonuniformGridSample',maxNumPoints);

%     percentage = 0.3;
%     pcObj_downSample = pcdownsample(pcObj,'random',percentage);

%% 筛选感兴趣区域(单位米),并排除车身内部的点云
xLimits = [insRegion(1), insRegion(2)];
yLimits = [insRegion(3), insRegion(4)];
zLimits = [insRegion(5), insRegion(6)]; % 原点在后轴中心，因此此处相对于轮芯高度

indices = find((pcObj_downSample.Location(:, 2) >= yLimits(1) ...
    & pcObj_downSample.Location(:,2) <=  yLimits(2) ...
    & pcObj_downSample.Location(:,1) >=  xLimits(1) ...
    & pcObj_downSample.Location(:,1) <=  xLimits(2) ...
    & pcObj_downSample.Location(:,3) <=  zLimits(2) ...
    & pcObj_downSample.Location(:,3) >=  zLimits(1) ...
    & ~(pcObj_downSample.Location(:,1)<(vehPara.length-vehPara.rearOverhang) ...
    & pcObj_downSample.Location(:,1)>(-vehPara.rearOverhang) ...
    & pcObj_downSample.Location(:,2)<vehPara.width/2 ...
    & pcObj_downSample.Location(:,2)>-vehPara.width/2)));% 设置感兴趣的点云区域

if ~isempty(indices)
    pcObj_downSample = select(pcObj_downSample,indices);
    
    %% 去除噪声
    [pcObj_downSample,inlierIndices,~] = pcdenoise(pcObj_downSample);
    
    pcID_noNoise = 1:1:pcObj_downSample.Count;
    
    if ~isempty(inlierIndices)
        outlierIndices = [];
        
        if ~isempty(outlierIndices) % 非空才输出
            pcRemainObj = select(pcObj_downSample,pcID_out);
        else
            pcRemainObj = pcObj_downSample;
        end
    else
        pcRemainObj = pcObj_downSample;
    end
    cowPCRemain = size(pcRemainObj.Location)*[1;0];
    if cowPCRemain>pcOutNum
        cowPCRemain = pcOutNum;
    end
    objPointVeh(1:cowPCRemain,:) = [pcRemainObj.Location pcRemainObj.Intensity];
    
    
    %         pcRemainObj = pcObj;
    %         cowPCRemain = size(pcRemainObj.Location)*[1;0];
    %         objPointVeh(1:cowPCRemain,:) = pcRemainObj.Location;
end
% end


% figure(2)
% % pcshow(plane1)
% pcshow(pcPlanel)
% title('First Plane')

% cowPCRemain = length(pcObj.Location(:,1));
% pcRemain(1:cowPCRemain,:) = pcObj.Location;

% figure(3)
% % pcshow(plane1)
% pcshow(pcRemain)
% title('remainPtCloud')


%% 欧式聚类
% 最小聚类欧式距离
minDist = 0.5;

% 执行欧式聚类分割
[labels,numClusters] = pcsegdist(pcRemainObj,minDist);

% 显示分割结果
hsvColorMap = hsv(numClusters);
hsvColorMap_H = hsvColorMap(:,1);
hsvColorMap_S = hsvColorMap(:,2);
hsvColorMap_V = hsvColorMap(:,3);
%     view(player,pcRemainObj.Location,[hsvColorMap_H(labels) hsvColorMap_S(labels) hsvColorMap_V(labels)]);
%     pcshow(pcRemainObj.Location,labels);
%     colormap(hsv(numClusters));


% 遍历所有聚类结果
figure(5);
clf
axis([insRegion(1) insRegion(2) insRegion(3) insRegion(4)])
title('欧式聚类分割');
xlabel('X(m)');
ylabel('Y(m)');
zlabel('Z(m)');
hold on;

% boundaryOut = zeros(200,1000)-100;
% boundaryOut = [];
boundaryOut = struct('xy_bd',[]);
bdCount = 1;
for clusterCount = 1:1:numClusters
    %% 进行多边形框计算
    pcClusterObjTemp = select(pcRemainObj,find(labels == clusterCount));
    
    % 求解获得凹多边形进行多边形框计算
    if length(pcClusterObjTemp.Location(:,1))>=3 %
        k_all = boundary(double(pcClusterObjTemp.Location(:,1:2)),0.04);
        xy_bd = pcClusterObjTemp.Location(k_all,1:2);
        
        plot(xy_bd(:,1), xy_bd(:,2), 'r');
        if ~isempty(xy_bd)
            % 整理到boundaryOut输出变量中
            %             for topPointCount = 1:1:length(xy_bd(:,1))
            %             boundaryOut(clusterCount,topPointCount:topPointCount+1) ...
            %                 = [xy_bd(topPointCount,1) xy_bd(topPointCount,2)];
            %         end
            boundaryOut(bdCount).xy_bd = xy_bd;
            bdCount = bdCount+1;
        end
    end
end

hold off










objVehPoint = objPointVeh;
%%
pcObjOut =   pointCloud(objVehPoint(:,1:3));
pcObjOut.Intensity = objVehPoint(:,4);

frameID = frameID+1;


toc

view(player,pcObjOut);
%     figure(4)
%     pcshow(pcObjOut.Location)
%     xlabel('X(m)');
%     ylabel('Y(m)');
%     zlabel('Z(m)');
%     axis([insRegion(1) insRegion(2) insRegion(3) insRegion(4)])

pause(0.02);

end