论文链接:Fast segmentation of 3D point clouds for ground vehicles | IEEE Conference Publication | IEEE Xplore
代码链接:GitHub - lorenwel/linefit_ground_segmentation: Ground Segmentation
论文的核心思想:
相比Fast Segmentation of 3D Point Clouds: A Paradigm on LiDAR Data for Autonomous Vehicle Applications,这篇论文的逻辑比较复杂,但思想比较好。
-
将雷达点云等间隔为
N
个Segment
,每个Segment
的角度为Δα
,每个Segment又等分M个Bin,即segments_(segment )(bin)
。
-
将当前帧所有点云投影到
segments_
中,也就是降维:将p(x,y,z)
三维,降到二维p(d, z)
。
-
在投影的过程中获取每个点对应的
(d,z)
,及每个bin中最小的(d,z)
-
对每个
segment
拟合直线z = kd + b
,这里的k
指的的路面的坡度。 -
基于拟合的直线判断点到路面的高度,如果高度小于阈值,则该点是路面点,否则为障碍物点。
-
如果路面有坡度,则拟合的直线是分段的,不同系数的(k,b)代表不同坡度的路面。我们将通过代码加深了解论文的核心思想。
这种方法可以适用于具有坡度的路面,作者在论文中也给出效果。
视频动态检测效果:
优化后的16线雷达的地面分割效果
接下来是代码的走读:
初始化
将雷达点云分割多少个Segment
及每个Segment
含有多少Bin
。
GroundSegmentation segmenter(params);
//每个点云对应的类别,地面点和非地面点
std::vector<int> labels;
segmenter.segment(cloud, &labels);
参数初始化
以下是作者代码中的原始参数,可根据项目中的实际情况设置合适的参数。从参数中可看出,共有360个Segment,每个Segment由120个bin:
n_threads: 4 # number of threads to use.
r_min: 0.5 # minimum point distance.
r_max: 50 # maximum point distance.
n_bins: 120 # number of radial bins.
n_segments: 360 # number of radial segments.
max_dist_to_line: 0.05 # maximum vertical distance of point to line to be considered ground.
sensor_height: 1.8 # sensor height above ground.
max_slope: 0.3 # maximum slope of a ground line.
max_fit_error: 0.05 # maximum error of a point during line fit.
long_threshold: 1.0 # distance between points after which they are considered far from each other.
max_long_height: 0.1 # maximum height change to previous point in long line.
max_start_height: 0.2 # maximum difference to estimated ground height to start a new line.
line_search_angle: 0.1 # how far to search in angular direction to find a line [rad].
latch: false # latch output topics or not
visualize: false # visualize segmentation result - USE ONLY FOR DEBUGGING
其他信息初始化
这里主要是点云标签、每个点云对应的(d,z)
、每个点云所在的(segment, bin)
初始化,且初始化的大小为点云的个数。
// 每一个点云的标签
segmentation->resize(cloud.size(), 0);
// 每个点云对应的(segment,bin)
bin_index_.resize(cloud.size());
// 每一个点对应的d、z
segment_coordinates_.resize(cloud.size());
点云投影
核心思想
将点云从三维p(x,y,z)
将到二维p(d, z)
,虽然点云的维度减少,但点云的有效信息是不减少的:
d = sqrt(x*x + y*y)
,相当于把笛卡尔坐标系转换到极坐标系。- 保留
Z
轴的信息,要基于z
的高度信息判断是否该点是否为路面点。
点云降维
以下是点云的投影的详细过程:
insertPoints(cloud);
点云投影过程中,采用多线程的并行运算,将所有点云分割为params_.n_threads
份,每份一个线程进行处理。
void GroundSegmentation::insertPoints(const PointCloud& cloud) {
std::vector<std::thread> threads(params_.n_threads);
const size_t points_per_thread = cloud.size() / params_.n_threads;
// Launch threads.
/*根据我们设定的数目来将整个的点云分为几个部分开始处理,利用多线程来处理*/
for (unsigned int i = 0; i < params_.n_threads - 1; ++i) {
const size_t start_index = i * points_per_thread;
const size_t end_index = (i+1) * points_per_thread - 1;
threads[i] = std::thread(&GroundSegmentation::insertionThread, this,
cloud, start_index, end_index);
}
// Launch last thread which might have more points than others.
/*启动最后一个可能含有更多点云的线程*/
const size_t start_index = (params_.n_threads - 1) * points_per_thread;
const size_t end_index = cloud.size() - 1;
threads[params_.n_threads - 1] =
std::thread(&GroundSegmentation::insertionThread, this, cloud, start_index, end_index);
// Wait for threads to finish.
for (auto it = threads.begin(); it != threads.end(); ++it) {
it->join();
}
}
在投影过程,
-
计算每个
Segment
的角度。 -
计算每个
Bin
步长,通过感兴趣区域的r_max_square
、r_min_square
和n_bins
确定。 -
计算每个点的极坐标
(d, angle)
,需要注意:std::atan2(point.y, point.x)
得到的angle范围[-M_PI,M_PI]
,需要转到到(0,2*M_PI]
,即angle = angle + M_PI
。 -
判断点是否在感兴趣区域:
d < r_max && d > r_min
。 -
如果点感兴趣区域,计算每个点所在的
Segment、Bin
,并保存到bin_index_[i] = std::make_pair(segment_index, bin_index)
。同时获取该(segment,bin)对应的(d, z)的最小值。-
segments_[segment_index][bin_index].addPoint(range, point.z)
-
在
bin
中插入(d, z)
时,首先has_point_ = true
,此bin
中已经有值了。后续取值的时候会用到has_point_
,此处先画个重点。从代码中可以看出,每次插入值,都会计算(d, z)
是否是最小值。void Bin::addPoint(const double& d, const double& z) { has_point_ = true; if (z < min_z) { min_z = z; min_z_range = d; } }
-
-
如果点不在感兴趣区域,它的Segment和bin的属性为默认值-1,即
bin_index_[i] = std::make_pair<int, int>(-1, -1)
。 -
将每个点(d, z)信息存放到
segment_coordinates_[i] = Bin::MinZPoint(range, point.z)
。
点云的投影的完整代码如下:
void GroundSegmentation::insertionThread(const PointCloud& cloud,
const size_t start_index,
const size_t end_index) {
/*同样的先算步长,然后再进行其他的操作*/
// 每个Segment2°
const double segment_step = 2*M_PI / params_.n_segments;
const double bin_step = (sqrt(params_.r_max_square) - sqrt(params_.r_min_square))
/ params_.n_bins;
const double r_min = sqrt(params_.r_min_square);
/*对于起始索引和终止索引进行遍历*/
for (unsigned int i = start_index; i < end_index; ++i) {
pcl::PointXYZ point(cloud[i]);
/*这里是算模长*/
const double range_square = point.x * point.x + point.y * point.y;
const double range = sqrt(range_square);
/*判断模场是否属于最小值和最大值之间*/
if (range_square < params_.r_max_square && range_square > params_.r_min_square) {
/*得到角度*/
const double angle = std::atan2(point.y, point.x);
/*根据模场和角度来算出bin的索引以及划分的索引*/
const unsigned int bin_index = (range - r_min) / bin_step;
// 从-PI~PI,转换为0~2PI
const unsigned int segment_index = (angle + M_PI) / segment_step;
/*对于设定的属于bin和划分中的集合进行数据的添加*/
segments_[segment_index][bin_index].addPoint(range, point.z);
/*将后面的数据作为一个元组全部传递到bin_index之中*/
bin_index_[i] = std::make_pair(segment_index, bin_index);
}
else {
bin_index_[i] = std::make_pair<int, int>(-1, -1);
}
/*获取到划分坐标为最小z点的坐标和range*/
segment_coordinates_[i] = Bin::MinZPoint(range, point.z);
}
}
直线拟合
流程图
直线拟合的核心思想以流程图的形式绘制如下。同时,还会结合代码进行细说。
示例演示
为加深论文的理解,我们以下面为例进行分析代码。某个Segment
共有7个bin
,其中bin1~bin4
在同一个平面,bin5~bin7
在同一屏幕。
-
获取当前
Segment
中bins
中的第一个点line_start
,即bin1,并将bin1
点放入current_line_points
中。如果当前Segment
中的bins
不包含点,则直接返回。``auto line_start = bins_.begin();
/在整个的bins中找第一个点/
while (!line_start->hasPoint()) {
++line_start;
// Stop if we reached last point.
if (line_start == bins_.end()) return;}
-
从
bin2
开始判断是否进行拟合直线for (auto line_iter = line_start+1; line_iter != bins_.end(); ++line_iter)
-
此时
line_iter
为bin2
,从bin2
获取它的cur_point
Bin::MinZPoint cur_point = line_iter->getMinZPoint();
-
计算
cur_point
与current_line_points
最后一个值的距离,也就是计算bin2
与bin1
的距离,如果二者的距离大于long_threshold_
,则认为两个bin
之间是一个长线段。if (cur_point.d - current_line_points.back().d > long_threshold_) is_long_line = true;
-
判断
current_line_points
中的是否有两个及以上的bin
,此时current_line_points
中只有bin1
一个值。接下来有两个流向。①:判断
bin2
和bin1
是否在同一个水平面,如果在,将bin2
放入current_line_points
。本例中bin2
和bin1
在同一水平面,执行该步骤。- 两点不是长直线:
cur_point.d - current_line_points.back().d < long_threshold_
。 bin1
离地高度比较低:std::fabs(current_line_points.back().z - cur_ground_height) < max_start_height_
if (cur_point.d - current_line_points.back().d < long_threshold_ && std::fabs(current_line_points.back().z - cur_ground_height) < max_start_height_) { // Add point if valid. current_line_points.push_back(cur_point); }
②:如果
bin2
和bin1
不在同一水平面,则以bin2
作为新的待拟合直线起点。current_line_points.clear(); current_line_points.push_back(cur_point);
- 两点不是长直线:
-
接下来到
bin3
,此时current_line_points
已有bin1、bin2
两个值。执行步骤4:判断两点是否为长直线。 -
将
bin3
加入到current_line_points
中,并开始拟合直线,并将bin1、bin2、bin3
拟合直线的k
和b
返回。current_line_points.push_back(cur_point); /*对于当前线点传入到本地线拟合中,得到最后的结果*/ cur_line = fitLocalLine(current_line_points);
在拟合直线
y = kx + b
时,调用eigen
库解(k, b)
。可以参考:Eigen学习(九)稠密问题之线性代数和分解_Fearless的博客-CSDN博客。Segment::LocalLine Segment::fitLocalLine(const std::list<Bin::MinZPoint> &points) { const unsigned int n_points = points.size(); Eigen::MatrixXd X(n_points, 2); Eigen::VectorXd Y(n_points); unsigned int counter = 0; for (auto iter = points.begin(); iter != points.end(); ++iter) { X(counter, 0) = iter->d; X(counter, 1) = 1; Y(counter) = iter->z; ++counter; } Eigen::VectorXd result = X.colPivHouseholderQr().solve(Y); LocalLine line_result; line_result.first = result(0); line_result.second = result(1); return line_result; }
-
计算拟合直线的最大误差,输入参数有当前有效的地面点集
current_line_points
和这些点集拟合的直线参数cur_line(k, b)
,将点集到拟合直线距离误差的最大值返回。const double error = getMaxError(current_line_points, cur_line); double Segment::getMaxError(const std::list<Bin::MinZPoint> &points, const LocalLine &line) { double max_error = 0; for (auto it = points.begin(); it != points.end(); ++it) { const double residual = (line.first * it->d + line.second) - it->z; const double error = residual * residual; if (error > max_error) max_error = error; } return max_error; }
-
判断最大误差
error
是否满足符合要求,此时bin1、bin2、bin3
拟合直线最大误差符合要求,进入下一次循环,获取bin4
。 -
在
bin4
时,执行步骤4 。此时
current_line_points已有三个值,获取
bin4相对于步骤7中拟合直线的距离
expected_z`。double expected_z = std::numeric_limits<double>::max(); /*如果是长线段且当前线段的点数大于2,则我们获取到期待的z,这个在第一次迭代的时候不会被执行*/ if (is_long_line && current_line_points.size() > 2) { expected_z = cur_line.first * cur_point.d + cur_line.second; }
-
然后执行步骤
8、9
,获取bin5
,进入下一步循环。 -
bin5
已经属于上坡的区域。执行步骤4、10、7、8
(注意顺序)。在步骤8或步骤10返回的结果满足以下条件时:- 最大投影误差
error
大于max_error_
。 bin1、bin2、bin3、bin4、bin5
拟合直线的坡度比较大:std::fabs(cur_line.first) > max_slope_
。- 如果
bin4
和bin5
是长直线且高度差大于最大允许高度差:is_long_line && std::fabs(expected_z - cur_point.z) > max_long_height_
if (error > max_error_ || std::fabs(cur_line.first) > max_slope_ || is_long_line && std::fabs(expected_z - cur_point.z) > max_long_height_) { // Add line until previous point as ground. /*添加线直到浅一点是地面点*/ current_line_points.pop_back(); // Start new line. is_long_line = false; /*erase在删除的过程中还是减少vector的size*/ current_line_points.erase(current_line_points.begin(), --current_line_points.end()); --line_iter; }
接下来执行的操作如下:
①:将
bin5
从current_line_points
移除。current_line_points.pop_back();
②:此时
current_line_points
还有四个值,则基于这四个值从新拟合直线。并将拟合直线两端的(d, z)
放入lines_
。 ③:基于拟合直线参数获取
bin4
的地面高度cur_ground_height
。// Don't let lines with 2 base points through. /*不要让有两个基点的线穿过*/ if (current_line_points.size() >= 3) { /*对于当前线点进行本地拟合,得到一条新的线*/ const LocalLine new_line = fitLocalLine(current_line_points); /*将进行处理后的点放入到线中*/ lines_.push_back(localLineToLine(new_line, current_line_points)); /*计算出当前地面点的高度*/ cur_ground_height = new_line.first * current_line_points.back().d + new_line.second; }
Segment::Line Segment::localLineToLine(const LocalLine& local_line, const std::list<Bin::MinZPoint>& line_points) { Line line; const double first_d = line_points.front().d; const double second_d = line_points.back().d; /*跟去前面的值来进行locl_line的处理*/ const double first_z = local_line.first * first_d + local_line.second; const double second_z = local_line.first * second_d + local_line.second; line.first.z = first_z; line.first.d = first_d; line.second.z = second_z; line.second.d = second_d; return line; }
- 最大投影误差
-
重置
is_long_line
-
current_line_points
只保留最后一个bin
,即bin4
; -
当前迭代器回退一个,此时迭代器指向
bin4
,也就是说下一次循环迭代器还是指向bin5
;is_long_line = false; /*erase在删除的过程中还是减少vector的size*/ current_line_points.erase(current_line_points.begin(), --current_line_points.end()); --line_iter;
-
此时还是
bin5
,由于current_line_points
只有bin4
一个值,此时执行步骤4、5,在步骤5中同样满足条件①,将bin5
放入current_line_points
,注意此时current_line_points
有bin4
和bin5
两个值,一个属于平面,一个属于坡面,此处需要画个重点,相信有很多人有疑问,我们继续往下看。 -
此时到
bin6
,它是坡面。执行步骤如下:- 判断
bin5
和bin6
是否为长直线。 - 将
bin6
放入current_line_points
,此时current_line_points
有三个值。 - 拟合直线,并计算最大投影误差
error
。
- 判断
-
bin6
计算得到最大投影误差error > max_error_
,则进入以下操作:- 将
bin6
从current_line_points
移除:current_line_points.pop_back();
- 此时
current_line_points
只有bin4
和bin5
两个bin
,它的size()
小于3,重置以下信息:is_long_line = false;
current_line_points
只保留最后一个bin
,即bin5
;- 当前迭代器回退一个,此时迭代器指向
bin5
,也就是说下一次循环迭代器还是指向bin6
;
- 将
-
这是迭代时还是指向bin6,执行以下操作:
①:判断
bin5
和bin6
是否为长直线②:将
bin6
放入current_line_points
,此时current_line_points
有二个值③:拟合
bin5
和bin6
的直线,并返回(k, b)
④:计算拟合的直线参数与
current_line_points
中所有点的最大投影误差const double error = getMaxError(current_line_points, cur_line);
⑤:因为
bin5
和bin6
在同一个水平面,所以计算得到的投影误差不会满足以下任意条件:error > max_error_
std::fabs(cur_line.first) > max_slope_
is_long_line && std::fabs(expected_z - cur_point.z) > max_long_height_
-
最后一个bin7,执行以下步骤:
①:判断
bin6
和bin7
是否为长直线②:将
bin7
放入current_line_points
,此时current_line_points
有三个值③:拟合
bin5
、bin6
、bin7
的直线,并返回(k, b)
④:计算拟合的直线参数与
current_line_points
中所有点的最大投影误差const double error = getMaxError(current_line_points, cur_line);
⑤:因为
bin5
、bin6
、bin7
在同一个水平面,所以计算得到的投影误差不会满足以下任意条件:error > max_error_
std::fabs(cur_line.first) > max_slope_
is_long_line && std::fabs(expected_z - cur_point.z) > max_long_height_
至此,示例的所有bin已计算完成,需要注意的是:此时current_line_points还有bin5
、bin6
、bin7
三个元素,并且还没有计算拟合直线起点和终点两个点,是不是感觉少点什么?
代码的最后,当current_line_points中至少有三个点时,则进行拟合直线,并将拟合直线的两端(d, z)
放入lines_
。此时才算最终结束。
if (current_line_points.size() > 2) {
const LocalLine new_line = fitLocalLine(current_line_points);
lines_.push_back(localLineToLine(new_line, current_line_points));
}
最后,全部的直线代码如下:
void Segment::fitSegmentLines() {
// Find first point.
auto line_start = bins_.begin();
/*在整个的bins中找第一个点*/
while (!line_start->hasPoint()) {
++line_start;
// Stop if we reached last point.
if (line_start == bins_.end()) return;
}
// Fill lines.
bool is_long_line = false;
double cur_ground_height = -sensor_height_;
/*将线的第一个点的信息传递到*/
std::list<Bin::MinZPoint> current_line_points(1, line_start->getMinZPoint());
LocalLine cur_line = std::make_pair(0,0);
/*从第一个点开始对于bins上的每一个点都进行遍历操作*/
for (auto line_iter = line_start+1; line_iter != bins_.end(); ++line_iter) {
/*如果我们设定的线上有点,则进行后面的操作*/
if (line_iter->hasPoint()) {
Bin::MinZPoint cur_point = line_iter->getMinZPoint();
/*如果两个的线的长度大于我们设定的阈值,则我们认为这两个点之间是长线*/
if (cur_point.d - current_line_points.back().d > long_threshold_) is_long_line = true;
/*针对于后面几次遍历而言的,至少有三个点的情况下*/
if (current_line_points.size() >= 2) {
// Get expected z value to possibly reject far away points.
/*获取远离点的z值*/
double expected_z = std::numeric_limits<double>::max();
/*如果是长线段且当前线段的点数大于2,则我们获取到期待的z,这个在第一次迭代的时候不会被执行*/
if (is_long_line && current_line_points.size() > 2) {
expected_z = cur_line.first * cur_point.d + cur_line.second;
}
/*将当前点插入到current_line之中*/
current_line_points.push_back(cur_point);
/*对于当前线点传入到本地线拟合中,得到最后的结果*/
cur_line = fitLocalLine(current_line_points);
/*将我们经过本地线拟合之后的*/
const double error = getMaxError(current_line_points, cur_line);
// Check if not a good line.
/*将算出来的误差和最大误差进行比较,判断是否是一个合格的线*/
if (error > max_error_ ||
std::fabs(cur_line.first) > max_slope_ ||
is_long_line && std::fabs(expected_z - cur_point.z) > max_long_height_) {
// Add line until previous point as ground.
/*添加线直到浅一点是地面点*/
current_line_points.pop_back();
// Don't let lines with 2 base points through.
/*不要让有两个基点的线穿过*/
if (current_line_points.size() >= 3) {
/*对于当前线点进行本地拟合,得到一条新的线*/
const LocalLine new_line = fitLocalLine(current_line_points);
/*将进行处理后的点放入到线中*/
lines_.push_back(localLineToLine(new_line, current_line_points));
/*计算出当前地面点的高度*/
cur_ground_height = new_line.first * current_line_points.back().d + new_line.second;
}
// Start new line.
is_long_line = false;
/*erase在删除的过程中还是减少vector的size*/
current_line_points.erase(current_line_points.begin(), --current_line_points.end());
--line_iter;
}
// Good line, continue.
else { }
}
/*在有1到2个点的情况下的处理*/
else {
// Not enough points.
/*判断是否满足添加条件,添加这些点*/
if (cur_point.d - current_line_points.back().d < long_threshold_ &&
std::fabs(current_line_points.back().z - cur_ground_height) < max_start_height_) {
// Add point if valid.
current_line_points.push_back(cur_point);
}
/*开始一条新的线*/
else {
// Start new line.
current_line_points.clear();
current_line_points.push_back(cur_point);
}
}
}
}
// Add last line.
/*添加最后一条线*/
if (current_line_points.size() > 2) {
const LocalLine new_line = fitLocalLine(current_line_points);
lines_.push_back(localLineToLine(new_line, current_line_points));
}
}
点云分类
点云分类,即那些是地面点,那些是障碍物点。这里用到的信息就是上一个阶段拟合的直线。点云分类也是使用多线程并行运算,将所有点云等分为params_.n_threads
个线程。
void GroundSegmentation::assignCluster(std::vector<int>* segmentation) {
std::vector<std::thread> thread_vec(params_.n_threads);
const size_t cloud_size = segmentation->size();
for (unsigned int i = 0; i < params_.n_threads; ++i) {
const unsigned int start_index = cloud_size / params_.n_threads * i;
const unsigned int end_index = cloud_size / params_.n_threads * (i+1);
thread_vec[i] = std::thread(&GroundSegmentation::assignClusterThread, this,
start_index, end_index, segmentation);
}
for (auto it = thread_vec.begin(); it != thread_vec.end(); ++it) {
it->join();
}
}
核心思想
点云到当前及相邻的Segment中投影误差如果小于设置阈值,则认为是地面点,否则为非地面点
具体的步骤
-
获取每个
Segment
的步长。 -
获取每个点的
(d, z)
及Segment
索引,如果点云不在感兴趣区域,则Segment
的索引为-1 。Bin::MinZPoint point_2d = segment_coordinates_[i]; /*得到第i个bin的第一个值,作为分割的索引*/ const int segment_index = bin_index_[i].first;
-
当点在感兴趣区域时,计算点到此Segment中所有拟合直线的投影误差。
-
double dist = segments_[segment_index].verticalDistanceToLine(point_2d.d, point_2d.z);
①:计算点是否在当前Segment中,这里使用到是拟合直线的两个端点d信息,并在两个端点基础上加上kMargin余量。这里要控制好阈值,否则会造成点云不属于任何一个拟合直线线段内,误将地面点当成非地面点引起误检。
-
如果点拟合直线线段内,计算投影误差
distance = std::fabs(z - expected_z)
;double Segment::verticalDistanceToLine(const double &d, const double &z) { static const double kMargin = 0.1; double distance = -1; for (auto it = lines_.begin(); it != lines_.end(); ++it) { /*这里设定了论文中要求的距离*/ /*针对于d点,按照设定的余量在前后范围内找到两个点*/ if (it->first.d - kMargin < d && it->second.d + kMargin > d) { /*这里是先算出斜率,将传入的两个点传入到直线中,算出一个最近的额z值差,也就是垂直的距离*/ /*算出找到的两个点之间的斜率*/ const double delta_z = it->second.z - it->first.z; const double delta_d = it->second.d - it->first.d; const double expected_z = (d - it->first.d)/delta_d *delta_z + it->first.z;//(delta_z/delta_d)是一个斜率 //算出最终的距离 distance = std::fabs(z - expected_z); } } return distance; }
-
-
如果点不在当前Segment所有拟合直线线段内,返回的值为-1,且在临近的Segment中查找。
①:以当前Segment为基准,向前和向后搜索,搜索的范围为
steps * segment_step < params_.line_search_angle
,其中params_.line_search_angle = 0.1
弧度,按照论文中设置的阈值,向左和向右搜索3个Segment。②:判断即将向左和向右搜索的
Segment
是否阈值。③:计算向正反双向最近搜索的点到线段投影的距离误差。
④:获取正反双向最大的投影误差。
⑤:如果找到投影误差,即
dist≠-1
,或超过搜索范围,则结束搜索。while (dist == -1 && steps * segment_step < params_.line_search_angle) { // Fix indices that are out of bounds. /*修复超出范围的索引*/ int index_1 = segment_index + steps;//进行正向步长的搜索 while (index_1 >= params_.n_segments) index_1 -= params_.n_segments; int index_2 = segment_index - steps;//进行反向步长的索引 while (index_2 < 0) index_2 += params_.n_segments; // Get distance to neighboring lines. /*算出根据正反双向最近搜索的点到线段投影的距离*/ const double dist_1 = segments_[index_1].verticalDistanceToLine(point_2d.d, point_2d.z); const double dist_2 = segments_[index_2].verticalDistanceToLine(point_2d.d, point_2d.z); // Select larger distance if both segments return a valid distance. /*如果两个分割都返回有效距离,则选择更大的距离*/ if (dist_1 > dist) { dist = dist_1; } if (dist_2 > dist) { dist = dist_2; } /*不断增加步长,一直持续下去,直到跳出循环,这样可以保证比较公平的遍历到里面的点*/ ++steps; }
-
如果投影误差在阈值范围内,该点为地面点,并进行标记
if (dist < params_.max_dist_to_line && dist != -1) { /*这里是对于是否属于地面点的判定*/ segmentation->at(i) = 1; }
至此,整个地面分割的代码到此结束,论文看着没有那么复杂,但代码的逻辑比较多,需要深入了解,才能真正了解论文的精髓。
作者的代码中还有很多值得优化的地方,可以根据自己的工程经验和使用场景进行优化。