空间三维散点数据的线性拟合

<span style="font-size:14px;">clc;
clear all;
close all;
num = 50; % num个随机点
Rand1 = randi([-1,1],num,3); %噪声范围
Rand2 = randi([-1,1],num,3);
Point1 = [1:0.5:0.5*(num+1); 1:0.5:0.5*(num+1); 1:0.5:0.5*(num+1)]'+ Rand1;
plot3(Point1(:,1),Point1(:,2),Point1(:,3),'r.');
Point2 = [0.5*(num+1):-0.5:1; 1:0.5:0.5*(num+1); 0.5*(num+1):-0.5:1]'+ Rand2;
hold on;
plot3(Point2(:,1),Point2(:,2),Point2(:,3),'g+');

%直线拟合1
t1=linspace(0,25);%有效范围
F1 = @(p)arrayfun(@(n)norm(cross(Point1(num,:)-[p(1),p(2),p(3)],...
    [p(4),p(5),p(6)]))/norm([p(4),p(5),p(6)]),[1:size(Point1,1)]);
p= lsqnonlin(F1,[1 1 1 1 1 1]);
plot3(p(1)+t1*p(4),p(2)+t1*p(5),p(3)+t1*p(6));

%直线拟合2
t2=linspace(0,-8); 
F2 = @(p)arrayfun(@(n)norm(cross(Point2(num,:)-[p(1),p(2),p(3)],...
    [p(4),p(5),p(6)]))/norm([p(4),p(5),p(6)]),[1:size(Point2,1)]);
p= lsqnonlin(F2,[25,2,25,1,1,1]); %初始化迭代
plot3(p(1)+t2*p(4),p(2)+t2*p(5),p(3)+t2*p(6));
grid on;
hold off;</span>

fit_line.m

<span style="font-size:14px;">function [a,resnorm] = fit_line(a0,data)
% 对三维数据进行直线拟合,将直线拟合成如下形式:  
% x-a(1)    y-a(2)  z-a(3)   
% ------ = ------ = ------   
%  a(4)     a(5)     a(6)   
% 原理:下面的 d 是各数据点到拟合直线的距离,利用 lsqnonlin 对 d 进行最小二乘   
% a0 是初始值,data 是三维数据,第一行是 x,第二行是 y,第三行是 z 
[a,resnorm] = lsqnonlin(@fit_line_fun,a0);   
   function d=fit_line_fun(a)   
	% 传给主函数的子函数   
	xdata=data(1,:);     
	ydata=data(2,:);      
	zdata=data(3,:);
          
	point=a(1:3);      
	v=a(4:6);      
	d(1:length(xdata))=0;       
	for n=1:length(xdata)           
		m=[xdata(n);ydata(n);zdata(n)]-point(:);
		d(n)=norm(cross(m,v(:)))/norm(v(:));% 利用解析几何的知识求距离d  
	end  
   end 
end
</span>
实验效果图:



相关推荐
©️2020 CSDN 皮肤主题: 代码科技 设计师:Amelia_0503 返回首页