C++使用最小二乘法计算点云法向量原理以及全部代码

最小二乘计算点云法向量原理

对于任意一点 $p(x,y,z)$查找其一定领域内的点 { p i } \{{p_i}\} {pi​}；

求得一个平面 $\prod :a_{0}x+a_{1}y+a_{2}z+1=0$使得其到平面距离的平方和最小，即：

$min{\sum }_{i=1}^{n}dist(p_i,\prod )$ = $min{\sum }_{i=1}^n(a_0{x}_i+a_1{y}_i+a_2{z}_i+1{)}^2$

记$S=min{\sum }_{i=1}^n(a_{0}x+a_{1}y+a_{2}z+1{)}^2$
要使得S最小，则$\frac{\partial S}{\partial a_0}=0,\frac{\partial S}{\partial b_0}=0,\frac{\partial S}{\partial c_0}=0$
即：
$\frac{\partial S}{\partial a_0}={\sum }_{i=1}^{n}2(a_{0}x+a_{1}y+a_{2}z+1)x_i=0$
$\frac{\partial S}{\partial b_0}={\sum }_{i=1}^{n}2(a_{0}x+a_{1}y+a_{2}z+1)y_i=0$
$\frac{\partial S}{\partial c_0}={\sum }_{i=1}^{n}2(a_{0}x+a_{1}y+a_{2}z+1)z_i=0$
上式经过变形可得（请读者自行下去推导）
$$\left[\begin{array}{ccc}\sum x_i^2& \sum x_i{y}_y& \sum x_i{z}_i\\ \sum x_i{y}_i& \sum y_i^2& \sum y_i{z}_i\\ \sum x_i{z}_i& \sum y_i{z}_i& \sum z_i^2\end{array}\right]\cdot \left[\begin{array}{c}{a}_0\\ b_0\\ c_0\end{array}\right]+\left[\begin{array}{c}\sum x_i\\ \sum y_i\\ \sum z_i\end{array}\right]=0$$
令$$A=\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ ⋮& ⋮& ⋮\\ x_n& y_n& z_n\end{array}\right];-b=\left[\begin{array}{c}1\\ 1\\ ⋮\\ 1\end{array}\right]$$
则有：$$A^{T}A\left[\begin{array}{c}{a}_0\\ b_0\\ c_0\end{array}\right]-A^{T}b=0$$
由于$A^{T}A$可逆
所以$$\left[\begin{array}{c}{a}_0\\ b_0\\ c_0\end{array}\right]=(A^{T}A{)}^{-1}{A}^{T}b$$
由高等数学等知识可知：$$\left[\begin{array}{c}{a}_0\\ b_0\\ c_0\end{array}\right]即为法向量$$

代码

void lsNormal(const PointCloud<PointXYZ>::Ptr& cloud, const int& k_neighbors, PointCloud<Normal>::Ptr& normals)
{
	//1. build kdtree index
	KdTreeFLANN<PointXYZ>::Ptr kdtree(new KdTreeFLANN<PointXYZ>);
	kdtree->setInputCloud(cloud);//set input cloud

	if (normals == NULL)
		normals = PointCloud<Normal>::Ptr(new PointCloud<Normal>);
	if (!normals->empty())
		normals->resize(0);

	for (const auto& point: * cloud)
	{
		// 3.1 set k nearest neighbors
		PointXYZ searchPoint = point;
		vector<int> KNNIndices(k_neighbors);//an int vector to store the k nearest neighbor of points
		vector<float> KNNSquareDistance(k_neighbors);//a float vector to store the distance of k nearest neighbor

		//3.2 least square for each point and calculate the normal vectors
		if (kdtree->nearestKSearch(searchPoint, k_neighbors, KNNIndices, KNNSquareDistance) > 0)
		{
			//if the search result > 0, search succeed
			//build a new point cloud, store the k nearest neighbor of current point
			PointCloud<PointXYZ>::Ptr neighborsPoints(new PointCloud<PointXYZ>);
			pcl::copyPointCloud(*cloud, KNNIndices, *neighborsPoints);

			size_t i = 0;
			MatrixXd A = MatrixXd::Random(k_neighbors, 3);
			MatrixXd b = MatrixXd::Random(k_neighbors, 1);
			MatrixXd X = MatrixXd::Random(3, 1);
			//A, b, X is correspond to A*X = b
			for (int idx = 0; idx < k_neighbors; idx++)
			{
				A(i, 0) = cloud->points[KNNIndices[idx]].x;
				A(i, 1) = cloud->points[KNNIndices[idx]].x;
				A(i, 2) = cloud->points[KNNIndices[idx]].x;
				b(i, 0) = -1;
				i = i + 1;
			}

			X = (A.transpose() * A).inverse() * A.transpose() * b;

			//Normalize
			double norm_xyz = sqrt(X(0) * X(0) + X(1) * X(1) + X(2) * X(2));
			double nx = X(0) / norm_xyz;
			double ny = X(1) / norm_xyz;
			double nz = X(2) / norm_xyz;
			normals->push_back(Normal(nx, ny, nz));
		}
		else
			normals->push_back(Normal());
	}

}