C++:基于激光雷达点云的ransac车道线多条拟合

前言

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

当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逆矩阵

参考链接
RANSAC直线拟合

C++实现ransac算法&车道线拟合

项目背景是激光雷达车道线检测及拟合，输入的数据是3d点云。

int ransac_line_fitting(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud, int maxiter,
	int consensus_thres, double dis_thres,
	pcl::PointCloud<pcl::PointXYZI>::Ptr inlier_cloud,
	pcl::PointCloud<pcl::PointXYZI>::Ptr outlier_cloud)
{
	int point_num = in_cloud->size();
	default_random_engine rng;
	rng.seed(10);
	uniform_int_distribution<unsigned> uniform(0, point_num-1);
	set<int> selectIndexs, consensusIndexs;
	pcl::PointCloud<pcl::PointXYZI>::Ptr selectPoints(new pcl::PointCloud<pcl::PointXYZI>);
	int isNonFind = 1;
	int bestconsensus_num = 0;
	int iter = 0;
	double tmp_A, tmp_B, tmp_C;
	while (iter < maxiter)
	{
		selectIndexs.clear();
		selectPoints->clear();
		while(1)
		{
			int index = uniform(rng);
			selectIndexs.insert(index);
			if(selectIndexs.size() == 2) // 2 samples
			{
				break;
			}
		}
		set<int>::iterator selectiter = selectIndexs.begin();
		while(selectiter != selectIndexs.end())
		{
			int index = *selectiter;
			selectPoints->push_back(in_cloud->points[index]);
			selectiter++;
		}
		// 这里是直线拟合，也可以换成多次曲线拟合
		double deltaY = selectPoints->points[1].y - selectPoints->points[0].y;
		double deltaX = selectPoints->points[1].x - selectPoints->points[0].x;
		tmp_A = deltaY;
		tmp_B = -deltaX;
		tmp_C = -deltaY * selectPoints->points[1].x + deltaX * selectPoints->points[1].y;
		int dataiter = 0;
		double meanError = 0;
		set<int> tmpConsensusIndexs;
		std::vector<int> inlierIndex, outlierIndex;
		while(dataiter < point_num)
		{
			double dist = (tmp_A*in_cloud->points[dataiter].x + tmp_B*in_cloud->points[dataiter].y + tmp_C) / sqrt(tmp_A*tmp_A + tmp_B*tmp_B);
			dist = dist > 0 ? dist : -dist;
			if(dist <= dis_thres)
			{
				tmpConsensusIndexs.insert(dataiter);
				inlierIndex.push_back(dataiter);
			}
			else{
				outlierIndex.push_back(dataiter);
			}
			meanError += dist;
			dataiter++;
		}
		if (tmpConsensusIndexs.size() >= consensus_thres && tmpConsensusIndexs.size() >= bestconsensus_num)
		{
			bestconsensus_num = consensusIndexs.size();
			consensusIndexs = tmpConsensusIndexs;
			isNonFind = 0;
			pcl::copyPointCloud(*in_cloud, inlierIndex, *inlier_cloud);
			pcl::copyPointCloud(*in_cloud, outlierIndex, *outlier_cloud);
		}
		iter++;
		inlierIndex.clear();
		outlierIndex.clear();
	}
	return isNonFind;
}

多条车道线拟合

上面我们只是得到一条车道线点，及其他外点，要想得到所有车道线，需要对其循环操作。

int maxiter = 100;
int consensus_thres = 20;
double dis_thres = 0.1;
pcl::PointCloud<pcl::PointXYZI>::Ptr inlier_points(new pcl::PointCloud<pcl::PointXYZI>),
outlier_points(new pcl::PointCloud<pcl::PointXYZI>);
while (in_cloud->size() > 10)
{
	if(!ransac_line_fitting(in_cloud, maxiter, consensus_thres, dis_thres, inlier_points, outlier_points))
	{
		*lane_cloud += *inlier_points;
	}
	if (outlier_pointss->size() == 0)
	{
		break;
	}
	in_cloud->clear();
	*in_cloud = *outlier_points;
	outlier_points->clear();
}