当前位置:   article > 正文

利用Eigen库实现最小二乘拟合平面_eigen最小二乘拟合直线

eigen最小二乘拟合直线

本文主要介绍最小二乘的直接求解法和SVD分解法,利用Eigen库进行矩阵运算求解。 假设有一散点集合,当各点v到一平面S的距离d的平方和最小,该平面s即为最小二乘下的最优解。将距离平方和描述为平面拟合误差

e=\sum_{i=1}^{n} d_{i}^{2}                            (1)

定义点类如下

  1. class vec
  2. {
  3. public:
  4. float v[3];
  5. vec(){}
  6. vec(float x,float y ,float z)
  7. {
  8. v[0] = x; v[1] = y; v[2] = z;
  9. }
  10. const float &operator [] (int i) const
  11. {
  12. return v[i];
  13. }
  14. float &operator [] (int i)
  15. {
  16. return v[i];
  17. }
  18. };

一、直接求解法

平面方程表示为A x+B y+C z+D=0(C \neq 0) 

两边同时除以C,-\frac{A}{C} x-\frac{B}{C} \mathrm{y}-\frac{D}{C}=z

a_{0}=-\frac{A}{C}, a_{1}=-\frac{B}{C}, a_{2}=-\frac{D}{C}

方程则可以表示为a_{0} x+a_{1} y+a_{2}=z

先介绍下用定义求解的方式:

(1)式误差可以描述为e=\sum_{i=0}^{n-1}(a 0 * x+a 1 * y+a 2-z)^{2}

要使e最小,应满足\frac{\partial e}{\partial a_{k}}=0, \quad \mathrm{k}=0,1,2

列出如下方程

\left\{\begin{array}{l} \sum 2\left(a_{0}x_{i}+a_{1} y_{i}+a_{2}-z_{i}\right) x_{i}=0 \\ \sum 2\left(a_{0} x_{i}+a_{1} y_{i}+a_{2}-z_{i}\right)y_{i}=0 \\ \sum 2\left(a_{0} x_{i}+a_{1} y_{i}+a_{2}-z_{i}\right)=0 \end{array}\right.

改写为矩阵

\left|\begin{array}{ccc} \Sigma \mathrm{x}_{\mathrm{i}}^{2} & \Sigma_{\mathrm{x}_{i} y_{i}} & \Sigma \mathrm{x}_{\mathrm{i}} \\ \Sigma \mathbf{x}_{\mathrm{i}} y_{i} & \Sigma \mathrm{y}_{\mathrm{i}}^{2} & \Sigma \mathrm{y}_{\mathrm{i}} \\ \Sigma_{\mathrm{x}_{\mathrm{i}}} & \Sigma_{\mathrm{y}_{\mathrm{i}}} & \mathrm{n} \end{array}\right|\left[\begin{array}{l} a_{0} \\ a_{1} \\ a_{2} \end{array}\right]=\left[\begin{array}{c} \Sigma_{\mathrm{x}_{i} z_{i}} \\ \Sigma \mathbf{y}_{\mathrm{i}} z_{i} \\ \Sigma_{\mathbf{z}_{\mathrm{i}}} \end{array}\right]

求解上述矩阵获得a0,a1,a2的值,即可求得平面方程。代码如下

  1. void ComputePlane(const vector<vec>&points, float &A, float &B, float &C, float &D)
  2. {
  3. int n = points.size();
  4. MatrixXd mleft = MatrixXd::Zero(3, 3);//初始化左边项
  5. VectorXd b = VectorXd::Zero(3); //初始化右边项
  6. for (int i = 0; i < n; ++i)//遍历点装填两个矩阵
  7. {
  8. mleft(0, 0) += points[i][0] * points[i][0];//a11,xi平方和
  9. mleft(0, 1) += points[i][0] * points[i][1];//a12=a21,xiyi和
  10. mleft(0, 2) += points[i][0];//a13=a31,xi和
  11. mleft(1, 1) += points[i][1] * points[i][1];//a22,yi平方和
  12. mleft(1, 2) += points[i][1];//a23=a32,yi和
  13. b[0] += points[i][0] * points[i][2];//xizi和
  14. b[1] += points[i][1] * points[i][2];//yizi和
  15. b[2] += points[i][2];//zi和
  16. }
  17. mleft(2, 2) = n;//a33,n
  18. mleft(1, 0) = mleft(0, 1);
  19. mleft(2, 0) = mleft(0, 2);
  20. mleft(2, 1) = mleft(1, 2);
  21. VectorXd x = mleft.fullPivHouseholderQr().solve(b);//QR分解求解
  22. float a0 = x[0]; float a1 = x[1]; float a2 = x[2];//为了便于理解,赋值给临时变量
  23. A = a0; B = a1; C = -1; D = a2;
  24. }

然而这种方式还需要计算左边项和右边项各元素的值,相对繁琐,Eigen库可以直接求解超定方程组,左边项为散点x,y值与1,右边项为z值,列出如下方程组直接求解得到的即是最小二乘解。

\left[\begin{array}{ccc} x_{1} & y_{1} & 1 \\ x_{2} & y_{2} & 1 \\ \vdots & \vdots & \vdots \\ x_{n} & y_{n} & 1 \end{array}\right]\left[\begin{array}{l} a_{0} \\ a_{1} \\ a_{2} \end{array}\right]=\left[\begin{array}{c} z_{1} \\ z_{2} \\ \vdots \\ z_{n} \end{array}\right]

实现代码如下,传入点集points和待求解平面系数ABCD。不论是采用定义法建立3*3的左边项还是解如下超定方程组,获得的结果是一样的,证明Eigen库求解超静定问题内部采用的是最小二乘的思想。

  1. void ComputePlane(const vector<vec>&points, float &A, float &B, float &C, float &D)
  2. {
  3. int n = points.size();
  4. MatrixXd mleft = MatrixXd::Ones(n, 3);//初始化左边项
  5. VectorXd b = VectorXd::Zero(n); //初始化右边项
  6. for (int i = 0; i < n; ++i)//遍历点装填两个矩阵
  7. {
  8. mleft(i, 0) = points[i][0];
  9. mleft(i, 1) = points[i][1];
  10. b[i] = points[i][2];
  11. }
  12. VectorXd x = mleft.fullPivHouseholderQr().solve(b);//QR分解求解
  13. float a0 = x[0]; float a1 = x[1]; float a2 = x[2];//为了便于理解,赋值给临时变量
  14. A = a0; B = a1; C = -1; D = a2;
  15. }

二、SVD分解法

平面方程表示为ax+by+cz = d,(1)

约束条件为a^{2}+b^{2}+c^{2}=1

计算散点坐标的平均值,应满足a \bar{x}+b \bar{y}+c \bar{z}=d (2)

(1)式代入各个散点,(1)-(2)得(3)

列为矩阵,式(3)等价于AX = 0

A=\left[\begin{array}{ccc} x_{1}-\bar{x} & y_{1}-\bar{y} & z_{1}-\bar{z} \\ x_{2}-\bar{x} & y_{2}-\bar{y} & z_{2}-\bar{z} \\ \vdots & \vdots & \vdots \\ x_{n}-\bar{x} & y_{n}-\bar{y} & z_{n}-\bar{z} \end{array}\right], X=\left[\begin{array}{l} a \\ b \\ c \end{array}\right]

目标函数为\min \|A X\|,约束条件\|X\|=1,对做奇异值分解A=U D V^{T},则\|A X\|=\left\|U D V^{T} X\right\|=\left\|D V^{T} X\right\|,且\left\|V^{T} X\right\|=\|X\|=1,假设D的最后一个对角元素为最小奇异值,当且仅当

V^{T} X=\left[\begin{array}{c}0 \\ 0 \\ \vdots \\ 1\end{array}\right]

时,取得\min \|A X\|,此时最小奇异值对应的特征向量即是平面的法向量,即a,b,c。

在Eigen库中,有现成的JacobiSVD类供调用,代码实现如下

  1. void ComputePlane(const vector<vec>&points, float &A, float &B, float &C, float &D)
  2. {
  3. int n = points.size();
  4. MatrixXd mleft = MatrixXd::Zero(n, 3);//初始化矩阵A
  5. vec averagepoint(0, 0, 0);//平均坐标
  6. for (int i = 0; i < n; ++i)
  7. {
  8. averagepoint[0] += points[i][0] / n;
  9. averagepoint[1] += points[i][1] / n;
  10. averagepoint[2] += points[i][2] / n;
  11. }
  12. for (int i = 0; i < n; ++i)//遍历点装填矩阵
  13. {
  14. mleft(i, 0) = points[i][0] - averagepoint[0];
  15. mleft(i, 1) = points[i][1] - averagepoint[1];
  16. mleft(i, 2) = points[i][2] - averagepoint[2];
  17. }
  18. JacobiSVD<MatrixXd> svd(mleft, ComputeFullU | ComputeFullV);//对矩阵A分解
  19. MatrixXd vt = svd.matrixV().transpose();//获得矩阵v的转置
  20. Vector3d b(0, 0, 1);
  21. VectorXd x = vt.fullPivHouseholderQr().solve(b);
  22. A = x[0]; B = x[1]; C = x[2];
  23. D = A*averagepoint[0] + B*averagepoint[1] + C * averagepoint[2];
  24. }

main函数的内容和输出结果如下。输入随机点个数n,随机点的坐标值为0-10的浮点型,输出点坐标集和最后拟合的平面方程。

  1. int main()
  2. {
  3. vector<vec>points;
  4. int n = 0;
  5. cin >> n;
  6. for (int i = 0; i < n;i++)
  7. {
  8. float x, y, z;//在0-1内取随机数
  9. x = double(rand()) / RAND_MAX*10;
  10. y = double(rand()) / RAND_MAX*10;
  11. z = double(rand()) / RAND_MAX*10;
  12. points.push_back(vec(x,y,z));
  13. }
  14. for (int i = 0; i < n; i++)
  15. {
  16. cout << "(" << points[i][0] << "," << points[i][1] << "," << points[i][2] << ")" << endl;
  17. }
  18. float A, B, C, D;
  19. ComputePlane(points, A, B, C, D);
  20. cout << "平面方程为 "<<A << "x+" << B << "y+" << C << "z =" << D << endl;
  21. system("pause");
  22. return 0;
  23. }

在做自己的项目过程中,发现svd分解法得到的平面更加准确,如下柱状图拟合1000个平面,横坐标表示平面的序号,纵坐标表示拟合点距离该平面的最大距离,左边是svd分解的结果,最大值在1.2以下,而右边直接求解的结果大于1.2的平面很多,当然检查它们的距离平方和结果也是一样的,svd同样优于直接求解,感兴趣的可以自行验证下。

参考链接

最小二乘拟合平面(C++版) - 知乎

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/很楠不爱3/article/detail/78158
推荐阅读
相关标签
  

闽ICP备14008679号