论文链接：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_pointBin::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; }
至此，整个地面分割的代码到此结束，论文看着没有那么复杂，但代码的逻辑比较多，需要深入了解，才能真正了解论文的精髓。
作者的代码中还有很多值得优化的地方，可以根据自己的工程经验和使用场景进行优化。
1449
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
