matlab练习程序（点云表面法向量）

发奋图强

1.首先使用KD树寻找当前点邻域的N个点，这里取了10个，直接调用了vlfeat。

2.用最小二乘估计当前邻域点组成的平面，得到法向量。

3.根据当前邻域点平均值确定邻域质心，通常质心会在弯曲表面的内部，反方向即为法线方向。

vlfeat下载地址：http://www.vlfeat.org/

处理效果如下：
原始点云：


点云表面法向量，做了降采样处理：


兔子果断变刺猬。

matlab代码如下：

1. clear all;
   
2. close all;
   
3. clc;
   
4. warning off;
   
5. 
   
6. pc = pcread('rabbit.pcd');
   
7. pc=pcdownsample(pc,'random',0.3);       %0.3倍降采样
   
8. pcshow(pc);
   
9. 
   
10. pc_point = pc.Location';                %得到点云数据
    
11. kdtree = vl_kdtreebuild(pc_point);      %使用vlfeat建立kdtree
    
12. 
    
13. normE=[];
    
14. for i=1:length(pc_point)
    
15.    
    
16.     p_cur = pc_point(:,i);
    
17.     [index, distance] = vl_kdtreequery(kdtree, pc_point, p_cur, 'NumNeighbors', 10);    %寻找当前点最近的10个点
    
18.     p_neighbour = pc_point(:,index)';
    
19.     p_cent = mean(p_neighbour);     %得到局部点云平均值，便于计算法向量长度和方向
    
20.    
    
21.     %最小二乘估计平面
    
22.     X=p_neighbour(:,1);
    
23.     Y=p_neighbour(:,2);
    
24.     Z=p_neighbour(:,3);
    
25.     XX=[X Y ones(length(index),1)];
    
26.     YY=Z;
    
27.     %得到平面法向量
    
28.     C=(XX'*XX)\XX'*YY;
    
29.    
    
30.     %局部平面指向局部质心的向量
    
31.     dir1 = p_cent-p_cur';
    
32.     %局部平面法向量
    
33.     dir2=[C(1) C(2) -1];
    
34.    
    
35.     %计算两个向量的夹角
    
36.     ang = dir1.*dir2 / (sqrt(dir1(1)^2 +dir2(1)^2) + sqrt(dir1(2)^2 +dir2(2)^2)+sqrt(dir1(3)^2 +dir2(3)^2) );
    
37.    
    
38.     %根据夹角判断法向量正确的指向
    
39.     flag = acos(ang);
    
40.     dis = norm(dir1);
    
41.     if flag<0
    
42.         dis = -dis;
    
43.     end
    
44.    
    
45.     %画出当前点的表面法向量
    
46.     t=(0:dis/100:dis)';
    
47.     x = p_cur(1) + C(1)*t;
    
48.     y = p_cur(2) + C(2)*t;
    
49.     z = p_cur(3) + (-1)*t;
    
50.    
    
51.     normE =[normE;x y z];
    
52.     i
    
53. end
    
54. pcshowpair(pc,pointCloud(normE));

复制代码

ztj001

matlab难不难学