matlab学习：最小二乘拟合&基于RANSAC的直线拟合&椭圆拟合

1.最小二乘拟合

最小二乘拟合是一种数学上的近似和优化，利用已知的数据得出一条直线或者曲线，使之在坐标系上与已知数据之间的距离的平方和最小。

2.RANSAC算法

参见王荣先老师的博文 http://www.cnblogs.com/xrwang/archive/2011/03/09/ransac-1.html

3，直线拟合

建立模型时利用直线的一般方程AX+BY+C=0，随机选取两点构建直线模型，计算每个点到此直线的TLS（Total Least Square），TLS小于一定阈值时的点为符合模型的点，点数最多时的模型即为最佳直线模型。再根据此时的直线参数画出最终拟合直线。

4.椭圆拟合

建立模型时利用椭圆的定义方程：dist（P,A）+dist（P,B）=DIST，其中P为椭圆上一点，A和B为椭圆两焦点。随机选取三点A,B,P构建椭圆模型，计算每个点到此两焦点的距离和与DIST的差值，差值小于一定阈值时的点为符合模型的点，点数最多时的模型即为最佳椭圆模型，再根据符合条件的点，利用椭圆一般方程Ax2+Bxy+Cy2+Dx+Ey+F=0 和得到符合点进行系数拟合，根据函数式画出最终拟合椭圆。

5.matlab代码

（1）最小二乘拟合

View Code

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%       FILENAME    LSF.m   

%       FUNCTION    Input points with mouse,Least-squares fit of lines to

%                   2D points

%       DATE        2012-10-12

%       AUTHOR      zhangying

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clc;

%% 鼠标输入点，enter键结束

axis([-10 10 -10 10]);

[x,y]=ginput;   %读取坐标直到按下回车键,返回坐标点的x,y坐标

num=length(x);  %计算点的个数



%% 直接用最小二乘进行拟合

%通过最小化误差的平方和寻找数据的最佳函数匹配

[p1,s1]=polyfit(x,y,1);     %n=1为直线拟合 x,y为数据点，n为多项式阶数，返回p为幂次从高到低的多项式系数向量

[p2,s2]=polyfit(x,y,num-2); %n>1为曲线拟合，找到次数为n的多项式，对于数据点集（x,y）,满足差的平方和最小

[p3,s3]=polyfit(x,y,num-1); %x必须是单调的。矩阵s用于在polyval中来估计误差。

xcurve=-10:2:10;            %在这些点处求多项式的值

p1curve=polyval(p1,xcurve); %多项式曲线求值,返回对应自变量xcurve在给定系数P的多项式的值

p2curve=polyval(p2,xcurve);

p3curve=polyval(p3,xcurve);

%% 画图

plot(xcurve,p1curve,'g-',xcurve,p2curve,'b-',...

    xcurve,p3curve,'r-',x,y,'*');

title('不同次数的最小二乘拟合');

legend('degree 1','degree num-2','degree num-1','points');

 （2）基于RANSAC的直线拟合

View Code

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%   FILENAME        RANSACF.m  

%   FUNCTION        RANSAC fit of lines to 2D points 

%   DATE            2012-10-11

%   AUTHOR          zhangying

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clc;

clear;

%% 随机点生成

g_NumOfPoints = 500;   % 点数

g_ErrPointPart = 0.4;  % 噪声

g_NormDistrVar = 1;    % 标准偏差

% 生成随机点

theta = (rand(1) + 1) * pi/6;

R = ( rand([1 g_NumOfPoints]) - 0.5) * 100;

DIST = randn([1 g_NumOfPoints]) * g_NormDistrVar;

Data = [cos(theta); sin(theta)] * R + [-sin(theta); cos(theta)] * DIST;

Data(:, 1:floor(g_ErrPointPart * g_NumOfPoints)) = 2 * [max(abs(Data(1,:))), 0; 0, max(abs(Data(2,:)))] *...

                                                        (rand([2 floor(g_ErrPointPart * g_NumOfPoints)]) - 0.5);

plot(Data(1, :), Data(2, :), '.', 'Tag', 'DATA');

hold on;

%% RANSAC拟合

% 拟合模型初始化

nSampLen = 2;                       %设定模型所依据的点数

nIter = 50;                         %最大循环次数

dThreshold = 2;                     %残差阈值

nDataLen = size(Data, 2);           %数据长度

RANSAC_model = NaN;                 %跳过缺失模型

RANSAC_mask = zeros([1 nDataLen]);  %全0矩阵，1表示符合模型，0表示不符合

nMaxInlyerCount = -1;

%% 主循环

for i = 1:nIter 

    %  抽样，选取两个不同的点

    SampleMask = zeros([1 nDataLen]);  

    while sum( SampleMask ) ~= nSampLen

        ind = ceil(nDataLen .* rand(1, nSampLen - sum(SampleMask))); %rand产生随机数，ceil向离它最近的大整数圆整

        SampleMask(ind) = 1;

    end    

    Sample = find( SampleMask );        %找出非零元素的索引值，即建立模型的点

    %% 建立模型,并查找符合模型的点

    ModelSet = feval(@TLS, Data(:, Sample));    %计算所有符合模型的点的最小二乘

    for iModel = 1:size(ModelSet, 3) 

      CurModel = ModelSet(:, :, iModel);        %当前模型对应的直线参数   

      CurMask =( abs( CurModel * [Data; ones([1 size(Data, 2)])])< dThreshold);%到直线距离小于阈值的点符合模型,标记为1

      nCurInlyerCount = sum(CurMask);           %计算符合直线模型的点的个数

        %% 选取最佳模型

        if nCurInlyerCount > nMaxInlyerCount    %符合模型的点数最多的模型即为最佳模型

            nMaxInlyerCount = nCurInlyerCount;

            RANSAC_mask = CurMask;

            RANSAC_model = CurModel;

        end

    end

end

%% 画最佳模型的拟合结果

MinX=min(Data(1, :));

MaxX=max(Data(1, :));

MinX_Y=-(RANSAC_model(1).*MinX+RANSAC_model(3))./RANSAC_model(2);

MaxX_Y=-(RANSAC_model(1).*MaxX+RANSAC_model(3))./RANSAC_model(2);

plot([MinX MaxX],[MinX_Y MaxX_Y],'r-');

title('ransac在噪声情况下的直线拟合');



%% 用RANSAC方法拟合原理

%   RANSAC算法的输入是一组观测数据，一个可以解释或者适应于观测数据的参数化模型，一些可信的参数。

%   RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点，并用下述方法进行验证：

%   1.有一个模型适应于假设的局内点，即所有的未知参数都能从假设的局内点计算得出。

%   2.用1中得到的模型去测试所有的其它数据，如果某个点适用于估计的模型，认为它也是局内点。

%   3.如果有足够多的点被归类为假设的局内点，那么估计的模型就足够合理。

%   4.然后，用所有假设的局内点去重新估计模型，因为它仅仅被初始的假设局内点估计过。

%   5.最后，通过估计局内点与模型的错误率来评估模型。

%   这个过程被重复执行固定的次数，每次产生的模型要么因为局内点太少而被舍弃，要么因为比现有的模型更好而被选用。



%% 问题分析

%    1.关于画直线：根据抽取的两点画最终直线，结果不稳定，每次运行后的直线都会有很大偏差。

%    程序中已经计算出了直线的参数，根据Ax+By+C=0，可以选择数据点中最左边的点和最右边的点确定最终直线，比较稳定。

%   2.调用函数A时，若有函数B做参数，则在函数B前加@，函数B的参数则另外传送，通过feval可将函数的执行方式统一起来。

%   3.关于随机点生成：rand的使用灵活多变。

View Code

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%   FILENAME        TLS.m  

%   FUNCTION        Calculate Total Least Squares of input data

%   DATE           2012-10-11

%   AUTHOR          unkown

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Total Least Squares      TLS(Data) 

%Return: [a,b,c] - line in a * x + b * y + c = 0 form

%                   where a ^ 2 + b ^ 2 = 1

%   TLS(X,Y) - approximates ALL points in array by one line



function line = TLS(Data)



  if any( size(Data) == 0)

      Line = [0, 0, 0];

      return;

  end



  X = Data(1, :);

  Y = Data(2, :);

  

  len = length(X);



  if size(X) ~= size(Y)

      X = X';

  end

    

  M = [ mid(X .^ 2) - mid(X) ^ 2, sum(X .* Y) / len - mid(X) * mid(Y);...

      sum(X .* Y) / len - mid(X) * mid(Y), mid(Y .^ 2) - mid(Y) ^ 2];

  [ev,tmp] = eig(M);

         

  ind = 2;

  if ErrFunc(X, Y, ev(:, 1)) < ErrFunc(X, Y, ev(:, 2))

      ind = 1;

  end

                  

  line = [ev(1,ind), ev(2,ind),...

         -ev(1,ind) * mid(X) - ev(2,ind) * mid(Y)];



return;



% Help function, calculates an error

function e = ErrFunc(X,Y,L)

maxE = 0;

e = 0;

c = -L(1) * mid(X) - L(2) * mid(Y);

for i = 1:length(X)

    e = e + ( L(1) * X(i) + L(2) * Y(i) + c )^2;

    if (L(1) * X(i) + L(2) * Y(i) + c )^2 > maxE

        maxE = (L(1) * X(i) + L(2) * Y(i) + c )^2;

    end;

end



return;



% Middle value of vector X

function l = mid(X)



if length(X) > 0

    l = sum(X) / length(X);

else 

    l = 0;

end;



return;

（3）基于RANSAC的椭圆拟合

View Code

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%   FILENAME        ellipsefit.m

%   FUNCTIPN        Least-squares fit of ellipse to 2D points

%   DATE            2012-10-12

%   AUTHOR          zhangying

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%

clc;

clear;

%%  生成 带噪声的椭圆

% 参数初始化

g_NumOfPoints = 500;   % 点数

g_ErrPointPart = 0.4;  % 噪声

g_NormDistrVar = 3;    % 标准偏差

a=10;b=20;             %长轴短轴

angle=60;              %倾斜角

%% 椭圆生成

beta = angle * (pi / 180);

alpha = linspace(0, 360, g_NumOfPoints) .* (pi / 180); 

X = (a * cos(alpha) * cos(beta)- b * sin(alpha) * sin(beta) )+wgn(1,length(alpha),g_NormDistrVar^2,'linear');    

Y = (a * cos(alpha) * sin(beta)+ b * sin(alpha) * cos(beta) )+wgn(1,length(alpha),g_NormDistrVar^2,'linear');

Data=[X;Y];

plot(Data(1, :), Data(2, :), '.', 'Tag', 'DATA');

hold on;



%% RANSAC椭圆拟合

%椭圆一般方程：Ax2+Bxy+Cy2+Dx+Ey+F=0

%F=@(p,x)p(1)*x(:,1).^2+p(2)*x(:,1).*x(:,2)+p(3)*x(:,2).^2+p(4)*x(:,1)+p(5)



%%  参数初始化

nSampLen = 3;               %设定模型所依据的点数

nDataLen = size(Data, 2);   %数据长度

nIter = 50;                 %最大循环次数

dThreshold = 2;             %残差阈值

nMaxInlyerCount=-1;         %点数下限

A=zeros([2 1]);

B=zeros([2 1]);

P=zeros([2 1]);

%%  主循环

for i = 1:nIter 

   SampleMask = zeros([1 nDataLen]);   

    while sum( SampleMask ) ~= nSampLen

        ind = ceil(nDataLen .* rand(1, nSampLen - sum(SampleMask))); %抽样，选取nSampLen个不同的点

        SampleMask(ind) = 1;

     end    

    Sample = find( SampleMask );                                    %找出非零元素的索引值，即建立模型的点

    %%  建立模型，存储建模需要的坐标点,焦点和过椭圆的一个点

      %椭圆定义方程：到两定点之间距离和为常数

      A(:,1)=Data(:,ind(1));    %焦点

      B(:,1)=Data(:,ind(2));    %焦点

      P(:,1)=Data(:,ind(3));    %椭圆上一点

      DIST=sqrt((P(1,1)-A(1,1)).^2+(P(2,1)-A(2,1)).^2)+sqrt((P(1,1)-B(1,1)).^2+(P(2,1)-B(2,1)).^2);

      xx=[]; 

      nCurInlyerCount=0;        %初始化点数为0个

     %%  是否符合模型？ 

     for k=1:g_NumOfPoints

        CurModel=[A(1,1)   A(2,1)  B(1,1)  B(2,1)  DIST ];

        pdist=sqrt((Data(1,k)-A(1,1)).^2+(Data(2,k)-A(2,1)).^2)+sqrt((Data(1,k)-B(1,1)).^2+(Data(2,k)-B(2,1)).^2);

        CurMask =(abs(DIST-pdist)< dThreshold);     %到直线距离小于阈值的点符合模型,标记为1

        nCurInlyerCount =nCurInlyerCount+CurMask;             %计算符合椭圆模型的点的个数

        if(CurMask==1)

             xx =[xx,Data(:,k)];

        end

     end

       %% 选取最佳模型

        if nCurInlyerCount > nMaxInlyerCount   %符合模型的点数最多的模型即为最佳模型

            nMaxInlyerCount = nCurInlyerCount;

            Ellipse_mask = CurMask;

             Ellipse_model = CurModel;

             Ellipse_points = [A B P];

             Ellipse_x =xx;

        end

     

end



%% 由符合点拟合椭圆

%椭圆一般方程：Ax2+Bxy+Cy2+Dx+Ey+F=0

F=@(p,x)p(1)*x(:,1).^2+p(2)*x(:,1).*x(:,2)+p(3)*x(:,2).^2+p(4)*x(:,1)+p(5)*x(:,2)+p(6);

p0=[1 1 1 1 1 1];

x=Ellipse_x';

pr=nlinfit(x,zeros(size(x,1),1),F,p0);  % 拟合系数，最小二乘方法

xmin=min(x(:,1));

xmax=max(x(:,1));

ymin=min(x(:,2));

ymax=max(x(:,2));

%% 画点作图

plot(Ellipse_points(1,:),Ellipse_points(2,:),'r*');

hold on;

plot(Ellipse_x(1,:),Ellipse_x(2,:),'yo');

hold on;

ezplot(@(x,y)F(pr,[x,y]),[-1+xmin,1+xmax,-1+ymin,1+ymax]);

title('RANSAC椭圆拟合');

legend('样本点','抽取点','符合点','拟合曲线')

%% 问题分析

%    1.关于如何生成随机点：在标准椭圆基础上，添加高斯白噪声--wgn();

%    2.关于如何建立椭圆模型：

%      方案一：椭圆一般方程：Ax2+Bxy+Cy2+Dx+Ey+F=0，认为由5个点确定一个椭圆，则用5个点代入方程式去做椭圆拟合，结果大多数

%      情况下画出双曲线，放弃；

%      方案二：利用椭圆定义：到两定点之间距离和为常数2a，选择平面内3个点，两个焦点，一个过椭圆的点，确定椭圆。

%    3.关于如何筛选符合条件的点：此时计点到椭圆距离过于复杂，用定义，到两焦点距离和与2a相差小于一定阈值，则符合。

%    4.关于拟合函数：使用nlinfit，对于输入参数的维数有要求，需要x为N*P维，y为n*1维，注意是列向量。

%    5.关于如何画椭圆：与一般的画图指定X和Y不同，此时要画的是函数图形，在网上查到，先建立函数F，再利用

%      ezplot(@(x,y)F(pr,[x,y])）可画出函数图形。

%    6.数据为随机产生，程序每次运行结果会不一样，在B(:,1)=Data(:,ind(2));时有时会数组越界出错，但单步调试时没问题，

%      原因还未找到。

%%

6.学习经验
（1）不能太依赖于现有函数，要多自己想算法，即便借用别人的函数，也要弄清楚原理及调用方式；

（2）matlab函数库不熟悉，要多用help；

（3）编写程序时要先总体规划好程序架构，模块化，条理清楚，自己要懂得自己程序的每一步原理。

（4）理论的力量是无穷的，要在理论深入理解的基础上进行代码优化。