基于matlab点云工具箱对点云进行处理二：对点云进行欧式聚类，获得聚类后点云簇的外接矩形

基于matlab点云工具箱对点云进行处理二：对点云进行欧式聚类，获得聚类后点云簇的外接矩形

步骤：

1. 读取velodyne数据包pcap文件内的点云数据
2. 使用pcdownsample函数对点云数据进行体素化采样，减少点云数量
3. 使用find函数对点云进行筛选
4. 使用pcdnoise去除点云内的噪声
5. 使用pcsegdist进行欧式聚类
6. 使用自定义函数getBoundary获得外接矩形，函数具体定义见附件

相关程序点这里https://download.csdn.net/download/rmrgjxeivt/59555459

存在的问题：
大型物体（例如花坛、弯曲的道路护栏等）容易被识别成为一个巨型矩形框。属于严重的错误识别。

基于matlab点云工具箱对点云进行处理一：去除地面，保留剩下的点https://blog.csdn.net/rmrgjxeivt/article/details/121830344
基于matlab点云工具箱对点云进行处理二：对点云进行欧式聚类，获得聚类后点云簇的外接矩形https://blog.csdn.net/rmrgjxeivt/article/details/121830919
基于matlab点云工具箱对点云进行处理三：对点云进行欧式聚类，使用三角剖分处理后获取点云簇的外接凸多边形https://blog.csdn.net/rmrgjxeivt/article/details/121831507
基于matlab点云工具箱对点云进行处理四：对点云进行欧式聚类，并获得包围点云簇的外接凹多边形https://blog.csdn.net/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.03;
    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
    
    
   
    
    %% 欧式聚类
    % 最小聚类欧式距离
    minDist = 0.7;
    
    % 执行欧式聚类分割
    [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(2);
    %     subplot(2,1,2)
    clf
    axis([insRegion(1) insRegion(2) insRegion(3) insRegion(4)])
    title('欧式聚类分割');
    xlabel('X(m)');
    ylabel('Y(m)');
    zlabel('Z(m)');
    hold on;
    for i = 1:1:numClusters
        pcClusterObjTemp = select(pcRemainObj,find(labels == i));
        pbar = getBoundary(pcClusterObjTemp.Location);
        plot([pbar(:,1); pbar(1,1)],[pbar(:,2); pbar(1,2)],'r')
    end
    hold off
    

    objVehPoint = objPointVeh;
    %%
    pcObjOut =   pointCloud(objVehPoint(:,1:3));
    pcObjOut.Intensity = objVehPoint(:,4);
    
    frameID = frameID+1;
    
    
    toc
    
        view(player,pcObjOut);
    pause(0.02);
    
end
% end

function [pbar]  =  getBoundary(pcCluster)

if length(pcCluster(:,1))>=3
    ind = convhull(double(pcCluster(:,1)),double(pcCluster(:,2)));
    pcNum = length(ind);
    hull = pcCluster(ind,1:2);          %随机点凸包
    area = inf;
    for i = 2:pcNum
        p1 = hull(i-1,:);     %凸包上两个点
        p2 = hull(i,:);
        
        k1 = (p1(2)-p2(2))/(p1(1)-p2(1));     %连接两点的直线，作为矩形的一条边
        b1 = p1(2)-k1*p1(1);
        
        d = abs(hull(:,1)*k1-hull(:,2)+b1)/sqrt(k1^2+1);  %所有凸包上的点到k1,b1直线的距离
        
        [h, ind] = max(d);                     %得到距离最大的点距离，即为高，同时得到该点坐标
        b2 = hull(ind,2)-k1*hull(ind,1);      %相对k1,b1直线相对的另一条平行边k1,b2;
        
        k2 = -1/k1;                           %以求得的直线的垂线斜率
        
        b = hull(:,2)-k2*hull(:,1);           %过凸包所有点构成的k2,b直线系
        x1 = -(b1-b)/(k1-k2);                 %凸包上所有点在已求得的第一条边的投影
        y1 = -(-b*k1+b1*k2)/(k1-k2);
        
        x2 = -(b2-b)/(k1-k2);                 %凸包上所有点在已求得的第二条边的投影
        y2 = -(-b*k1+b2*k2)/(k1-k2);
        
        [~, indmax1] = max(x1);             %投影在第一条边上x方向最大与最小值
        [~, indmin1] = min(x1);
        
        [~, indmax2] = max(x2);             %投影在第二条边上x方向最大与最小值
        [~, indmin2] = min(x2);
        
        w = sqrt((x1(indmax1)-x1(indmin1))^2+(y1(indmax1)-y1(indmin1))^2);    %矩形的宽
        
        if area >= h*w                        %使面积最小
            area = h*w;
            pbar = [x1(indmax1) y1(indmax1);  %矩形四个角点
                x2(indmax2) y2(indmax2);
                x2(indmin2) y2(indmin2);
                x1(indmin1) y1(indmin1)];
        end
    end
    % pbar(5,:) = pbar(1,:);
end

if length(pcCluster(:,1))==2 % 只含两个点的情况
     hull = pcCluster(:,1:2);
    pbar = [pcCluster(1,1) pcCluster(1,2);
        pcCluster(2,1) pcCluster(2,2);
        pcCluster(2,1)+0.05 pcCluster(2,2)+0.05;
        pcCluster(1,1)+0.05 pcCluster(1,2)+0.05;];
end

if length(pcCluster(:,1))==1 % 只含一个点的情况
    hull = pcCluster(:,1:2);
    pbar = [pcCluster(1,1) pcCluster(1,2);
        pcCluster(1,1)+0.05 pcCluster(1,2);
        pcCluster(1,1)+0.05 pcCluster(1,2)+0.05;
        pcCluster(1,1) pcCluster(1,2)+0.05;];
end

% figure(2)
% plot(hull(:,1),hull(:,2),'.');
% hold on;
% plot([pbar(:,1); pbar(1,1)],[pbar(:,2); pbar(1,2)],'r')
% axis equal;
% hold off

end