2026-09-05

laser_geometry 源码详细分析

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros-perception/laser_geometry
版本：2.4.1，许可证 BSD。

laser_geometry 是 ROS 感知栈中的 2D 激光几何转换库：将 sensor_msgs/LaserScan 投影为 sensor_msgs/PointCloud2，并在 C++ 中支持 运动畸变补偿（扫描期间机器人/激光头移动时的 skew 校正）。提供 C++ 与 Python 两套 API，是 RViz LaserScan 显示、costmap 等组件的基础工具库。

1. 仓库结构

laser_geometry/
├── include/laser_geometry/
│   ├── laser_geometry.hpp       # C++ 公共 API：LaserProjection
│   └── visibility_control.hpp
├── src/
│   ├── laser_geometry.cpp       # C++ 实现（~467 行）
│   └── laser_geometry/
│       ├── laser_geometry.py    # Python 实现（仅 projectLaser）
│       └── __init__.py
├── test/
│   ├── projection_test.cpp      # C++ gtest
│   └── projection_test.py       # Python pytest
├── CMakeLists.txt
├── package.xml
└── CHANGELOG.rst

构建产物：

目标	类型	说明
`liblaser_geometry.so`	C++ 库	`LaserProjection`
Python 包 `laser_geometry`	ament_python	`from laser_geometry import LaserProjection`

2. 在感知栈中的位置

组件	使用的 API
rviz2 LaserScanDisplay	`transformLaserScanToPointCloud` + `tf2::BufferCore`
Python 节点	`projectLaser`（无 TF 变换）
Nav2 costmap	通常直接用 LaserScan，不经过本库

3. 核心类：`LaserProjection`

单一类，无 ROS 节点；纯几何/TF 工具。

3.1 两个公共方法（C++）

方法	作用
`projectLaser(scan, cloud, range_cutoff, channel_options)`	2D 极坐标 → 3D 笛卡尔，保持 scan 原始 frame
`transformLaserScanToPointCloud(target_frame, scan, cloud, tf, ...)`	先投影，再按扫描时间插值 TF，变换到固定参考系

void projectLaser(
  const sensor_msgs::msg::LaserScan & scan_in,
  sensor_msgs::msg::PointCloud2 & cloud_out,
  double range_cutoff = -1.0,
  int channel_options = channel_option::Default)

void transformLaserScanToPointCloud(
  const std::string & target_frame,
  const sensor_msgs::msg::LaserScan & scan_in,
  sensor_msgs::msg::PointCloud2 & cloud_out,
  tf2::BufferCore & tf,
  double range_cutoff = -1.0,
  int channel_options = channel_option::Default)

3.2 可选输出通道（ChannelOption）

位标志 OR 组合：

标志	字段名	类型	含义
`Intensity` (0x01)	`intensity`	FLOAT32	反射强度
`Index` (0x02)	`index`	INT32	原 ranges 数组下标
`Distance` (0x04)	`distances`	FLOAT32	测距值
`Timestamp` (0x08)	`stamps`	FLOAT32	相对 scan 起点的时间偏移
`Viewpoint` (0x10)	`vp_x/y/z`	FLOAT32	视点（默认 0,0,0）
`Default`	—	—	`Intensity \| Index`

ROS 2 已 移除 PointCloud1 支持（见 header TODO / GitHub #29）。

4. `projectLaser` 算法

4.1 极坐标投影

对每个 beam i：

[
\theta_i = \text{angle_min} + i \cdot \text{angle_increment}
]
[
x_i = r_i \cos\theta_i,\quad y_i = r_i \sin\theta_i,\quad z_i = 0
]

2D 激光在 激光平面内，Z 恒为 0。

4.2 cos/sin 缓存

if (co_sine_map_.rows() != static_cast<int>(n_pts) || angle_min_ != scan_in.angle_min ||
  angle_max_ != scan_in.angle_max)
{
  co_sine_map_ = Eigen::ArrayXXd(n_pts, 2);
  for (size_t i = 0; i < n_pts; ++i) {
    co_sine_map_(i, 0) = cos(angle_min + i * angle_increment);
    co_sine_map_(i, 1) = sin(angle_min + i * angle_increment);
  }
}
output = ranges * co_sine_map_;  // Eigen 逐元素乘法

当 beam 数量或角度范围不变 时复用 co_sine_map_，避免重复三角函数计算。

4.3 有效点过滤

if (range_cutoff < 0) range_cutoff = scan_in.range_max;

for (size_t i = 0; i < n_pts; ++i) {
  const float range = scan_in.ranges[i];
  if (range < range_cutoff && range >= scan_in.range_min) {
    // 写入 x,y,z + 可选通道
    ++count;
  }
}
cloud_out.width = count;  // 压缩为有效点

无效测距（< range_min 或 >= range_cutoff）直接丢弃，不保留 NaN 占位
旧版保留 NaN 点的逻辑已注释掉（源码 TODO 质疑其合理性）
若需 index 与原 ranges 一一对应，应预先过滤 scan 或启用 Index 通道

4.4 PointCloud2 布局

固定字段顺序：x, y, z（各 FLOAT32，offset 0/4/8），再按 flags 追加可选字段；is_dense = false（因过滤后 width 可能小于原 ranges 数）。

5. `transformLaserScanToPointCloud`：运动畸变补偿

5.1 流程

5.2 时间端点

rclcpp::Time start_time(scan_in.header.stamp);
rclcpp::Time end_time = start_time + Duration(
  (ranges.size() - 1) * time_increment);

start_transform = tf.lookupTransform(target_frame, scan_in.header.frame_id, t_start);
end_transform   = tf.lookupTransform(target_frame, scan_in.header.frame_id, t_end);

5.3 逐点插值变换

tfScalar ratio = pt_index * ranges_norm;  // ranges_norm = 1/(N-1)

v.setInterpolate3(origin_start, origin_end, ratio);   // 平移线性插值
cur_transform.setRotation(slerp(quat_start, quat_end, ratio));  // 旋转 slerp

point_out = cur_transform * point_in;

假设：扫描期间 匀速运动（constant velocity），各 beam 按 index 均匀分布在 [t_start, t_end]。

target_frame 应为 固定世界系（如 map/odom），否则补偿无意义。

5.4 Index 通道的临时使用

内部强制 channel_options |= Index，变换完成后若用户未请求 Index，则复制点云并 剥离 index 字段（调整 offset/point_step）。

6. Python 实现

from laser_geometry import LaserProjection
projector = LaserProjection()
cloud = projector.projectLaser(scan, range_cutoff=-1.0,
    channel_options=LaserProjection.ChannelOption.DEFAULT)

对比	C++	Python
`projectLaser`	✅ Eigen 向量化	✅ NumPy
`transformLaserScanToPointCloud`	✅	❌ 未实现
cos/sin 缓存	✅	✅
`range_cutoff`	默认 `range_max`	额外 `min(cutoff, range_max)`

Python 用 sensor_msgs_py.point_cloud2.create_cloud 构建消息，逻辑与 C++ 对齐。

7. 下游集成：RViz LaserScanDisplay

projector_->transformLaserScanToPointCloud(
  fixed_frame_.toStdString(),
  *scan,
  *cloud,
  *tf_wrapper->getBuffer(),
  -1,
  laser_geometry::channel_option::Intensity);

RViz 将 LaserScan 转为 fixed frame 的 PointCloud2 再渲染；并根据 scan 时长动态调整 TF filter tolerance。

8. 依赖关系

laser_geometry
  ├── Eigen3              # C++ 矩阵运算
  ├── rclcpp              # Time / Duration
  ├── sensor_msgs         # LaserScan, PointCloud2
  ├── tf2                 # BufferCore, Transform, slerp
  └── (Python) rclpy, numpy, sensor_msgs_py

9. 测试

测试	状态	覆盖
`projection_test.cpp::projectLaser2`	✅ 启用	多角度/距离/通道组合
`projection_test.cpp::transformLaserScanToPointCloud2`	❌ `#if 0` 禁用	需 TF 树，ROS 2 端口未完成
`projection_test.py::test_project_laser`	✅ 启用	Python projectLaser 与 C++ 对齐

10. 设计特点与局限

特点	说明
轻量无节点	库函数，易嵌入驱动/显示/算法
cos/sin 缓存	同分辨率重复 scan 高效
运动补偿	扫描期内 TF 插值，适合移动机器人
可扩展通道	位标志控制附加字段
C++/Python 双实现	Python 适合快速原型

局限	说明
仅 2D 平面	Z=0，不含俯仰/多线激光
匀速假设	加减速/振动时补偿有误差
无效点丢弃	丢失与原 index 对齐（除非预过滤）
Python 无 TF 变换	移动补偿仅 C++
transform 单测禁用	ROS 2 回归覆盖不足
PointCloud1 已移除	老代码需迁移 PointCloud2

11. 使用示例

11.1 静态投影（C++）

laser_geometry::LaserProjection projector;
sensor_msgs::msg::PointCloud2 cloud;
projector.projectLaser(scan, cloud, -1.0,
  laser_geometry::channel_option::Intensity |
  laser_geometry::channel_option::Index);
// cloud.header.frame_id == scan.header.frame_id

11.2 变换到 fixed frame（C++）

1
2
3

projector.transformLaserScanToPointCloud(
  "odom", scan, cloud, tf_buffer);
// cloud.header.frame_id == "odom"

11.3 Python

1 2	from laser_geometry import LaserProjection cloud = LaserProjection().projectLaser(scan)

12. 推荐阅读顺序

laser_geometry.hpp — API 与 ChannelOption 文档
projectLaser_ — 投影 + 过滤 + 字段布局
transformLaserScanToPointCloud_（带 quat 版本） — 运动补偿核心
transformLaserScanToPointCloud_（带 tf 版本） — TF 查表入口
laser_geometry.py — Python 子集对照
rviz laser_scan_display.cpp — 真实集成方式

13. 小结

laser_geometry 解决两个经典问题：LaserScan → PointCloud2 的极坐标投影，以及 扫描期间运动造成的几何畸变（通过 scan 起止 TF + 逐 beam 插值）。C++ 版功能完整；Python 版仅 projectLaser。在 Humble 中它是 已完整 ROS 2 端口 的感知基础库，RViz 激光显示是其最主要消费者。

如需，我可以把本文写入 ros2doc/ros-perception/laser_geometry源码详细分析.md，或继续分析 Nav2 costmap 如何直接使用 LaserScan（不经 laser_geometry） 的对比。

文章互动

阅读 --

留言

0 条留言

正在加载留言…

fishBlog

专注嵌入式开发

laser_geometry 源码详细分析

laser_geometry 源码详细分析

1. 仓库结构

2. 在感知栈中的位置

3. 核心类：`LaserProjection`

3.1 两个公共方法（C++）

3.2 可选输出通道（ChannelOption）

4. `projectLaser` 算法

4.1 极坐标投影

4.2 cos/sin 缓存

4.3 有效点过滤

4.4 PointCloud2 布局

5. `transformLaserScanToPointCloud`：运动畸变补偿

5.1 流程

5.2 时间端点

5.3 逐点插值变换

5.4 Index 通道的临时使用

6. Python 实现

7. 下游集成：RViz LaserScanDisplay

8. 依赖关系

9. 测试

10. 设计特点与局限

11. 使用示例

11.1 静态投影（C++）

11.2 变换到 fixed frame（C++）

11.3 Python

12. 推荐阅读顺序

13. 小结

文章互动

留言

laser_geometry 源码详细分析

1. 仓库结构

2. 在感知栈中的位置

3. 核心类：LaserProjection

3.1 两个公共方法（C++）

3.2 可选输出通道（ChannelOption）

4. projectLaser 算法

4.1 极坐标投影

4.2 cos/sin 缓存

4.3 有效点过滤

4.4 PointCloud2 布局

5. transformLaserScanToPointCloud：运动畸变补偿

5.1 流程

5.2 时间端点

5.3 逐点插值变换

5.4 Index 通道的临时使用

6. Python 实现

7. 下游集成：RViz LaserScanDisplay

8. 依赖关系

9. 测试

10. 设计特点与局限

11. 使用示例

11.1 静态投影（C++）

11.2 变换到 fixed frame（C++）

11.3 Python

12. 推荐阅读顺序

13. 小结

文章互动

留言

3. 核心类：`LaserProjection`

4. `projectLaser` 算法

5. `transformLaserScanToPointCloud`：运动畸变补偿