Matlab学习笔记#08

一些好用的自编写函数：

1.计算点到直线的距离

function dist=zg_point2line_dist(data1,data2,coord)
dist=norm(cross((coord-data1),(coord-data2)))/norm(data2-data1);
end

• coord是点的坐标；
• data1和data2是空间直线上的两点。

2.计算点到平面的距离

function [fpoint,dist]=zg_point2planep3(plane3,coord)
op1=plane3(1,:)-plane3(2,:);
op2=plane3(1,:)-plane3(3,:);
n=cross(op1,op2);
n=n/norm(n);
t=dot(plane3(1,:),n)-dot(coord,n);
fpoint=coord+n*t;
dist=sqrt(sum((fpoint-coord).^2,2));
end

• coord是点的坐标；
• plane3至少包含3组不共线平面点坐标；
• fpoint是垂足坐标，dist是距离。
• 以Ax+By+Cz+D=0表示的平面计算点到面距离代码如下

function dist=zg_point2plane(a,b,c,d,coord)
    dist=abs([a,b,c,d]*[coord,ones(size(coord,1),1)]')/sqrt(a*a+b*b+c*c);
    %去掉abs可以使距离有正负，基于此可判断点在平面哪一侧。
end

3.计算两组点（顺序对应）之间的距离

function dist=zg_distance(X1,X2)
dist=sqrt(sum((X1-X2).^2,2));
end

4.保留指定位数的小数

function D=zg_decimal(d)
n=4;%保留几位小数
D=round(d*10^n)/10^n;
end

5.基于DTW分析两段轨迹的相似性

function ds=zg_dtw(X1,X2)
m=size(X1,1);
n=size(X2,1);
M=zeros(m,n);
for i=1:m
    for j=1:n
        M(i,j)=zg_distance(X1(i,:),X2(j,:));                  
    end
end
Mc=zeros(m,n);
Mc=[0,inf*ones(1,n);
    inf*ones(m,1),Mc];
for i=2:m+1
    for j=2:n+1
        Mc(i,j)=min([Mc(i-1,j),Mc(i,j-1),Mc(i-1,j-1)])+M(i-1,j-1);                  
    end
end
ds=Mc(m+1,n+1);
end

6.求B轨迹每点到A轨迹的最小距离

function errort=zg_errort(A,B)
dist=zeros(size(B,1),1);
for i=1:size(B,1)
        dist(i)=min(zg_distance(A,B(i,:)));
end
errort=dist;
end

7.最小二乘法拟合空间球

方法一

function [center,radius]=zg_find_center_radius(data)
%由P生成A（包括Ax，Ay，Az，Ad）和e
x = data(:,1);
y = data(:,2);
z = data(:,3);
Ax = -2*x;
Ay = -2*y;
Az = -2*z;
Ad =  0*data(:,1) + 1;
A = [ Ax Ay Az Ad];
e = -x.^2 -y.^2 -z.^2;
%求解abcd, Ax=e
X = inv(A'*A)*A'*e;
a = X(1);
b = X(2);
c = X(3);
r2 = a^2 + b^2 + c^2 - X(4);
r  = r2^0.5;
%验证结果
center=[a b c];
radius=r;
end

（来源：利用最小二乘法拟合空间圆（球））

方法二

function [center,radius]=zg_find_center_radius(data)
x=lsqnonlin(@(x)(data(:,1)-x(1))^2+(data(:,2)-x(2))^2+(data(:,3)-x(3))^2-x(4)^2,[1,1,1,1]);
center=x(1:3);
radius=x(4);
end

8.两组点序列对应连线绘图

function zg_point2point_draw(seq1,seq2)
for i=1:size(seq1,1)
    temp=[seq1(i,:);seq2(i,:)];
    plot3(temp(:,1),temp(:,2),temp(:,3),'k')
end
end

9.箱线图绘制代码

function zg_boxchart_draw(errort1;errort2;errort3;errort4)
errot=[errort1;errort2;errort3;errort4];
g1 = repmat(categorical({'rose'}),length(errort1),1);
g2 = repmat(categorical({'rectangle'}),length(errort2),1);
g3 = repmat(categorical({'sine'}),length(errort3),1);
g4 = repmat(categorical({'zigzag'}),length(errort4),1);
g = [g1; g2; g3; g4];
boxchart(g,errot)
end

• errort需要是列向量；
• 可以不作为函数，直接截取代码使用；

10.柱状误差棒图绘制

function zg_errorbar_draw(errort1;errort2;errort3;errort4)
name=[categorical({'rose'}),categorircal({'rectangle'}),categorical({'sine'}),categorical({'zigzag'})];
error_mean=[mean(errort1),mean(errort2),mean(errort3),mean(errort4)];
bar(name,error_mean,'EdgeColor','none','FaceColor','#d9531a');
grid on;hold on;
up=[max(errort1)-mean(errort1),max(errort2)-mean(errort2),max(errort3)-mean(errort3),max(errort4)-mean(errort4)];
down=-[min(errort1)-mean(errort1),min(errort2)-mean(errort2),min(errort3)-mean(errort3),min(errort4)-mean(errort4)];
errorbar(name,error_mean,down,up,'ko');
end

• 柱状图可以表示数据的中位值、平均值等，此处表示的是平均值；
• 可以不作为函数，直接截取代码使用；

11.基于协方差计算点云的方向量和曲率

function [vec,q]=zg_normal_curv_cal(Seq_Point)
a = Seq_Point;
vec = zeros(size(a));
q = zeros(length(a),1);
k = 8;
neighbors = knnsearch(a(:,1:3),a(:,1:3), 'k', k+1);
for i = 1:length(a)
    curtemp = neighbors(i,2:end);
    indpoint = a(curtemp,:);
    [v, c] = eig(cov(indpoint));
    c = diag(c)';
    z = sum(c);
    p1 = c(:,1)/z;
    q(i,:) = abs(p1);
    vec(i,:) = v(:,1)';
end
end

• Seq_Point是点云的坐标，为N×3数组；

12.计算两矢量的夹角和旋转轴

function [k,alpha]=zg_rotate_vector(v1,v2)
alpha=acos(dot(v1,v2)/(norm(v1)*norm(v2)));
k=cross(v1,v2);
k=k/norm(k);
end 

13.基于SVD拟合三维平面

function [a,b,c,d]=zg_find_plane(planeData)
% 协方差矩阵的SVD变换中，最小奇异值对应的奇异向量就是平面的方向
xyz0=mean(planeData,1);
centeredPlane=bsxfun(@minus,planeData,xyz0);
[~,~,V]=svd(centeredPlane);
 
a=V(1,3);
b=V(2,3);
c=V(3,3);
d=-dot([a b c],xyz0);
end 

• a,b,c,d是空间直线一般式的四个参数

14.基于点云拟合三维平面

function [a,b,c,d]=zg_ptfind_plane(planeData)
% 将XYZ数据转换为点云数据
ptCloud = pointCloud(planeData);
% 设置点到平面的最小距离
maxDistance = 0.1;
% 执行平面拟合，并输出平面方程ax+by+cz+d=0系数
model = pcfitplane(ptCloud,maxDistance);
% 结果输出
[a,b,c,d]=model.Parameters;
end 

15.数组循环移位

上移

function res=zg_MatUpMoveN(data_mat,N)
m=size(data_mat,1);
res=[data_mat(N+1:m,:);data_mat(1:N,:)];
end

下移

function res=zg_MatDownMoveN(data_mat,N)
m=size(data_mat,1);
res=[data_mat(m-N+1:m,:);data_mat(1:m-N,:)];
end

左移

function res=zg_MatLeftMoveN(data_mat,N)
n=size(data_mat,2);
res=[data_mat(:,1+N:n),data_mat(:,1:N)];
end

右移

function res=zg_MatRightMoveN(data_mat,N)
n=size(data_mat,2);
res=[data_mat(:,n-N+1:n),data_mat(:,1:n-N)];
end