基于Mid-360与FAST-LIO的导航地图生成全流程技术大纲

技术背景与目标

• FAST-LIO实时建图与点云保存机制：介绍mapping_mid360.launch中pcd_save_enable参数的作用及PCD文件生成逻辑
• 导航地图需求：去顶（移除天花板点云）、动态物体滤波、PGM/YAML格式适配SLAM框架（如ROS导航栈）

---

FAST-LIO点云数据预处理流程

        先保证雷达（Mid-360的驱动正常安装，并且在它的环境下面正常配置了FAST-LIO算法），进入工作空间，先输入下列代码保证雷达和FAST-LIO正常工作

#进入工作空间，启动雷达
 
source devel/setup.bash
roslaunch livox_ros_driver2 msg_MID360.launch

#重开一个终端，启动FAST_LIO

source devel/setup.bash
roslaunch fast_lio mapping_mid360.launch 

这个时候Rviz会正常弹出打开，使用者可以通过自己的雷达开始建图，到Rviz中已经显示觉得不错的图像的时候，这个时候在运行了FAST-LIO的终端中按住Ctrl+c ，中断建图。

PCD文件获取与初始处理

• 环境配置：FAST-LIO运行环境（ROS、Livox Mid-360驱动）、PCD文件存储路径设置（默认配置到/FAST-LIO/PCD文件夹中）
• 点云特性分析：Mid-360点云密度、噪声分布、动态物体干扰

• 承接上述建图，为了获取点云文件，首先要保证mapping_mid360.launch中已经配置

  <param name="pcd save enabkle" value="1">

  若没有配置，则无法在/FAST-LIO/PCD文件夹中看到默认的scans.pcd输出

PCD文件后续去顶，动态滤波，转.pgm和.yaml文件的操作：

去顶处理（remove_ceiling.cpp）

• 算法原理：基于高度阈值或RANSAC平面分割的天花板检测
• 实现细节：PCL库调用、去顶后点云可视化验证

  #include <pcl/io/pcd_io.h>
  #include <pcl/filters/passthrough.h>
  #include <iostream>
  
  int main(int argc, char**argv) {
      // 加载点云文件，这里假设要处理的点云文件是 scans3.pcd，若实际文件名不同需修改
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
      if (pcl::io::loadPCDFile<pcl::PointXYZ>("../scans3.pcd", *cloud) == -1) {
          std::cerr << "错误：无法加载 scans3.pcd 文件！请检查文件是否存在或路径是否正确" << std::endl;
          return -1;
      }
      std::cout << "原始点云包含 " << cloud->points.size() << " 个点" << std::endl;
  
      // 创建直通滤波器，用于过滤 Z 轴（高度）方向的点
      pcl::PassThrough<pcl::PointXYZ> pass;
      pass.setInputCloud(cloud);
      pass.setFilterFieldName("z");  // 指定过滤的字段为 Z 轴（高度）
      // 设置 Z 轴的过滤范围，这里保留 Z 轴在 0.0 到 1.8 之间的点，可根据实际场景调整范围
      pass.setFilterLimits(0.0, 1.8);
      pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
      pass.filter(*filtered_cloud);
  
      // 保存过滤后的点云文件，保存到桌面，文件名为 filtered_scans3.pcd
      pcl::io::savePCDFileASCII("../filtered_scans3.pcd", *filtered_cloud);
      std::cout << "过滤后点云包含 " << filtered_cloud->points.size() << " 个点" << std::endl;
      std::cout << "已将过滤后的点云保存为 filtered_scans3.pcd" << std::endl;
  
      return 0;
  }

  先在桌面建立一个名叫remove_ceiling.cpp的文件，然后把上述代码复制进去，具体的参数可以按照自己的需求进行更改。去顶之后，pcd图像生成为这个样子

• 注意代码中的scans3,使用的时候一定要改为自己实际pcd的名字，至于filtered_scans3,读者为了方便可以不进行更改，从而方便下面的操作。

动态物体滤波（filter_dynamic.cpp）

• 方法选择：统计离群点移除（SOR）或时序一致性滤波
• 参数调优：动态物体与静态环境的分离阈值设置

• #include <pcl/io/pcd_io.h>
  #include <pcl/point_types.h>
  #include <pcl/filters/radius_outlier_removal.h>
  #include <pcl/filters/statistical_outlier_removal.h>
  #include <iostream>
  
  int main() {
      // 1. 加载原始点云（路径适配新文件夹结构）
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
      if (pcl::io::loadPCDFile<pcl::PointXYZ>("~/Desktop/filtered_scans3.pcd", *cloud) == -1) {
          std::cerr << "错误：无法加载 filtered_scans3.pcd 文件！" << std::endl;
          return -1;
      }
      std::cout << "原始点云点数: " << cloud->points.size() << std::endl;
  
      // 2. 统计滤波（去除离群噪声）
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered_sor(new pcl::PointCloud<pcl::PointXYZ>);
      pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
      sor.setInputCloud(cloud);
      sor.setMeanK(50);
      sor.setStddevMulThresh(1.0);
      sor.filter(*cloud_filtered_sor);
      std::cout << "统计滤波后点数: " << cloud_filtered_sor->points.size() << std::endl;
  
      // 3. 半径滤波（去除动态障碍）
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_static(new pcl::PointCloud<pcl::PointXYZ>);
      pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror;
      ror.setInputCloud(cloud_filtered_sor);
      ror.setRadiusSearch(0.5);
      ror.setMinNeighborsInRadius(10);
      ror.filter(*cloud_static);
      std::cout << "去除动态障碍后点数: " << cloud_static->points.size() << std::endl;
  
      // 4. 保存结果（保持保存到桌面）
      pcl::io::savePCDFileASCII("~/Desktop/static_cloud.pcd", *cloud_static);
      std::cout << "已保存静态点云到 static_cloud.pcd" << std::endl;
  
      return 0;
  }

  建立一个filter_dynamic.cpp,把上述代码复制进去，这个cpp的作用是生成一个滤波之后的pcd文件，滤波效果如下图所示

  

• 由此可见，现在的pcd图像已经很干净了，可以进行转化为pgm和yaml格式的操作了

---

PGM/YAML地图生成（pcd_to_pgm.cpp）

一、输入处理

• 静态点云加载（去除动态障碍后）

• 自动计算点云XY坐标边界

二、栅格地图生成

• 可调分辨率（默认0.05米/像素）

• 自动计算地图尺寸（宽高像素）

• 点云坐标到像素坐标转换

• 二值化处理：255=障碍物，0=自由空间

三、文件输出

• PGM地图：灰度图像格式

• YAML配置文件：包含关键元数据

  • image: 地图文件名

  • resolution: 分辨率

  • origin: 原点坐标

  • negate: 0（白为障碍）

  • occupied_thresh: 0.65

  • free_thresh: 0.196

四、ROS兼容性

• 标准PGM+YAML格式

• 预设参数优化（适配move_base）

• 固定桌面输出路径

  #include <pcl/io/pcd_io.h>
  #include <pcl/point_types.h>
  #include <opencv2/opencv.hpp>
  #include <vector>
  #include <fstream>
  
  #include <pcl/io/pcd_io.h>
  #include <pcl/point_types.h>
  #include <opencv2/opencv.hpp>
  #include <vector>
  #include <fstream>
  
  int main() {
      // 1. 加载静态点云（去除动态障碍后的点云，调整为从桌面加载）
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
      if (pcl::io::loadPCDFile<pcl::PointXYZ>("/home/vd/Desktop/static_cloud.pcd", *cloud) == -1) {
          std::cerr << "错误：无法加载 static_cloud.pcd 文件！请检查文件是否存在" << std::endl;
          return -1;
      }
  
      // 2. 确定点云 x/y 坐标范围（用于计算地图尺寸）
      float min_x = FLT_MAX, max_x = FLT_MIN;
      float min_y = FLT_MAX, max_y = FLT_MIN;
      for (const auto& point : cloud->points) {
          min_x = std::min(min_x, point.x);
          max_x = std::max(max_x, point.x);
          min_y = std::min(min_y, point.y);
          max_y = std::max(max_y, point.y);
      }
  
      // 3. 设置地图分辨率（单位：米/像素，可根据需要调整）
      float resolution = 0.05;  // 5厘米/像素，数值越小地图越精细
      int width = static_cast<int>((max_x - min_x) / resolution) + 1;  // 地图宽度（像素）
      int height = static_cast<int>((max_y - min_y) / resolution) + 1; // 地图高度（像素）
  
      // 4. 创建占据栅格地图（0=空闲，255=有障碍物）
      cv::Mat map = cv::Mat::zeros(height, width, CV_8UC1);  // 初始化为黑色（空闲）
      for (const auto& point : cloud->points) {
          // 将点云坐标转换为地图像素坐标
          int x = static_cast<int>((point.x - min_x) / resolution);
          int y = static_cast<int>((point.y - min_y) / resolution);
          // 确保坐标在地图范围内
          if (x >= 0 && x < width && y >= 0 && y < height) {
              map.at<uchar>(y, x) = 255;  // 标记为障碍物（白色）
          }
      }
  
      // 5. 保存为 PGM 格式地图（保存到桌面，move_base 可识别）
      cv::imwrite("/home/vd/Desktop/map.pgm", map);
  
      // 6. 生成配套的 YAML 配置文件（保存到桌面，move_base 必需）
      std::ofstream yaml_file("/home/vd/Desktop/map.yaml");
      yaml_file << "image: map.pgm\n";          // PGM 地图文件名（相对于YAML文件的路径，这里都在桌面，可直接写文件名）
      yaml_file << "resolution: " << resolution << "\n";  // 分辨率（米/像素）
      yaml_file << "origin: [" << min_x << ", " << min_y << ", 0.0]\n";  // 地图原点（左下角坐标）
      yaml_file << "negate: 0\n";               // 0=白色表示障碍物，1=相反
      yaml_file << "occupied_thresh: 0.65\n";   // 占据阈值
      yaml_file << "free_thresh: 0.196\n";      // 空闲阈值
      yaml_file.close();
  
      std::cout << "地图生成完成！已在桌面保存 map.pgm 和 map.yaml" << std::endl;
      return 0;
  }

  这里的代码核心就是生成一个pmg文件和一个yaml配置文件，供给move_base一些基础的定位导航信息，在桌面上生成map.pgm和map.yaml

---

GIMP后处理优化

栅格地图编辑

• 噪声修复：手动擦除残留动态物体痕迹
• 边沿修复：手动修改边缘的建图噪声

在GIMP中打开所需要更改的pgm文件

将不需要的障碍物（白色），手动涂为黑色（通过点击这个换颜色）

然后把current这个颜色换为我们目标的颜色，这个时候就可以进行更改了，更改完成后 ，点击file中的export as 在桌面中进行输出 便于修改目标的查询。（记得要把文件的后缀改为pgm格式）

这个时候，按照自己的需求去修改之前的map.yaml文件，一般情况下，直接修改image中的文件名字，和修改的pgm名字符合就行了，至于更多的yaml参数设置，则需要自己更具情况设置。

三个.cpp文件的使用说明

在桌面上建立一个名字为point_cloud_tools的文件夹

然后把三个.cpp文件都放进去，之后生成可编译文件

# 进入 point_cloud_tools 目录
cd ~/Desktop/point_cloud_tools

# 进入 build 目录（若没有则创建）
mkdir -p build && cd build

# 生成编译配置
cmake ..

# 编译
make

编译成功后，就可以开始脚本的使用了（在文件夹的终端下进行操作）

1.运行remove_ceiling(去顶)

cd ~/Desktop/point_cloud_tools/build
./remove_ceiling

2.运行filter_dynamic(去动态障碍)

./filter_dynamic

3.运行pcd_to_pgm(转PGM地图)

./pcd_to_pgm

这三步的过程可能有些缓慢，可以通过查看CPU运行的方式看看脚本是不是在运行，要是CPU占用很大的话，说明在运行，要是CPU没有运行占比很高的话，则需要进行更加深度的检查。

运行完三个脚本之后，可以生成相关的.pmg格式文件和相关的.yaml文件，可以根据生成的情况来选择是否进行GIMP的修改。

---