3D点云处理：用SVD分解法和最小二乘法拟合平面点云，求解平面方程_svd平面拟合

学习目标：

本文主要介如何用SVD分解法和最小二乘法拟合平面点云，包含原理推导和代码

---

1.SVD分解法求解平面点云

1.1问题描述

1. 将空间中的离散点拟合为一个平面，就是使离散点到某个平面距离和最小的问题，可以将求解过程看作最优化的过程。
2. 一个先验知识为拟合平面一定经过离散点的质心（离散点坐标的平均值）。平面方程可以通过求解求解平面的法向量来获得。根据协方差矩阵的SVD变换，最小奇异值对应的奇异向量就是平面的方向。
   注意：这个方法是直接的计算方法，没办法解决数值计算遇到的病态矩阵问题.在公式转化代码之前必须对空间点坐标进行近似归一化！

1.2问题建模：

已知若干三维点坐标$(x_i,y_i,z_i)$,拟合出平面方程$ax+by+cz=d$ (1)
约束条件为$a^2+b^2+c^2=1$ (2)
目标为使该平面到所有点的距离之和最小

1.3推导：

所有点的平均坐标为$(\bar{x},\bar{y},\bar{z})$,则由先验知识（拟合平面一定经过离散点的质心），可以到以下方程$a\bar{x}+b\bar{y}+c\bar{z}=d.$(3)

式（1）与式（3）相减，得 $a(x_i-\bar{x})+b(y_i-\bar{y})+c(z_i-\bar{z})=0$(4)

将所有点数据带入式4，整理可以得到矩阵 A = [ x 1 − x ˉ , y 1 − y ˉ , z 1 − z ˉ x 2 − x ˉ , y 2 − y ˉ , z 2 − z ˉ x 3 − x ˉ , y 3 − y ˉ , z 3 − z ˉ . . . x n − x ˉ , y n − y ˉ , z n − z ˉ ] A=

$$\begin{bmatrix} x_{1}-\bar{x},y_{1}-\bar{y},z_{1}-\bar{z}\\ x_{2}-\bar{x},y_{2}-\bar{y},z_{2}-\bar{z}\\ x_{3}-\bar{x},y_{3}-\bar{y},z_{3}-\bar{z}\\ ...\\ x_{n}-\bar{x},y_{n}-\bar{y},z_{n}-\bar{z} \end{bmatrix}$$ A= ​x1​−xˉ,y1​−yˉ​,z1​−zˉx2​−xˉ,y2​−yˉ​,z2​−zˉx3​−xˉ,y3​−yˉ​,z3​−zˉ...xn​−xˉ,yn​−yˉ​,zn​−zˉ​ ​,列矩阵$X=\left[\begin{array}{c}a\\ b\\ c\end{array}\right]$ ,则式（4）等价与AX=0 (5)

理想情况下所有点都在平面上，式（5）成立；实际情况下有部分点在平面外，拟合的目的为平面距离所有点的距离之和尽量小，所以目标函数为 $min\Vert AX\Vert$ (6)

约束条件为$\Vert X\Vert =1$ (7)

若A可做奇异值分解：$A=UD{V}^T$ (8)

其中，D是对角矩阵，U和V均为酉矩阵。

则 $\Vert AX\Vert =\Vert UD{V}^{T}X\Vert =\Vert D{V}^{T}X\Vert$ (9)

其中为列矩阵，并且 $\Vert V^{T}X\Vert =\Vert X\Vert =1$ (10)

因为D的对角元素为奇异值，假设最后一个对角元素为最小奇异值，则当且仅当
V T X = [ 0 0 0 . . . 1 ] V^{T}X=

$$\begin{bmatrix} 0\\ 0\\ 0\\ ...\\ 1 \end{bmatrix}$$ VTX= ​000...1​ ​（11）时，式（9）可以取得最小值，即式(6)成立。

此时 X = V [ 0 0 0 . . . 1 ] = [ v 1 v 2 v 3 . . . v n ] [ 0 0 0 . . . 1 ] = v n X=V

$$\begin{bmatrix} 0\\ 0\\ 0\\ ...\\ 1 \end{bmatrix}$$ = $$\begin{bmatrix} v_{1} &v_{2} &v_{3} &... &v_{n} \end{bmatrix}$$ $$\begin{bmatrix} 0\\ 0\\ 0\\ ...\\ 1 \end{bmatrix}$$ =v_{n} X=V ​000...1​ ​=[v1​​v2​​v3​​...​vn​​] ​000...1​ ​=vn​ (12)

所以，目标函数（6）在约束条件（7）下的最优解为 $X=(a,b,c)=(v_{n,1},v_{n,2},v_{n,3})$ (13)

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

1.4 伪代码

1 读取点云数据
2 求解点云质心(x0,y0,z0)
3 将原始点云坐标减去点云质心，构建矩阵A
4 对矩阵A进行SVD分解，A = U * S * VT
5 V最后一列对应为(A,B,C); D=-(Ax0+By0+C*z0)

1.5 使用opencv进行求解

void fitPlane(cv::Mat &points,cv::Mat &plane)
//points输入点云
//plane输出平面方程系数
{
	cv::Mat centor = cv::Mat::zeros(1,points.cols,CV_32FC1);
	for(int i =0;i < points.cols;++i)
	{
		for(int j =0;j < points.rows;++j)
		{
			centor.at<float>(0,i) = centor.at<float>(0,i) + points.at<float>(j,i);  
		}
		centor.at<float>(0.i) = centor.at<float>(0.i) / points.rows;
	}
	cv::Mat pointC = cv::Mat::ones(points.rows,points.cols,CV_32FC1);
	for(int i =0;i < points.cols;++i)
	{
		for(int j =0;j < points.rows;++j)
		{
			pointC.at<float>(j,i) =  points.at<float>(j,i) - centor.at<float>(0.i);
		}
	}
	//SVD分解
	cv::Mat A,W,U,V;
	//构建奇异值矩阵(gemm矩阵相乘)
	SVD::compute(pointC,W,U,V);
	// 提取最小奇异值和相应的向量
    double min_sing_val = W.at<double>(svd.w.rows-1); // 最后一个元素为最小奇异值
    Mat min_sing_vec = V.row(V.rows-1); // 最后一行为最小奇异值对应的右奇异向量
	plane[0]=min_sing_vec[0]
	plane[1]=min_sing_vec[1]
	plane[2]=min_sing_vec[2]
	plane[4]= -(plane[0]*centor.at<float>(0,0)+plane[1]*centor.at<float>(0,1)+plane[2]*centor.at<float>(0,2)
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34

1.6 使用Eigen进行求解

void FitPlaneSVD::compute()
{
	// 1、计算质心
	Eigen::RowVector3d centroid = m_cloud.colwise().mean();
	// 2、去质心
	Eigen::MatrixXd demean = m_cloud;
	demean.rowwise() -= centroid;
	// 3、SVD分解求解协方差矩阵的特征值特征向量
	Eigen::JacobiSVD<Eigen::MatrixXd> svd(demean, Eigen::ComputeThinU | Eigen::ComputeThinV);
	Eigen::Matrix3d V = svd.matrixV();
	Eigen::MatrixXd U = svd.matrixU();
	Eigen::Matrix3d S = U.inverse() * demean * V.transpose().inverse();
	// 5、平面的法向量a,b,c
	Eigen::RowVector3d normal;
	normal << V(0,2), V(1,2), V(2,2);
	// 6、原点到平面的距离d
	double d = -normal * centroid.transpose();
	// 7、获取拟合平面的参数a,b,c,d和质心x,y,z。
	m_planeparameters << normal, d, centroid
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

---

2.最小二乘法求解平面点云

2.1问题描述

找到一个平面
Z=Ax+By+C
根据最小二乘法，使各个点到这个平面的距离最近：
S=∑(Axi + Byi + C - Zi) 2

2.2问题描述

2.3问题使用opencv进行求解

void CaculateLaserPlane(std::vector<cv::Point3f> Points3ds,vector<double> &res)
{
	//最小二乘法拟合平面
	//获取cv::Mat的坐标系以纵向为x轴，横向为y轴，而cvPoint等则相反
	//系数矩阵
	cv::Mat A = cv::Mat::zeros(3, 3, CV_64FC1);
	//
	cv::Mat B = cv::Mat::zeros(3, 1, CV_64FC1);
	//结果矩阵
	cv::Mat X = cv::Mat::zeros(3, 1, CV_64FC1);
	double x2 = 0, xiyi = 0, xi = 0, yi = 0, zixi = 0, ziyi = 0, zi = 0, y2 = 0;
	for (int i = 0; i < Points3ds.size(); i++)
	{
		x2 += (double)Points3ds[i].x * (double)Points3ds[i].x;
		y2 += (double)Points3ds[i].y * (double)Points3ds[i].y;
		xiyi += (double)Points3ds[i].x * (double)Points3ds[i].y;
		xi += (double)Points3ds[i].x;
		yi += (double)Points3ds[i].y;
		zixi += (double)Points3ds[i].z * (double)Points3ds[i].x;
		ziyi += (double)Points3ds[i].z * (double)Points3ds[i].y;
		zi += (double)Points3ds[i].z;
	}
	A.at<double>(0, 0) = x2;
	A.at<double>(1, 0) = xiyi;
	A.at<double>(2, 0) = xi;
	A.at<double>(0, 1) = xiyi;
	A.at<double>(1, 1) = y2;
	A.at<double>(2, 1) = yi;
	A.at<double>(0, 2) = xi;
	A.at<double>(1, 2) = yi;
	A.at<double>(2, 2) = Points3ds.size();
	B.at<double>(0, 0) = zixi;
	B.at<double>(1, 0) = ziyi;
	B.at<double>(2, 0) = zi;
	//计算平面系数
	X = A.inv() * B;
	//A
	res.push_back(X.at<double>(0, 0));
	//B
	res.push_back(X.at<double>(1, 0));
	//Z的系数为1
	res.push_back(1.0);
	//C
	res.push_back(X.at<double>(2, 0));
	return;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47

---

参考链接

OpenCV最小二乘法拟合空间平面
最小二乘法拟合平面
点云拟合—平面拟合
SVD解决平面拟合问题