0. 简介
作为3D车道线训练来说，数据集是至关重要的，而使用点云的精确性来完成准确的车道线地图构建是一个非常重要且有趣的事情。下面我们将会从一个例子开始，分阶段来告诉大家该怎么样去完成一个简单的3D点云车道线自动识别标注工具。
1. 前言
按照《Automatic Road Markings Extraction Classification And Vectorization Mobile From Laser Scanning Data》文章的工作，提出了一种高效的从移动激光扫描(MLS)点云中进行道路标线的自动化提取、分类和矢量化实现的方法。首先，将移动激光扫描点云划分为地面点和非地面点。其次，在图像处理方法下，生成多个地理参考图像，并进一步用于道路标线像素的检测。第三，从图像中检索（匹配）道路标线的点云，并进一步分割为连接的对象。采用OSTU最大类间方差—阈值分割法和统计离群点去除法对道路标线目标进行细化。接下来，根据边界框信息将每个道路标线对象分为几类，如边界线、矩形道路标线等。其它不规则的道路标线则通过模型匹配方法进行分类。最后，在重新连接断裂边界线后，将所有分类的道路标线矢量化为封闭或非封闭多段线。在城市和高速公路场景的各种移动激光扫描点云上进行了综合实验，实验结果表明，该方法的道路标线提取准确率和召回率均超过95%，公路场景的准确率和召回率高达93%。其中城市场景的比例分别为92%和85%。相关代码也已经在Github上开源
我们根据上文的提炼，基本上就是将里面的内容分为四个部分
- 地面过滤 Ground Filter
- 二值化，即根据地理参考图像处理 Geo-referenced Image Processing
- DBSCAN聚类，即道路标线点云分割和提取
- 曲线拟合
下面我们来对这四个部分进行扩展，这里我们选用的是Lane_Extractor这一个项目作为模板，并向大家展示如何完成每一个部分的搭建
2. 地面过滤
这一部分就是根据z轴的坐标来粗略的完成拟合的操作。这部分的操作相对而言比较简单，而如果想要获取更准确的地面信息，则可以采用FEC地面滤除算法将点云分割为地面点和非地面点。这一步对于自动提取道路标线非常必要，点云数据中来自汽车、树木和交通标志上的非地面点会干扰地面元素的检测。
void CloudProcessing::passthrough() // 滤波
{
pcl::PassThrough<pcl::PointXYZI> pass; // 创建直通滤波器对象
pass.setInputCloud(cloud); // 设置输入点云
// pass.setFilterFieldName ("x"); // X..
// pass.setFilterLimits (-150.0, 150.0);//设置在过滤字段的范围
// pass.filter(*cloud);
// pass.setFilterFieldName ("y"); // Y..
// pass.setFilterLimits (-100.0, 300.0);
// pass.filter(*cloud);
pass.setFilterFieldName("z"); // 选择
pass.setFilterLimits(-1.0, 1.0); // 设置在过滤字段的范围
pass.filter(*cloud);
ROS_INFO("Z axis Passthrough Complete!");
}
void CloudProcessing::VoxelGridFilter()
{
pcl::VoxelGrid<pcl::PointXYZI> voxel; // 创建体素格滤波对象
voxel.setInputCloud(cloud);
voxel.setLeafSize(leaf_size, leaf_size, leaf_size);
voxel.filter(*cloud);
ROS_INFO("%fM VoxelGridFilter Complete!", leaf_size);
}
void CloudProcessing::MapDownsampling(bool voxel) // 地图降采样
{
passthrough();
if (voxel)
{
VoxelGridFilter();
}
ROS_INFO("downsampling Complete!");
}
2. 二值化
在这一步，主要的就是每帧点云在分割出地面后，然后根据反射率的强度来完成车道线地图的查询，这部分的内容相对而言比较简单。当然这样会带来噪声等问题，我们可以通过DBSCAN聚类等方法可以有效地避免误检，但是同样的可以通过上述论文中的方法。
if ((cp.cloud->points[pointIdxRadiusSearch[i]].intensity > cp.searchinfo.min_intensity)) // 指定强度 && (cp.cloud->points[pointIdxRadiusSearch[i]].intensity < cp.searchinfo.max_intensity)
{
cp.cloud_filtered->points.push_back(cp.cloud->points[pointIdxRadiusSearch[i]]); // 插入所有找到的调试点
centroidpoint.add(cp.cloud->points[pointIdxRadiusSearch[i]]); // 插入所有找到的点以获得重心
s_point.push_back(cp.cloud->points[pointIdxRadiusSearch[i]]); // 插入所有找到的点进行条件判定
}
文中认为虽然道路标线的反射强度随范围大小、入射角和道路材质的变化很大，但是单独一块点云的强度梯度与道路标线边界大致相同，因此一个阈值足以检测标线边界。最大熵阈值分割方法无监督地选择了实现图像直方图后验已知熵的先验最大化的阈值。结果发现，在Otsu大津法和GMM高斯混合模型等常用的阈值分割算法中，最大熵阈值分割对该任务的效果最好。在这一步，移动激光扫描点云沿着激光扫描仪的运动轨迹被划分成若干块。非地面点和地面点都被投影到水平面上，然后根据平均点间距计算出的分辨率进行光栅化。对于每个点云块，生成三个灰度地理参考图像（高程图、强度图以及点密度图像）。对于每个网格，这三幅图像的灰度值分别计算为非地面点的平均Z值、地面点的平均反射强度值和地面点的数目。中值滤波用于去噪，利用Sobel算子从高程图得到斜率（高程的梯度）图像，从反射强度图像得到强度梯度图像。然后，应用最大熵阈值分割（Pun，1980）生成斜率二值图像和点密度二值图像。
3. DBSCAN聚类
这部分其实就是将没条车道线提取出来的方法，当然也有用kd-tree做的，比如说下面的就是通过半径搜索的方法，并根据kd-tree来完成检索的。
int LaneExtractor::RadiusSearch(int SearchLine, float rad) // 按照半径搜索
{
std::vector<pcl::PointXYZI> s_point;
pcl::PointXYZI cetroidpoint;
int chance = 5; // 检查出线范围的机会数
float z = 0.3; // 利用点的高度差(没用)
cp.searchinfo.radius = rad;
if (rotation_direction == LEFT || rotation_direction == RIGHT)
{
int chance = 6; // 重置次数
L_lane_break = LANE_BREAK_LIMIT; //
R_lane_break = LANE_BREAK_LIMIT;
cp.searchinfo.radius = cp.searchinfo.radius + 0.4f; // 范围增大
// cp.searchinfo.min_intensity = cp.searchinfo.min_intensity+change; intensity change
}
int size = lineRadiusSearch(cetroidpoint, s_point, SearchLine); // 使用kd-tree获取个数，并返回重心点以及对应的点集
if (size > 0) // 分割出来一些特征
{
// Apply to Multi-Rain Only
if (SearchLine == MULTILEFT || SearchLine == MULTIRIGHT) // 判断是否
{
if (size <= 3)
{
s_point.clear(), chance = 0; // 聚类过小，则认为没问题
} // Under 3 point remove;
while (!s_point.empty())
{
Eigen::Vector2f poseP(cetroidpoint.x, cetroidpoint.y); // 获取重心位置
pcl::PointXYZI target_p = s_point.back(); // 获取调试点
s_point.pop_back(); // 压出
Eigen::Vector2f targetP(target_p.x, target_p.y);
double distance = getPointToDistance(poseP, tan_yaw, targetP); // 判断距离
if (distance > 0.26f)
chance--; // 重校验次数减一
if (chance == 0)
break;
}
s_point.clear();
if (chance != 0 && (fabs(cetroidpoint.z - save_point.z) > z)) // 没用这段
{ // pass -> extract
if (SearchLine == MULTILEFT)
{
Multi_left_point.push_back(cetroidpoint); // just save Multi lnae
cp.cloud_filtered->points.push_back(cetroidpoint);
L_continuos_line = 1; // continuous multiline
}
else if (SearchLine == MULTIRIGHT)
{
Multi_right_point.push_back(cetroidpoint); // just save Multi lnae
cp.cloud_filtered->points.push_back(cetroidpoint);
R_continuos_line = 1; // continuous multiline
}
}
else if (L_continuos_line == 1 || R_continuos_line == 1)
{ // extract type change
if (SearchLine == MULTILEFT)
{
L_lane_break--;
if (Multi_left_point.size() >= 8)
{
while (!Multi_left_point.empty())
{
cp.Intesity_Cloud->points.push_back(Multi_left_point.back());
Multi_left_point.pop_back();
}
L_lane_break = LANE_BREAK_LIMIT;
}
else if (L_lane_break == 0)
{
Multi_left_point.clear();
L_lane_break = LANE_BREAK_LIMIT;
}
L_continuos_line = 0; // non-continuous multiline
}
else if (SearchLine == MULTIRIGHT)
{
R_lane_break--;
if (Multi_right_point.size() >= 8)
{
while (!Multi_right_point.empty())
{
cp.Intesity_Cloud->points.push_back(Multi_right_point.back());
Multi_right_point.pop_back();
}
R_lane_break = LANE_BREAK_LIMIT;
}
else if (R_lane_break == 0)
{
Multi_right_point.clear();
R_lane_break = LANE_BREAK_LIMIT;
}
R_continuos_line = 0; // non-continuous multiline
}
}
} // Apply to Multi-Rain Only
else
{
cp.Intesity_Cloud->points.push_back(cetroidpoint); // Not Multi-lane
}
return SearchLine; // Return the lane number if lane found
}
else
return 0; // If you don't find the lane, return 0.
}
// just kdtree radius search
int LaneExtractor::lineRadiusSearch(pcl::PointXYZI ¢erpoint, std::vector<pcl::PointXYZI> &s_point, int SearchLine)
{
pcl::KdTreeFLANN<pcl::PointXYZI> kdtree; // Set up your search tree
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
kdtree.setInputCloud(cp.cloud);
pcl::CentroidPoint<pcl::PointXYZI> centroidpoint;
// ( setting search_point ), ( setting radius ),
int size = kdtree.radiusSearch(cp.searchPoint[SearchLine], cp.searchinfo.radius, pointIdxRadiusSearch, pointRadiusSquaredDistance); // 使用kd-tree来拿到对应的索引
if (size > 0)
{
for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
{
if ((cp.cloud->points[pointIdxRadiusSearch[i]].intensity > cp.searchinfo.min_intensity)) // 指定强度 && (cp.cloud->points[pointIdxRadiusSearch[i]].intensity < cp.searchinfo.max_intensity)
{
cp.cloud_filtered->points.push_back(cp.cloud->points[pointIdxRadiusSearch[i]]); // 插入所有找到的调试点
centroidpoint.add(cp.cloud->points[pointIdxRadiusSearch[i]]); // 插入所有找到的点以获得重心
s_point.push_back(cp.cloud->points[pointIdxRadiusSearch[i]]); // 插入所有找到的点进行条件判定
}
}
centroidpoint.get(centerpoint); // 得到重心点;
return size;
}
else
{
// ROS_INFO("No point could be found.."); //TOO NOISY INFO
return 0;
}
}
当然也可以通过DBSCAN完成聚类，pcl里面已经有非常好的嵌套，如果说我们没有一个标准的相对车辆距离的车道线信息，则可以使用DBSCAN来完成拟合，DBSCAN就不单单会处理车道线的点云信息了
DBSCANKdtreeCluster<pcl::PointXYZ> ec;
ec.setCorePointMinPts(10);
// test 4. uncomment the following line to test the EuclideanClusterExtraction
// pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud(cloud);
std::vector<pcl::PointIndices> cluster_indices;
ec.setClusterTolerance(0.1);//搜索近邻点半径
ec.setMinClusterSize(100);//最小簇点数要求
ec.setMaxClusterSize(5000000);//最大簇点数限制
ec.setSearchMethod(tree);
ec.setInputCloud(cloud);
ec.extract(cluster_indices);
clock_t end_ms = clock();
std::cout << "cluster time cost:" << double(end_ms - start_ms) / CLOCKS_PER_SEC << " s" << std::endl;
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_clustered(new pcl::PointCloud<pcl::PointXYZI>);
int j = 0;
// visualization, use indensity to show different color for each cluster.
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); it++, j++) {
for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) {
pcl::PointXYZI tmp;
tmp.x = cloud->points[*pit].x;
tmp.y = cloud->points[*pit].y;
tmp.z = cloud->points[*pit].z;
tmp.intensity = j % 8;
cloud_clustered->points.push_back(tmp);
}
}
4. 曲线拟合
这部分的内容其实就是线的拟合方程，下面这段代码就是根据点云求拟合直线的操作。基本操作就是先将3D点云2D化，在2D中得到几乎能够代表车道线正确方向的拟合曲线，有了这个车道线的方向，模型才能更好地进行车道线拟合。当然这种情况还是比较简单的，无法应对上下坡这种情况，如果想要更复杂的表示建议可以参考空间曲线拟合这类方法
直线拟合：
// 拟合求系数
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);
}
多次曲线拟合
Eigen::VectorXf FitterLeastSquareMethod(vector<float> &X, vector<float> &Y, uint8_t orders)
{
// abnormal input verification
if (X.size() < 2 || Y.size() < 2 || X.size() != Y.size() || orders < 1)
exit(EXIT_FAILURE);
// map sample data from STL vector to eigen vector
Eigen::Map<Eigen::VectorXf> sampleX(X.data(), X.size());
Eigen::Map<Eigen::VectorXf> sampleY(Y.data(), Y.size());
Eigen::MatrixXf mtxVandermonde(X.size(), orders + 1); // Vandermonde matrix of X-axis coordinate vector of sample data
Eigen::VectorXf colVandermonde = sampleX; // Vandermonde column
// construct Vandermonde matrix column by column
for (size_t i = 0; i < orders + 1; ++i)
{
if (0 == i)
{
mtxVandermonde.col(0) = Eigen::VectorXf::Constant(X.size(), 1, 1);
continue;
}
if (1 == i)
{
mtxVandermonde.col(1) = colVandermonde;
continue;
}
colVandermonde = colVandermonde.array()*sampleX.array();
mtxVandermonde.col(i) = colVandermonde;
}
// calculate coefficients vector of fitted polynomial
Eigen::VectorXf result = (mtxVandermonde.transpose()*mtxVandermonde).inverse()*(mtxVandermonde.transpose())*sampleY;
return result;
}
更多的拟合函数可以参考这个博客。
至此为止我们就已经获取了三维空间中车道线的信息了，然后就可以根据我们的需求来完成相应的3D点云车道线提取了，相较于深度相机而言，个人感觉该方法更加准确。
5. 参考链接
https://blog.csdn.net/m0_37957160/article/details/106212491
https://blog.csdn.net/qq_32867925/article/details/125899276
https://blog.csdn.net/qq_39506862/article/details/122087297
评论（9）
您还未登录，请登录后发表或查看评论