3D point cloud processing: use SVD decomposition method and least square method to fit plane point cloud and solve plane equation

learning target:

This article mainly introduces how to use the SVD decomposition method and the least squares method to fit the plane point cloud, including principle derivation and code

---

1. SVD decomposition method to solve planar point cloud

1.1 Problem description

1. Fitting discrete points in space to a plane is the problem of minimizing the sum of distances from discrete points to a certain plane, and the solution process can be regarded as an optimization process.
2. A priori knowledge is that the fitting plane must pass through the centroid of the discrete point (the average value of the coordinates of the discrete point). The plane equation can be obtained by solving for the normal vector of the solution plane. According to the SVD transformation of the covariance matrix, the singular vector corresponding to the smallest singular value is the direction of the plane.
   Note: This method is a direct calculation method, and there is no way to solve the ill-conditioned matrix problem encountered in numerical calculation. The coordinates of the spatial points must be approximately normalized before the formula is converted into code!

1.2 Problem modeling:

Known coordinates of several three-dimensional points $(x_i,y_i,z_i)$ , fit the plane equation$ax+by+cz=d$ (1)
The constraints are$a^2+b^2+c^2=1$ (2)
The goal is to minimize the sum of distances from the plane to all points

1.3 Derivation:

The average coordinates of all points are $(\bar{x},\bar{y},\bar{z})$, then from the prior knowledge (the fitting plane must pass through the centroid of discrete points), the following equation$a\bar{x}+b\bar{y}+c\bar{z}=d.$(3)

Subtract formula (1) from formula (3), and get $a(x_i-\bar{x})+b(y_i-\bar{y})+c(z_i-\bar{z})=0$(4)

Put all the point data into formula 4, and organize to get the matrix $A=\left[\begin{array}{c}{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{array}\right]$, column matrix $X=\left[\begin{array}{c}a\\ b\\ c\end{array}\right]$, then formula (4) is equivalent to AX=0 (5)

Ideally, all points are on the plane, and formula (5) holds true; in reality, some points are out of the plane, and the purpose of fitting is to minimize the sum of the distances from the plane to all points, so the objective function is min ∥ AX $min\Vert AX\Vert$ (6)

The constraints are $\Vert X\Vert =1$ (7)

If A can do singular value decomposition: $A=UD{V}^T$ (8)

Among them, D is a diagonal matrix, and U and V are unitary matrices.

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

where is a column matrix, and $\Vert V^{T}X\Vert =\Vert X\Vert =1$ (10)

Because the diagonal elements of D are singular values, assuming that the last diagonal element is the smallest singular value, then if and only if
$V^{T}X=\left[\begin{array}{c}0\\ 0\\ 0\\ ...\\ 1\end{array}\right]$(11), formula (9) can obtain the minimum value, that is, formula (6) is established.

此时 $X=V\left[\begin{array}{c}0\\ 0\\ 0\\ ...\\ 1\end{array}\right]=\left[\begin{array}{ccccc}{v}_1& v_2& v_3& ...& v_n\end{array}\right]\left[\begin{array}{c}0\\ 0\\ 0\\ ...\\ 1\end{array}\right]=v_n$ (12)

Therefore, the optimal solution of the objective function (6) under the constraints (7) is $X=(a,b,c)=(v_{n,1},v_{n,2},v_{n,3})$ (13)

To sum up: Singular value decomposition is performed on the matrix A, and the eigenvector corresponding to the smallest singular value is the coefficient vector of the fitting plane.

1.4 Pseudocode

1 Read point cloud data
2 Solve point cloud centroid (x0, y0, z0)
3 Subtract point cloud centroid from original point cloud coordinates to construct matrix A
4 SVD decompose matrix A, A = U * S * VT
5 V The last column corresponds to (A,B,C); D=-(A x0+B y0+C*z0)

1.5 Use opencv to solve

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.6 Solve using 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
}

---

2. The least squares method to solve the planar point cloud

2.1 Problem description

Find a plane
Z=Ax+By+C
According to the least square method, make the distance from each point to this plane the shortest:
S=∑(Axi + Byi + C - Zi) 2

2.2 Problem description

2.3 The problem is solved using 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;
}

---

reference link

OpenCV Least Squares Fitting Space Plane
Least Squares Fitting Plane
Point Cloud Fitting—Plane Fitting
SVD Solving Plane Fitting Problems