MATLAB电云插值你和曲面，求工作点

最近在做一个机器人相关的项目，需要通过雷达和红外线扫描出来的点，你和出一个曲面，求出法向量，并且根据工件上的点和法向量求出安全工作点。废话不多说，线上代码。

%学习记录1：2017.08.17 读取txt文件数据，插值法取XYZ，surf出面
clear;
data = importdata( ‘D:\rplidar12.txt’); %导入txt文件数据
figure(1);
%plot3(data(:,1),data(:,2),data(:,3),’b.’); %用plot3函数可以显示点云

%从data中取出每一列数据
x=data(:,1);
y=data(:,2);
z=data(:,3);
%y=y/1000;
%z=z/1000;
%z=z*1000;
[X,Y,Z]=griddata(x,y,z,linspace(min(x),max(x),100)’,linspace(min(y),max(y),100)); %插值
%surf(X,Y,Z)
[Nx,Ny,Nz]=surfnorm(X,Y,Z); %导出法向量的值

%去除插值以后的not a number（NaN）数据
A=find(~isnan(Nx));
B=find(~isnan(Ny));
C=find(~isnan(Nz));
NA = (Nx(A));
NB = (Ny(B));
NC = (Nz(C));
FA = X(A);
FB = Y(B);
FC = Z(C);
%根据工件点和法向量求出工作点。
%未注释的是给定安