[学习SLAM]]根据空间点坐标拟合平面和直线

SVD解决平面拟合问题

 

综上：对矩阵A做奇异值分解，最小奇异值对应的特征向量就是拟合平面的系数向量。
 

根据一组点的坐标拟合空间平面，有两种方法
         第一种：如果在测量得到的数据中，x，y值都是确认没有误差的，而误差只是出现在z值上，则可以使用线性回归的方法，此方法最小二乘的目标是在z方向上de残差

Matlab 代码 % 随机生成一组（x,y,z),这些点的坐标离一个空间平面比较近 x0=1,L1=2; y0=1,L2=2; x=x0+rand(20,1)*L1; y=y0+rand(20,1)*L2; z=1+2*x+3*y; scatter3(x,y,z,'filled') hold on; X = [ones(length(x),1) x y];   % 拟合，其实是线性回归，但可以用来拟合平面 % 输出为 b = [b(1) b(2) b(3)] 表示 z = b(1) + b(2)*x + b(3)*y 是拟合出来的平面的方程 [b,bint,r,rint,stats] = regress(z,X,95);   % 图形绘制 xfit = min(x):0.1:max(x); yfit = min(y):0.1:max(y); [XFIT,YFIT]= meshgrid (xfit,yfit); ZFIT = b(1) + b(2) * XFIT + b(3) * YFIT; mesh(XFIT,YFIT,ZFIT); 复制代码输出结果如图1 第二中： 如果在测量得到的数据中，x，y，z都存在误差，则最小化的目标应该是测量点到平面距离的残差 Matlab代码 % 随机生成一组（x,y,z),这些点的坐标离一个空间平面比较近 x0=1,L1=2; y0=1,L2=2; x=x0+rand(20,1)*L1; y=y0+rand(20,1)*L2; z=1+2*x+3*y; scatter3(x,y,z,'filled') hold on;   planeData=[x,y,z];   % 协方差矩阵的SVD变换中，最小奇异值对应的奇异向量就是平面的方向 xyz0=mean(planeData,1); centeredPlane=bsxfun(@minus,planeData,xyz0); [U,S,V]=svd(centeredPlane);   a=V(1,3); b=V(2,3); c=V(3,3); d=-dot([a b c],xyz0);   % 图形绘制 xfit = min(x):0.1:max(x); yfit = min(y):0.1:max(y); [XFIT,YFIT]= meshgrid (xfit,yfit); ZFIT = -(d + a * XFIT + b * YFIT)/c; mesh(XFIT,YFIT,ZFIT); 复制代码输出结果如图2 而根据空间点拟合一条空间直线的思路比较直接，就是最小化这些点到直线的距离 % 随机生成一组点，这写点距离直线l比较近，l的过点[1,1,1]，方向向量为[1,2,3] lineData=bsxfun(@plus, [1,1,1], (-1:.1:1).'*[1,2,3]); Noise=rand(size(lineData))*.1; lineData=lineData+Noise; scatter3(lineData(:,1), lineData(:,2), lineData(:,3),'filled') hold on;   % 拟合的直线必过所有坐标的算数平均值 xyz0=mean(lineData,1),   % 协方差矩阵奇异变换，与拟合平面不同的是 % 所得直线的方向实际上与最大奇异值对应的奇异向量相同 centeredLine=bsxfun(@minus,lineData,xyz0); [U,S,V]=svd(centeredLine); direction=V(:,1);   % 画图 t=-8:0.1:8; xx=xyz0(1)+direction(1)*t; yy=xyz0(2)+direction(2)*t; zz=xyz0(3)+direction(3)*t; plot3(xx,yy,zz) 复制代码输出结果如图3

 

图1：线性回归拟合平面

 

图2：最小距离平方拟合平面

图3： 最小距离平方拟合空间直线

转：http://www.ilovematlab.cn/thread-220252-1-1.html

opencv的一些函数来对平面进行拟合。

[cpp] view plain copy

1. //Ax+by+cz=D  
2. void cvFitPlane(const CvMat* points, float* plane){  
3.     // Estimate geometric centroid.  
4.     int nrows = points->rows;  
5.     int ncols = points->cols;  
6.     int type = points->type;  
7.     CvMat* centroid = cvCreateMat(1, ncols, type);  
8.     cvSet(centroid, cvScalar(0));  
9.     for (int c = 0; c
10.         for (int r = 0; r < nrows; r++)  
11.         {  
12.             centroid->data.fl[c] += points->data.fl[ncols*r + c];  
13.         }  
14.         centroid->data.fl[c] /= nrows;  
15.     }  
16.     // Subtract geometric centroid from each point.  
17.     CvMat* points2 = cvCreateMat(nrows, ncols, type);  
18.     for (int r = 0; r
19.         for (int c = 0; c
20.             points2->data.fl[ncols*r + c] = points->data.fl[ncols*r + c] - centroid->data.fl[c];  
21.     // Evaluate SVD of covariance matrix.  
22.     CvMat* A = cvCreateMat(ncols, ncols, type);  
23.     CvMat* W = cvCreateMat(ncols, ncols, type);  
24.     CvMat* V = cvCreateMat(ncols, ncols, type);  
25.     cvGEMM(points2, points, 1, NULL, 0, A, CV_GEMM_A_T);  
26.     cvSVD(A, W, NULL, V, CV_SVD_V_T);  
27.     // Assign plane coefficients by singular vector corresponding to smallest singular value.  
28.     plane[ncols] = 0;  
29.     for (int c = 0; c
30.         plane[c] = V->data.fl[ncols*(ncols - 1) + c];  
31.         plane[ncols] += plane[c] * centroid->data.fl[c];  
32.     }  
33.     // Release allocated resources.  
34.     cvReleaseMat(¢roid);  
35.     cvReleaseMat(&points2);  
36.     cvReleaseMat(&A);  
37.     cvReleaseMat(&W);  
38.     cvReleaseMat(&V);  
39. }  

调用的方式：

[cpp] view plain copy

1. CvMat*points_mat = cvCreateMat(X_vector.size(), 3, CV_32FC1);//定义用来存储需要拟合点的矩阵   
2.         for (int i=0;i < X_vector.size(); ++i)  
3.         {  
4.             points_mat->data.fl[i*3+0] = X_vector[i];//矩阵的值进行初始化   X的坐标值  
5.             points_mat->data.fl[i * 3 + 1] = Y_vector[i];//  Y的坐标值  
6.             points_mat->data.fl[i * 3 + 2] = Z_vector[i];//  Z的坐标值  
7.   
8.         }  
9.         float plane12[4] = { 0 };//定义用来储存平面参数的数组   
10.         cvFitPlane(points_mat, plane12);//调用方程   

我们拟合出来的方程：Ax+By+Cz=D

其中 A=plane12[0],    B=plane12[1],   C=plane12[2],   D=plane12[3],

这是要注意的方程的表示

 

RANSAC介绍（Matlab版直线拟合+平面拟合）

一、RANSAC介绍

   随机抽样一致算法（RANdom SAmple Consensus,RANSAC）,采用迭代的方式从一组包含离群的被观测数据中估算出数学模型的参数。维基百科中的RANSAC该算法最早由Fischler和Bolles于1981年提出。RANSAC算法假设数据中包含正确数据和异常数据（或称为噪声）。正确数据记为内点（inliers），异常数据记为外点（outliers）。同时RANSAC也假设，给定一组正确的数据，存在可以计算出符合这些数据的模型参数的方法。该算法核心思想就是随机性和假设性，随机性是根据正确数据出现概率去随机选取抽样数据，根据大数定律，随机性模拟可以近似得到正确结果。假设性是假设选取出的抽样数据都是正确数据，然后用这些正确数据通过问题满足的模型，去计算其他点，然后对这次结果进行一个评分。 
  RANSAC算法被广泛应用在计算机视觉领域和数学领域，例如直线拟合、平面拟合、计算图像或点云间的变换矩阵、计算基础矩阵等方面，使用的非常多。本文将在对RANSAC介绍完后，附两段直线拟合以及平面拟合的matlab代码。关于计算机视觉中基于RANSAC框架的矩阵求解问题，在OpenCV中都有对应的函数接口，如果以后有机会再把这个整理出来。

二、算法基本思想

  如下图所示，存在很多离散的点，而我们认为这些点构成了一条直线。当然，人眼能很清晰地拟合出这条直线，找到外点。但要让计算机找到这条直线，在很久之前是很难的，RACSAC的出现是通过数学之美解决这一难题的重要发明。 

通过实例对算法基本思想进行描述：

（1）首先，我们知道要得到一个直线模型，我们需要两个点唯一确定一个直线方程。所以第一步我们随机选择两个点。 
（2）通过这两个点，我们可以计算出这两个点所表示的模型方程y=ax+b。 
（3）我们将所有的数据点套到这个模型中计算误差。 
（4）我们找到所有满足误差阈值的点，第4幅图中可以看到，只有很少的点支持这个模型。 
（5）然后我们再重复（1）~（4）这个过程，直到达到一定迭代次数后，我们选出那个被支持的最多的模型，作为问题的解。如下图所示： 

以上是对RANSAC算法的基本思想的介绍，我们可以发现，虽然这个数据集中外点和内点的比例几乎相等，但是RANSAC算法还是能找到最合适的解。试想一下，这个问题如果使用最小二乘法进行优化，由于噪声数据的干扰，我们得到的结果肯定是一个错误的结果，如下图所示。这是由于最小二乘法是一个将外点参与讨论的代价优化问题，而RANSAC是一个使用内点进行优化的问题。经实验验证，对于包含80%误差的数据集，RANSAC的效果远优于直接的最小二乘法。 

三、RANSAC的数学推导

  我们假设内点在整个数据集中的概率为t，即： 

确定该问题的模型需要n个点，这个n是根据问题而定义的，例如拟合直线时n为2，平面拟合时n=3，求解点云之间的刚性变换矩阵时n=3，求解图像之间的射影变换矩阵是n=4，等等。k表示迭代次数，即随机选取n个点计算模型的次数。P为在这些参数下得到正确解的概率。为方便表示，我们可以得到，n个点都是内点的概率为，则n个点中至少有一个是外点的概率为 

表示k次随机抽样中都没有找到一次全是内点的情况，这个时候得到的是错误解，也就是： 

内点概率t是一个先验值，可以给出一些鲁棒的值。同时也可以看出，即使t给的过于乐观，也可以通过增加迭代次数k，来保证正确解的概率P。同样的，我们可以通过上面式子计算出来迭代次数k，即我们假设需要正确概率为P（例如我们需要99%的概率取到正确解），则： 

 

四、利用RANSAC拟合直线

1. clc;clear all;close all;

2.  

3. %%%二维直线拟合

4. %%%生成随机数据

5. %内点

6. mu=[0 0]; %均值

7. S=[1 2.5;2.5 8]; %协方差

8. data1=mvnrnd(mu,S,200); %产生200个高斯分布数据

9. %外点

10. mu=[2 2];

11. S=[8 0;0 8];

12. data2=mvnrnd(mu,S,100); %产生100个噪声数据

13. %合并数据

14. data=[data1',data2'];

15. iter = 100;

16.  

17. %%% 绘制数据点

18. figure;plot(data(1,:),data(2,:),'o');hold on; % 显示数据点

19. number = size(data,2); % 总点数

20. bestParameter1=0; bestParameter2=0; % 最佳匹配的参数

21. sigma = 1;

22. pretotal=0; %符合拟合模型的数据的个数

23.  

24. for i=1:iter

25. %%% 随机选择两个点

26. idx = randperm(number,2);

27. sample = data(:,idx);

28.  

29. %%%拟合直线方程 y=kx+b

30. line = zeros(1,3);

31. x = sample(:, 1);

32. y = sample(:, 2);

33.  

34. k=(y(1)-y(2))/(x(1)-x(2)); %直线斜率

35. b = y(1) - k*x(1);

36. line = [k -1 b]

37.  

38. mask=abs(line*[data; ones(1,size(data,2))]); %求每个数据到拟合直线的距离

39. total=sum(mask

40.  

41. if total>pretotal %找到符合拟合直线数据最多的拟合直线

42. pretotal=total;

43. bestline=line; %找到最好的拟合直线

44. end

45. end

46. %显示符合最佳拟合的数据

47. mask=abs(bestline*[data; ones(1,size(data,2))])

48. hold on;

49. k=1;

50. for i=1:length(mask)

51. if mask(i)

52. inliers(1,k) = data(1,i);

53. k=k+1;

54. plot(data(1,i),data(2,i),'+');

55. end

56. end

57.  

58. %%% 绘制最佳匹配曲线

59. bestParameter1 = -bestline(1)/bestline(2);

60. bestParameter2 = -bestline(3)/bestline(2);

61. xAxis = min(inliers(1,:)):max(inliers(1,:));

62. yAxis = bestParameter1*xAxis + bestParameter2;

63. plot(xAxis,yAxis,'r-','LineWidth',2);

64. title(['bestLine: y = ',num2str(bestParameter1),'x + ',num2str(bestParameter2)]);

结果如图所示：

五、利用RANSAC拟合平面

1. clc;clear all;close all;

2.  

3. %%%三维平面拟合

4. %%%生成随机数据

5. %内点

6. mu=[0 0 0]; %均值

7. S=[2 0 4;0 4 0;4 0 8]; %协方差

8. data1=mvnrnd(mu,S,300); %产生200个高斯分布数据

9. %外点

10. mu=[2 2 2];

11. S=[8 1 4;1 8 2;4 2 8]; %协方差

12. data2=mvnrnd(mu,S,100); %产生100个噪声数据

13. %合并数据

14. data=[data1',data2'];

15. iter = 1000;

16.  

17. %%% 绘制数据点

18. figure;plot3(data(1,:),data(2,:),data(3,:),'o');hold on; % 显示数据点

19. number = size(data,2); % 总点数

20. bestParameter1=0; bestParameter2=0; bestParameter3=0; % 最佳匹配的参数

21. sigma = 1;

22. pretotal=0; %符合拟合模型的数据的个数

23.  

24. for i=1:iter

25. %%% 随机选择三个点

26. idx = randperm(number,3);

27. sample = data(:,idx);

28.  

29. %%%拟合直线方程 z=ax+by+c

30. plane = zeros(1,3);

31. x = sample(:, 1);

32. y = sample(:, 2);

33. z = sample(:, 3);

34.  

35. a = ((z(1)-z(2))*(y(1)-y(3)) - (z(1)-z(3))*(y(1)-y(2)))/((x(1)-x(2))*(y(1)-y(3)) - (x(1)-x(3))*(y(1)-y(2)));

36. b = ((z(1) - z(3)) - a * (x(1) - x(3)))/(y(1)-y(3));

37. c = z(1) - a * x(1) - b * y(1);

38. plane = [a b -1 c]

39.  

40. mask=abs(plane*[data; ones(1,size(data,2))]); %求每个数据到拟合平面的距离

41. total=sum(mask

42.  

43. if total>pretotal %找到符合拟合平面数据最多的拟合平面

44. pretotal=total;

45. bestplane=plane; %找到最好的拟合平面

46. end

47. end

48. %显示符合最佳拟合的数据

49. mask=abs(bestplane*[data; ones(1,size(data,2))])

50. hold on;

51. k = 1;

52. for i=1:length(mask)

53. if mask(i)

54. inliers(1,k) = data(1,i);

55. inliers(2,k) = data(2,i);

56. plot3(data(1,i),data(2,i),data(3,i),'r+');

57. k = k+1;

58. end

59. end

60.  

61. %%% 绘制最佳匹配平面

62. bestParameter1 = bestplane(1);

63. bestParameter2 = bestplane(2);

64. bestParameter3 = bestplane(4);

65. xAxis = min(inliers(1,:)):max(inliers(1,:));

66. yAxis = min(inliers(2,:)):max(inliers(2,:));

67. [x,y] = meshgrid(xAxis, yAxis);

68. z = bestParameter1 * x + bestParameter2 * y + bestParameter3;

69. surf(x, y, z);

70. title(['bestPlane: z = ',num2str(bestParameter1),'x + ',num2str(bestParameter2),'y + ',num2str(bestParameter3)]);

结果如图所示：

 

 

 

任意向量通过平面法线,计算投影向量

http://math.oregonstate.edu/home/programs/undergrad/CalculusQuestStudyGuides/vcalc/dotprod/dotprod.html

任意平面上的投影

已知向量q，平面法线n，求q在法线为n的平面上的投影向量t

 

示意图

通过公式很容易就能得出

    Vector3 ProjectOnPlane(Vector3 vp,Vector3 vn)

    {

        Vector3 vt = new Vector3();

        vt = vp - vn * Vector3.Dot(vp, vn) / Vector3.Dot(vn, vn);

        return vt;

    }

其中的Vector3代表一个三维数据结构(x,y,z)

其中Vector3.Dot(v1,v2)代表的是向量的点乘,如果大家不知道点乘就可以看下面这段

点乘  :   v1 ⋅v2  = value;

v1是一个向量,v2也是一个向量,而点乘的结果value是一个浮点数(float,double)

点乘的空间意义,如果v1,v2都是单位向量(即长度为1的向量)那么v1 ⋅v2的结果可以反映两个向量的方向信息

当value值为1的时候,代表v1 v2的方向相同

当value值为0的时候代表v1 v2是互相垂直的

当value值为-1的时候代表v1 v2是反向的

很多人会把点乘和叉乘混淆,两个的意义和结果是完全不一样的。