算法思路是首先創建kd樹,而後找到每一個點距離最近的點的距離,對距離求和再求平均便可。算法
代碼以下:dom
clear all; close all; clc; pc = pcread('rabbit.pcd'); pc = pcdownsample(pc,'random',0.1); %下降一下數據量 pc_point = pc.Location'; %獲得點雲數據 kdtree = vl_kdtreebuild(pc_point); %使用vlfeat創建kdtree dissum = 0; for i=1:length(pc_point) p_cur = pc_point(:,i); [index, distance] = vl_kdtreequery(kdtree, pc_point, p_cur, 'NumNeighbors',2); %尋找當前點最近的非自身點 dissum = dissum + sqrt(distance(2)); %距離求和 end avg = dissum / length(pc_point);