算法思路是首先建立kd树,然后找到每个点距离最近的点的距离,对距离求和再求平均即可。
代码如下:
1 clear all;
2 close all;
3 clc;
4
5 pc = pcread('rabbit.pcd');
6 pc = pcdownsample(pc,'random',0.1); %降低一下数据量
7 pc_point = pc.Location'; %得到点云数据
8 kdtree = vl_kdtreebuild(pc_point); %使用vlfeat建立kdtree
9
10 dissum = 0;
11 for i=1:length(pc_point)
12 p_cur = pc_point(:,i);
13 [index, distance] = vl_kdtreequery(kdtree, pc_point, p_cur, 'NumNeighbors',2); %寻找当前点最近的非自身点
14 dissum = dissum + sqrt(distance(2)); %距离求和
15 end
16
17 avg = dissum / length(pc_point);