laser_line_extraction线段提取开源功能包解读和使用例程

laser_line_extraction 功能包介绍

laser_line_extraction 是一个功能明确的 ROS 软件包，专门用于从二维激光雷达数据中实时提取直线线段。

一、包的核心功能与处理流程

该包订阅激光扫描话题（通常为 /scan），对每一帧点云执行两个主要步骤：

1. 点云分割

首先剔除距离超出设定范围（min_range、max_range）或孤立的噪点（outlier_dist）。然后采用分割合并算法，根据相邻点之间的欧氏距离是否超过阈值 max_line_gap 将点云划分为不同的候选点簇，每个点簇可能对应环境中的一条直线结构。

2. 线段拟合

对每个候选点簇，使用加权最小二乘法迭代拟合最优直线。该方法来源于 Pfister 等人的论文，能够同时输出线段在极坐标下的参数（距离 radius 和角度 angle）以及这些参数的协方差矩阵，从而反映拟合的不确定性。拟合结果再经处理得到线段端点的笛卡尔坐标。

二、消息结构

包内定义了两种自定义消息类型：

LineSegment.msg

：描述单条线段，包含以下字段
- float32 radius
  
  ：极坐标系下原点到线段所在直线的距离
- float32 angle
  
  ：线段法向量的角度（范围通常为 [-π/2, π/2)）
- float32[2] start
  
  和 float32[2] end：线段端点的笛卡尔坐标
- float32[4] covariance
  
  ：radius 和 angle 的协方差矩阵
LineSegmentList.msg

：表示一帧激光数据中提取的所有线段，内含 LineSegment[] 数组和时间戳。

三、安装与使用

1. 获取源码

bash

复制代码

cd ~/catkin_ws/srcgit clone https://github.com/kam3k/laser_line_extraction.gitcd ~/catkin_wscatkin_make

2. 参数配置

基础配置

包的所有参数通过 ROS 参数服务器加载，官方推荐将参数写入 launch 文件进行管理。关键参数及其作用如下：

参数名	默认值	说明
`scan_topic`	`"scan"`	订阅的激光话题名
`frame_id`	`"laser"`	发布线段的参考坐标系
`publish_markers`	`false`	是否发布可视化标记（用于 RViz 调试）

算法相关的参数配置

参数名	默认值	说明
`min_line_length`	0.5 m	线段最小长度，短于此值的线段被丢弃
`max_line_gap`	0.4 m	点云分割时允许的最大间隔
`outlier_dist`	0.05 m	孤立点判定阈值
`min_line_points`	9	构成一条线段所需的最少点数
`bearing_var`	-	方位方差
`range_var`	-	距离方差
`least_sq_angle_thresh`	-	改变角度（rad）阈值以停止迭代最小二乘法（还必须满足 `least_sq_radius_thresh`）
`least_sq_radius_thresh`	-	改变半径（m）阈值，以停止迭代最小二乘法（还必须满足 `least_sq_angle_thresh`）
`max_line_length`	-	大于此长度的行未发布（m）
`min_range`	-	比这更近的点将被忽略（m）
`max_range`	-	超过此值的点将被忽略（m）
`min_split_dist`	-	当执行拆分和合并的"拆分"步骤时，两个点相距至少这么远（m）会导致拆分
`outlier_dist`	-	与所有邻居相距至少该距离的点被视为异常值（m）

3. 运行节点

创建 launch 文件后，执行：

bash

复制代码

roslaunch laser_line_extraction your_config.launch

节点将发布两个话题：

/line_segments

（laser_line_extraction/LineSegmentList）：检测到的线段列表
/line_markers

（visualization_msgs/MarkerArray）：用于 RViz 显示的线段标记（若 publish_markers 为 true）

4. 运行的效果

绿色的雷达原始的点云，红色提取出的线段。

四、关键代码解读

1. 最小二乘线拟合方法

cpp

复制代码

void Line::leastSqFit(){  // 第一步：计算点的协方差矩阵  calcPointCovariances();  // 第二步：初始化前一次迭代的参数值  // 用于判断收敛条件（当前参数与上一次参数的差值）  double prev_radius = 0.0;  double prev_angle = 0.0;  // 第三步：迭代优化循环  // 当半径或角度的变化超过阈值时继续迭代  while (fabs(radius_ - prev_radius) > params_.least_sq_radius_thresh ||         fabs(angle_ - prev_angle) > params_.least_sq_angle_thresh)   {    prev_radius = radius_;    prev_angle = angle_;    // 计算点的标量协方差    calcPointScalarCovariances();    // 基于最小二乘原理重新计算半径    // 通过优化方法得到更精确的半径值    radiusFromLeastSq();    // 基于最小二乘原理重新计算角度    // 通过优化方法得到更精确的角度值    angleFromLeastSq();  }  // 第四步：计算线段的协方差矩阵  // 反映拟合线段参数的不确定性  calcCovariance();  // 第五步：投影端点  // 将线段的端点投影到拟合的直线上，确保端点在线上  projectEndpoints();}

2. 主要的识别线段函数

cpp

复制代码

void LineExtraction::extractLines(std::vector<Line>& lines) {  filtered_indices_ = c_data_.indices;  lines_.clear();
  // 过滤近距离和远距离点  filterCloseAndFarPoints();  // 过滤异常点  filterOutlierPoints();
  // 如果剩余点不够，则不返回任何线段  if (filtered_indices_.size() <= std::max(params_.min_line_points, static_cast<unsigned int>(3)))  {    return;  }
  // 将索引拆分为线段并过滤掉短行和稀疏线段  split(filtered_indices_);  // 根据线段的最大最小长度，过滤线段  filterLines();
  // 使用最小二乘法拟合每条直线并合并共线段  for (std::vector<Line>::iterator it = lines_.begin(); it != lines_.end(); ++it)  {    it->leastSqFit();  }  // 如果有多线段，检查是否应根据合并条件合并线段  if (lines_.size() > 1)  {    mergeLines();  }  lines = lines_;}

3. 使用贝叶斯估计框架合并线段

cpp

复制代码

void LineExtraction::mergeLines(){  std::vector<Line> merged_lines;
  for (std::size_t i = 1; i < lines_.size(); ++i)  {    // Get L, P_1, P_2 of consecutive lines    Eigen::Vector2d L_1(lines_[i-1].getRadius(), lines_[i-1].getAngle());    Eigen::Vector2d L_2(lines_[i].getRadius(), lines_[i].getAngle());    Eigen::Matrix2d P_1;    P_1 << lines_[i-1].getCovariance()[0], lines_[i-1].getCovariance()[1],           lines_[i-1].getCovariance()[2], lines_[i-1].getCovariance()[3];    Eigen::Matrix2d P_2;    P_2 << lines_[i].getCovariance()[0], lines_[i].getCovariance()[1],           lines_[i].getCovariance()[2], lines_[i].getCovariance()[3];
    // Merge lines if chi-squared distance is less than 3    if (chiSquared(L_1 - L_2, P_1, P_2) < 3)    {      // Get merged angle, radius, and covariance      Eigen::Matrix2d P_m = (P_1.inverse() + P_2.inverse()).inverse();      Eigen::Vector2d L_m = P_m * (P_1.inverse() * L_1 + P_2.inverse() * L_2);      // Populate new line with these merged parameters      boost::array<double, 4> cov;      cov[0] = P_m(0,0);      cov[1] = P_m(0,1);      cov[2] = P_m(1,0);      cov[3] = P_m(1,1);      std::vector<unsigned int> indices;      const std::vector<unsigned int> &ind_1 = lines_[i-1].getIndices();      const std::vector<unsigned int> &ind_2 = lines_[i].getIndices();      indices.resize(ind_1.size() + ind_2.size());      indices.insert(indices.end(), ind_1.begin(), ind_1.end());      indices.insert(indices.end(), ind_2.begin(), ind_2.end());      Line merged_line(L_m[1], L_m[0], cov, lines_[i-1].getStart(), lines_[i].getEnd(), indices);      // Project the new endpoints      merged_line.projectEndpoints();      lines_[i] = merged_line;    }    else    {      merged_lines.push_back(lines_[i-1]);    }
    if (i == lines_.size() - 1)    {      merged_lines.push_back(lines_[i]);    }  }  lines_ = merged_lines;}