4. 从激光雷达扫描数据生成点云

任务目标

本案例将模拟激光雷达单线扫描输出（距离 r 与角度 θ），解析后转换为笛卡尔坐标系下的点云 (x, y)，并输出为文本文件供后续滤波处理使用。

实现原理

激光雷达通常以极坐标形式输出测量值：距离 r 和扫描角度 θ（本文以度为单位，x 轴正方向为 0°，逆时针为正）。转换为笛卡尔坐标的公式为：

x = r * cos(θ_rad)
y = r * sin(θ_rad)

其中 θ_rad = θ * π / 180。本案例生成 360° 范围内每 1° 一个测量点的模拟数据，并加入少量高斯噪声以贴近真实传感器特性。

完整代码

import numpy as np

# ========== 1. 模拟激光雷达扫描数据 ==========
def simulate_lidar_scan(num_points=360, max_range=10.0, noise_std=0.02):
    """生成模拟单线激光雷达数据：距离 + 角度（度）"""
    angles_deg = np.linspace(0, 360, num_points, endpoint=False)
    # 模拟一个前方有障碍物的场景：距离随角度变化
    true_ranges = 5.0 + 2.0 * np.sin(np.deg2rad(angles_deg * 2))
    true_ranges = np.clip(true_ranges, 1.0, max_range)
    # 加入高斯噪声模拟测量误差
    ranges = true_ranges + np.random.normal(0, noise_std, size=angles_deg.shape)
    ranges = np.clip(ranges, 0.1, max_range)
    return angles_deg, ranges

# ========== 2. 极坐标转笛卡尔坐标 ==========
def polar_to_cartesian(angles_deg, ranges):
    """将 (r, θ) 转换为 (x, y) 点云"""
    angles_rad = np.deg2rad(angles_deg)
    x = ranges * np.cos(angles_rad)
    y = ranges * np.sin(angles_rad)
    return np.column_stack((x, y))

# ========== 3. 保存点云到文本文件 ==========
def save_point_cloud(points, filename="lidar_scan.xyz"):
    """保存为每行 'x y' 的文本文件"""
    np.savetxt(filename, points, fmt="%.4f", header="x y", comments="")
    print(f"点云已保存至: {filename}，共 {len(points)} 个点")

# ========== 主流程 ==========
if __name__ == "__main__":
    # 固定随机种子保证可复现
    np.random.seed(42)

    # 生成模拟扫描数据
    angles, ranges = simulate_lidar_scan()

    # 转换为点云
    point_cloud = polar_to_cartesian(angles, ranges)

    # 保存结果
    save_point_cloud(point_cloud)

    # 打印前 5 个点作为示例输出
    print("\n前 5 个点 (x, y):")
    print(point_cloud[:5])

运行与验证

运行方式：确保已安装 NumPy，在终端执行：

python lidar_to_pointcloud.py

预期输出：程序将生成 lidar_scan.xyz 文件，每行包含一个点的 x y 坐标，共 360 个点。控制台打印前 5 个点的坐标，格式类似：

点云已保存至: lidar_scan.xyz，共 360 个点

前 5 个点 (x, y):
[[5.0123  0.    ]
 [4.9987  0.0873]
 [4.9876  0.1745]
 [4.9765  0.2618]
 [4.9654  0.3491]]

验证方法：检查输出文件中点的数量是否为 360；用任意文本编辑器打开 lidar_scan.xyz，确认每行两个浮点数且数值在合理范围（x, y ∈ [-10, 10]）内。由于固定了随机种子，每次运行结果一致。

可视化参考

上图示意了本案例生成的模拟点云在笛卡尔坐标系中的分布形态（实际点云因噪声略有偏移）。转换后的点云文件可直接作为后续滤波、聚类等处理步骤的输入。