matlab練習程序（點雲表面法向量）

思路還是很容易想到的：

1.首先使用KD樹尋找當前點鄰域的N個點，這里取了10個，直接調用了vlfeat。

2.用最小二乘估計當前鄰域點組成的平面，得到法向量。

3.根據當前鄰域點平均值確定鄰域質心，通常質心會在彎曲表面的內部，反方向即為法線方向。

vlfeat在這里下載，配置參考這里，rabbit.pcd下載地址

處理效果如下：

原始點雲：

點雲表面法向量，做了降采樣處理：

兔子果斷變刺蝟。

matlab代碼如下：

clear all;
close all;
clc;
warning off;

pc = pcread('rabbit.pcd');
pc=pcdownsample(pc,'random',0.3);       %0.3倍降采樣
pcshow(pc);

pc_point = pc.Location';                %得到點雲數據
kdtree = vl_kdtreebuild(pc_point);      %使用vlfeat建立kdtree

normE=[];
for i=1:length(pc_point)
    
    p_cur = pc_point(:,i);
    [index, distance] = vl_kdtreequery(kdtree, pc_point, p_cur, 'NumNeighbors', 10);    %尋找當前點最近的10個點
    p_neighbour = pc_point(:,index)';
    p_cent = mean(p_neighbour);     %得到局部點雲平均值，便於計算法向量長度和方向
    
    %最小二乘估計平面
    X=p_neighbour(:,1);
    Y=p_neighbour(:,2);
    Z=p_neighbour(:,3);
    XX=[X Y ones(length(index),1)];
    YY=Z;
    %得到平面法向量
    C=(XX'*XX)\XX'*YY;
    
    %局部平面指向局部質心的向量
    dir1 = p_cent-p_cur';
    %局部平面法向量
    dir2=[C(1) C(2) -1];
    
    %計算兩個向量的夾角
    ang = sum(dir1.*dir2) / (sqrt(dir1(1)^2 +dir2(1)^2) + sqrt(dir1(2)^2 +dir2(2)^2)+sqrt(dir1(3)^2 +dir2(3)^2) );
    
    %根據夾角判斷法向量正確的指向
    flag = acos(ang);
    dis = norm(dir1);
    if flag<0
        dis = -dis;
    end
    
    %畫出當前點的表面法向量
    t=(0:dis/100:dis)';
    x = p_cur(1) + C(1)*t;
    y = p_cur(2) + C(2)*t;
    z = p_cur(3) + (-1)*t;
    
    normE =[normE;x y z];
    i
end
pcshowpair(pc,pointCloud(normE));