三维SLAM算法SegMap源码阅读（一）概要篇

最新推荐文章于 2025-05-21 11:38:56 发布

原创最新推荐文章于 2025-05-21 11:38:56 发布 · 3k 阅读

25 ·

CC 4.0 BY-SA版权

SLAM算法阅读专栏收录该内容

21 篇文章

订阅专栏

本文深入探讨了SegMap技术，一种基于三维点云的SLAM方案，通过点云分割与语义识别提升环境适应性和鲁棒性。文章分析了SegMap的前端模块，包括增量估计器、匹配器和里程计，以及后端的回环检测与地图更新过程。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

今天来阅读一下比较前沿的三维点云与SLAM技术的SegMap，望文生义，它是基于点云的分割（segmentation）与语义识别，从而降低数据的处理量，这样也降低了对环境的依赖，增强了鲁棒性。要注意，在下载源码的同时需要下载laser_slam包，地址为https://github.com/ethz-asl/laser_slam，这个模块是slam的前端模块。

我们先看看运行它的launch文件：

<?xml version="1.0" encoding="utf-8"?>

<launch>
  <!-- ICP configuration file -->
  <arg name="icp_configuration_file" default="$(find segmapper)/launch/icp_dynamic_indoor.yaml"/>
  <!-- Input filters file -->
  <arg name="icp_input_filters_file" default="$(find segmapper)/launch/input_filters_indoor.yaml"/>

  <!-- Enable callgrind profiling -->
  <arg name="enable_callgrind" default="false"/>
  <arg name="callgrind_ouput_path" value="/tmp/segmatch-callgrind.out"/>
  <arg name="launch_prefix" value="valgrind --tool=callgrind --callgrind-out-file=$(arg callgrind_ouput_path) --instr-atstart=no" if="$(arg enable_callgrind)"/>
  <arg name="launch_prefix" value="" unless="$(arg enable_callgrind)"/>

  <!-- segmapper node -->
  <node name="segmapper" pkg="segmapper" type="segmapper_node" output="screen" respawn="false" launch-prefix="$(arg launch_prefix)">
    <param name="icp_configuration_file" value="$(arg icp_configuration_file)"/>
    <param name="icp_input_filters_file" value="$(arg icp_input_filters_file)"/>
  </node>

</launch>

发现它只有一个运行节点，转到segmapper_node.cpp：

SegMapper mapper(node_handle);

std::thread publish_map_thread(&SegMapper::publishMapThread, &mapper);
std::thread publish_tf_thread(&SegMapper::publishTfThread, &mapper);
std::thread segmatch_thread(&SegMapper::segMatchThread, &mapper);

它拥有三个线程，分别用于发布地图、发布位置信息以及点云匹配。主线程是SegMapper的对象，再转到构造函数，在构造函数中创造了IncrementalEstimator的对象，以及LaserSlamWorker的对象，也就是说，slam的前端位于SegMapper构造函数中，而后端位于segMatchThread线程中。

前端的部分包括如下模块：

1.IncrementalEstimator（增量估计器）

它的具体实现在laser_slam包中，增量估计其实就是在因子图中，用中间结果去递增地计算新的一帧传来的位姿数据，而不是像图优化的过程中更新整个已构建的graph。这样的优越性在于速度较快，需要优化的部分就交给回环检测后统一更新。

2.SegMatch（匹配器）

这一块分为描述器、分割器与分类器，正是segmap让人不明觉厉的部分，大致是利用欧几里得距离分割各个点云，再使用knn对点云进行识别与分类，达到语义地图的作用，这一块之后再详细学习一下。

3.LaserSlamWorker（里程计）

这一部分也是位于laser_slam包中，起到的作用就是slam中更容易理解的前端，也就是里程计作用，具体是在laser_track中，利用ICP算法对帧与帧之间的点云进行配准，从而得到新一帧的位姿，并在因子图中进行更新。

相对于前端部分，后端部分更加传统一些，最主要的内容是更新已建成的地图以及回环检测，而回环检测成功后便会对整个因子图进行重新更新。

因此，整个SegMap的建图过程是前后端协作的过程，在前端，SegMatch更新语义模型同时LaserSlamWorker利用匹配更新机器人位姿，后端进行回环检测，累加点云进行地图构建。

另外看看两个分线程的实现，首先是发布地图的线程，这个线程在不断地发送地图点云消息（默认频率是3Hz）。

void SegMapper::publishMapThread() {
  // Check if map publication is required.
  if (!laser_slam_worker_params_.publish_local_map)
    return;

  ros::Rate thread_rate(laser_slam_worker_params_.map_publication_rate_hz);
  while (ros::ok()) {
    LOG(INFO) << "publishing local maps";
    MapCloud local_maps;
    for (size_t i = 0u; i < local_maps_.size(); ++i) {
      std::unique_lock<std::mutex> map_lock(local_maps_mutexes_[i]);
      local_maps += local_maps_[i].getFilteredPoints();
      map_lock.unlock();
    }
    sensor_msgs::PointCloud2 msg;
    laser_slam_ros::convert_to_point_cloud_2_msg(
        local_maps,
        params_.world_frame, &msg);
    local_map_pub_.publish(msg);
    thread_rate.sleep();
  }
}

再接下来是发布TF的线程，laser_slam_workers就是参与建图的机器人数量，默认为1，getWorldToOdom是位于laser_slam_worker.cpp中。

void SegMapper::publishTfThread() {
  if (params_.publish_world_to_odom) {
    ros::Rate thread_rate(params_.tf_publication_rate_hz);
    while (ros::ok()) {
      for (size_t i = 0u; i < laser_slam_workers_.size(); ++i) {
        tf::StampedTransform world_to_odom = laser_slam_workers_[i]->getWorldToOdom();
        world_to_odom.stamp_ = ros::Time::now();
        tf_broadcaster_.sendTransform(world_to_odom);
      }
      thread_rate.sleep();
    }
  }
}

更重要的是前后端线程，即匹配的线程以及建图并优化的线程，咱们下一篇再进行详细分析。