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

转载：http://blog.csdn.net/shenziheng1/article/details/51030700

[cpp] view plain copy

 

1. <span style="font-size:14px;">clc;  
2. clear all;  
3. close all;  
4. num = 50; % num个随机点  
5. Rand1 = randi([-1,1],num,3); %噪声范围  
6. Rand2 = randi([-1,1],num,3);  
7. Point1 = [1:0.5:0.5*(num+1); 1:0.5:0.5*(num+1); 1:0.5:0.5*(num+1)]'+ Rand1;  
8. plot3(Point1(:,1),Point1(:,2),Point1(:,3),'r.');  
9. Point2 = [0.5*(num+1):-0.5:1; 1:0.5:0.5*(num+1); 0.5*(num+1):-0.5:1]'+ Rand2;  
10. hold on;  
11. plot3(Point2(:,1),Point2(:,2),Point2(:,3),'g+');  
12.   
13. %直线拟合1  
14. t1=linspace(0,25);%有效范围  
15. F1 = @(p)arrayfun(@(n)norm(cross(Point1(num,:)-[p(1),p(2),p(3)],...  
16.     [p(4),p(5),p(6)]))/norm([p(4),p(5),p(6)]),[1:size(Point1,1)]);  
17. p= lsqnonlin(F1,[1 1 1 1 1 1]);  
18. plot3(p(1)+t1*p(4),p(2)+t1*p(5),p(3)+t1*p(6));  
19.   
20. %直线拟合2  
21. t2=linspace(0,-8);   
22. F2 = @(p)arrayfun(@(n)norm(cross(Point2(num,:)-[p(1),p(2),p(3)],...  
23.     [p(4),p(5),p(6)]))/norm([p(4),p(5),p(6)]),[1:size(Point2,1)]);  
24. p= lsqnonlin(F2,[25,2,25,1,1,1]); %初始化迭代  
25. plot3(p(1)+t2*p(4),p(2)+t2*p(5),p(3)+t2*p(6));  
26. grid on;  
27. hold off;</span>  

fit_line.m

[cpp] view plain copy

 

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

实验效果图：