2013年专门针对地面分割的文章：
Fast segmentation of 3D point clouds for ground vehicles
代码链接：
https://github.com/lorenwel/linefit_ground_segmentation
代码阅读：
目录
1、新建一个GroundSegmentation::GroundSegmentation类对象，struct GroundSegmentationParams结构体在头文件中赋初值，用于初始化类，包含了如下参数：
2、segment函数：输入为需要进行地面分割的点云以及标签容器
3、insertPoints函数
4、insertionThread函数
5、getLines函数：用于获取拟合直线
6、lineFitThread()函数：
7、fitSegmentLines函数
8、assignCluster(segmentation)函数
9、assignClusterThread函数
1、新建一个GroundSegmentation::GroundSegmentation类对象，struct GroundSegmentationParams结构体在头文件中赋初值，用于初始化类，包含了如下参数：
visualize(false), //是否可视化
r_min_square(0.3 * 0.3), //设置点云离lidar最近的距离
r_max_square(20*20), //设置点云离lidar最远的距离
n_bins(30), //将点云分为多少个环形
n_segments(180), //将点云分为多少个扇形
max_dist_to_line(0.15), //对点到所有直线的距离，如果小于此值，则认为是地面，反之则不是
min_slope(0), //拟合直线的斜率最小值
max_slope(1), //拟合直线的斜率最大值，大于此值则直线不满足要求，去掉最后一点
n_threads(4), //4个线程
max_error_square(0.01), //点到线性拟合的点的最大误差
long_threshold(2.0), //判断为一根长线的阈值，大于此阈值则is_long_line=true
max_long_height(0.1), //判断拟合直线的最后一点是否符合条件，是长线但是期望值与真实值大于此阈值则剔除
max_start_height(0.2), //不符合长直线，但是最后拟合点和传感器所在高度差小于此范围，则加入拟合点
sensor_height(0.2), //传感器高度差，与max_start_height是相对关系
line_search_angle(0.2) //搜索步长小于此角度，则在邻域内找拟合直线。
2、segment函数：输入为需要进行地面分割的点云以及标签容器
该函数就是最重要的函数，主要分为3个子函数：1、insertPoints(cloud)函数：用于获取每个点云的环向索引和扇形索引；2、 getLines()函数：用于获取每一个segment中的拟合直线；3、assignCluster(segmentation)函数：执行分割任务，计算点到拟合直线的距离，将地面点云标签置为1。segment函数的注释代码如下：
输入为：待分割的点云和标签
void GroundSegmentation::segment(const PointCloud& cloud, std::vector<int>* segmentation)
{
std::cout << "Segmenting cloud with " << cloud.size() << " points...\n";
// 计时
std::chrono::high_resolution_clock::time_point start = std::chrono::high_resolution_clock::now();
// segmention表示标签
segmentation->clear();
segmentation->resize(cloud.size(), 0);
//bin_index_存储了每个点的bin索引和segment索引
bin_index_.resize(cloud.size());
//segment_coordinates_表示每个点在(d,z)上的索引
segment_coordinates_.resize(cloud.size());
// 获取了每个点云的bin索引和segment索引
insertPoints(cloud);
std::list<PointLine> lines;
// 没用,不显示
if (params_.visualize) {
getLines(&lines);
}
// 执行这个
else {
getLines(NULL);
}
// 执行分割任务,输入就是标签
assignCluster(segmentation);
//不显示
if (params_.visualize)
{
// Visualize.
PointCloud::Ptr obstacle_cloud(new PointCloud());
// Get cloud of ground points.
PointCloud::Ptr ground_cloud(new PointCloud());
for (size_t i = 0; i < cloud.size(); ++i) {
if (segmentation->at(i) == 1) ground_cloud->push_back(cloud[i]);
else obstacle_cloud->push_back(cloud[i]);
}
PointCloud::Ptr min_cloud(new PointCloud());
getMinZPointCloud(min_cloud.get());
visualize(lines, min_cloud, ground_cloud, obstacle_cloud);
}
// 求取分割时间
std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now();
std::chrono::duration<double, std::milli> fp_ms = end - start;
std::cout << "Done! Took " << fp_ms.count() << "ms\n";
}
3、insertPoints函数
void GroundSegmentation::insertPoints(const PointCloud& cloud)
{
// 线程数为4
std::vector<std::thread> threads(params_.n_threads);
// 将所有点分为4等分
const size_t points_per_thread = cloud.size() / params_.n_threads;
// Launch threads.0-1,1-2,2-3,3-4
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;
// 执行insertionThread函数
threads[i] = std::thread(&GroundSegmentation::insertionThread, this,
cloud, start_index, end_index);
}
// 若未除尽,则需要另加线程处理剩下的点
const size_t start_index = (params_.n_threads - 1) * points_per_thread;
const size_t end_index = cloud.size();
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();
}
}
再看一下其中的线程调用函数
4、insertionThread函数
void GroundSegmentation::insertionThread(const PointCloud& cloud,
const size_t start_index,
const size_t end_index)
{
// n_segments表示将360度分为多少等分,segment_step表示segment的步长
const double segment_step = 2*M_PI / params_.n_segments;
// bin_step表示环形的步长
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);
// 4等分后的每个起点和终点索引
for (unsigned int i = start_index; i < end_index; ++i)
{
pcl::PointXYZ point(cloud[i]);
// 求每个点云的d值
const double range_square = point.x * point.x + point.y * point.y;
const double range = sqrt(range_square);
//若点云的d在设定的r值范围内
if (range_square < params_.r_max_square && range_square > params_.r_min_square)
{
// 求每个点云的水平角
const double angle = std::atan2(point.y, point.x);
// 每个点云的bin索引,从r_min圈外0开始
const unsigned int bin_index = (range - r_min) / bin_step;
// 每个点云的segment索引
const unsigned int segment_index = (angle + M_PI) / segment_step;
// segment_index_clamped存储点云的segment索引，包含了等于n_segments的情况
const unsigned int segment_index_clamped = segment_index == params_.n_segments ? 0 : segment_index;
// segments_存储每一个点云(bin和segment索引)的d和z，每一个segment和bin中都有多个range和z，其间也获取了每一个segment和bin中的一个最小range和z
segments_[segment_index_clamped][bin_index].addPoint(range, point.z);
// 每个点的bin索引和segment索引
bin_index_[i] = std::make_pair(segment_index_clamped, bin_index);
}
else {
bin_index_[i] = std::make_pair<int, int>(-1, -1);
}
// 每个点的d和z
segment_coordinates_[i] = Bin::MinZPoint(range, point.z);
}
}
// 最后，这里可以知道每一个点云所在的segment和bin，以及每一个segment和bin中的z的最低点云
5、getLines函数：用于获取拟合直线
void GroundSegmentation::getLines(std::list<PointLine> *lines)
{
std::mutex line_mutex;
std::vector<std::thread> thread_vec(params_.n_threads);
unsigned int i;
for (i = 0; i < params_.n_threads; ++i) {
// 按n_segment索引，0-45，46-90，91-135，136-180
const unsigned int start_index = params_.n_segments / params_.n_threads * i;
const unsigned int end_index = params_.n_segments / params_.n_threads * (i+1);
thread_vec[i] = std::thread(&GroundSegmentation::lineFitThread, this,
start_index, end_index, lines, &line_mutex);
}
for (auto it = thread_vec.begin(); it != thread_vec.end(); ++it) {
it->join();
}
}
其中的线程调用函数
6、lineFitThread()函数：
void GroundSegmentation::lineFitThread(const unsigned int start_index,
const unsigned int end_index,
std::list<PointLine> *lines, std::mutex* lines_mutex)
{
const bool visualize = lines;
const double seg_step = 2*M_PI / params_.n_segments;
double angle = -M_PI + seg_step/2 + seg_step * start_index;
// i表示等某个segment，将按照segment等分索引
for (unsigned int i = start_index; i < end_index; ++i)
{
// segment.cc中的fitsegment函数进行直线拟合
segments_[i].fitSegmentLines();
//不可视化线
if (visualize) {
std::list<Segment::Line> segment_lines;
segments_[i].getLines(&segment_lines);
for (auto line_iter = segment_lines.begin(); line_iter != segment_lines.end(); ++line_iter) {
const pcl::PointXYZ start = minZPointTo3d(line_iter->first, angle);
const pcl::PointXYZ end = minZPointTo3d(line_iter->second, angle);
lines_mutex->lock();
lines->emplace_back(start, end);
lines_mutex->unlock();
}
angle += seg_step;
}
}
}
7、fitSegmentLines函数
// 对于每一个segment，都进行如下操作：
void Segment::fitSegmentLines()
{
// line_start指针指向segment的第一个bin
auto line_start = bins_.begin();
// 如果没有点，则第二个bin
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_;
// 第一个bin中的最小点
std::list<Bin::MinZPoint> current_line_points(1, line_start->getMinZPoint());
//存储拟合直线的k和b
LocalLine cur_line = std::make_pair(0,0);
// 从第二个点开始
for (auto line_iter = line_start+1; line_iter != bins_.end(); ++line_iter)
{
if (line_iter->hasPoint()) {
Bin::MinZPoint cur_point = line_iter->getMinZPoint();
// 前后两个bin中的最低点的d的差大于2，则判断为一个长线
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.
double expected_z = std::numeric_limits<double>::max();
//利用k、b求期望
if (is_long_line && current_line_points.size() > 2)
{
expected_z = cur_line.first * cur_point.d + cur_line.second;
}
current_line_points.push_back(cur_point);
// 利用eigen求解，获取拟合直线的k、b
cur_line = fitLocalLine(current_line_points);
// 返回三个点拟合的最大误差
const double error = getMaxError(current_line_points, cur_line);
// 如果满足下列条件则弹出第三个bins
if (error > max_error_ ||
std::fabs(cur_line.first) > max_slope_ ||
(current_line_points.size() > 2 && std::fabs(cur_line.first) < min_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));
// 获取bin4的地面高度
cur_ground_height = new_line.first * current_line_points.back().d + new_line.second;
}
// Start new line.
is_long_line = false;
// 只保留最后一个
current_line_points.erase(current_line_points.begin(), --current_line_points.end());
--line_iter;
}
// Good line, continue.
else { }
}
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 {
// 如果不在一水平面，则新建bin2为起点
// Start new line.
current_line_points.clear();
current_line_points.push_back(cur_point);
}
}
}
}
// Add last line.
// 如果大于两个点则拟合直线，将拟合的直线存储至lines_中，在下一函数中使用。
if (current_line_points.size() > 2) {
const LocalLine new_line = fitLocalLine(current_line_points);
lines_.push_back(localLineToLine(new_line, current_line_points));
}
}
8、assignCluster(segmentation)函数
// 分为四个线程进行处理
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();
}
}
其中的线程处理函数
9、assignClusterThread函数
void GroundSegmentation::assignClusterThread(const unsigned int &start_index,
const unsigned int &end_index,
std::vector<int> *segmentation)
{
const double segment_step = 2*M_PI/params_.n_segments;
for (unsigned int i = start_index; i < end_index; ++i)
{
// 表示每个点的d个z存储在point—_2d里面
Bin::MinZPoint point_2d = segment_coordinates_[i];
// 每个点的segment和bin索引存储在bin_index_;
const int segment_index = bin_index_[i].first;
if (segment_index >= 0)
{
// 求每一个点到直线的距离
double dist = segments_[segment_index].verticalDistanceToLine(point_2d.d, point_2d.z);
// 在邻域内搜索
int steps = 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;
// 计算点在邻域拟合直线的距离
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);
// 选取最大的距离
if (dist_1 > dist) {
dist = dist_1;
}
if (dist_2 > dist) {
dist = dist_2;
}
++steps;
}
//若投影误差在一定范围内，则标签值为1
if (dist < params_.max_dist_to_line && dist != -1) {
segmentation->at(i) = 1;
}
}
}
}
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
