PCL：激光点云车道线检测及最小二乘法拟合

目前已经将车道线的最小二乘拟合及配合rviz可视化实现了。

数学部分

补充：拟合的数学模型是直线一般式

当x1≠x2，y1≠y2时，直线的斜率k=(y2-y1)/(x2-x1)
故直线方程为y-y1=(y2-y1)/(x2-x1)×(x-x1)
即x2y-x1y-x2y1+x1y1=(y2-y1)x-x1(y2-y1)
即(y2-y1)x-(x2-x1)y-x1(y2-y1)+(x2-x1)y1=0
即(y2-y1)x+(x1-x2)y+x2y1-x1y2=0 ①
可以发现，当x1=x2或y1=y2时，①式仍然成立。所以直线AX+BY+C=0的一般式方程就是：
A = Y2 - Y1
B = X1 - X2
C = X2*Y1 - X1*Y2

对于一元二次多项式，可以转换为线性方程组求解，我们一般写成矩阵形式 Ax = y。

Ax = y非一致方程和一致方程的求解
一致与非一致方程

    一致方程是指Ax = y有至少一个解
    非一致方程指Ax = y没有解

Ax = y求解

    如果A是满秩的方阵，则x = inv(A）*y

    如果A不是方阵，但是是行满秩或者列满秩，那么解为A的伪逆乘以y

    如果A是秩亏的，那么A的解为A的广义逆乘以y

实际上广义逆包括逆、伪逆，广义逆又称为：Moore-Penrose逆矩阵，所以Ax = y的解可以统一为A 的Moore-Penrose逆矩阵乘以y，特别的是，对于一致性方程，该解为最小范数解，对于非一致方程，该解为最小范数最小二乘解

Moore-Penrose逆矩阵

代码实现

// 拟合求系数
void line_fitting(pcl::PointCloud<pcl::PointXYZ>::Ptr &in_cloud, double &a, double&b, double&c)
{
	vector<double>x, y;
	int num = in_cloud->size();
	for (int i = 0; i < num; i++)
	{
		pcl::PointXYZ p = in_cloud->at(i);
		x.push_back(p.x);
		y.push_back(p.y);
	}
	MatrixXd A(3, 3), B(3, 1), Y(3, 1);
	double A01(0), A02(0), A12(0), A22(0), B00(0), B10(0), B12(0);
	for (int i=0; i<num; i++)
	{
		A01 += x[i];
		A02 += x[i] * x[i];
		A12 += x[i] * x[i] * x[i];
		A22 += x[i] * x[i] * x[i] * x[i];
		B00 += y[i];
		B10 += x[i] * y[i];
		B12 += x[i] * x[i] * y[i];
	}
	A << num, A01, A02;
		 A01, A02, A12;
		 A02, A12, A22;
	B << B00,
		 B10,
		 B12;
	Y = A.inverse() * B;
	a = Y(2, 0);
	b = Y(1, 0);
	c = Y(0, 0);
}

// rviz
void line_fitting_show(pcl::PointCloud<pcl::PointXYZ>::Ptr &in_cloud, visualization_msgs::Marker &line, double a0, double a1, double a2;
{
	pcl::PointXYZ min, max;
	line.lifetime = ros::Buration(0.3);
	line.type = visualization_msgs::Marker::LINE_STRIP;
	line.action = visualization_msgs::Marker::ADD;
	line.ns = "lane";
	line.id = 0;
	line.scale.x = 0.2;
	line.color.g = 1;
	line.color.a = 1;
	line.pose.orientation.w = 1;
	pcl::getMinMax3D(*in_cloud, min, max);
	float x_min = min.x;
	float x_max = max.x;
	vector<double> xx, yy;
	double step = 0.5;
	int step_num = std::ceil((x_max - x_min) /step);
	for （int i = 0; i < step_num + 2; i++)
	{
		double tmp_value = x_min + i * step;
		if (tmp_value > x_max)
		{
			tmp_value = x_max;
		}
		xx.push_back(tmp_value);
		geometry_msgs::Point p;
		p.x = tmp_value;
		p.y = a0 + a1*xx[i] + a2*xx[i]*xx[i];
		p.z = min.z;
		line.points.push_back(p);
	}
}