FAST-LIO2在ROS2中的性能调优与实战技巧：从建图到重定位

FAST-LIO2在ROS2中的性能调优与实战技巧：从建图到重定位

在机器人自主导航领域，激光惯性里程计（LIO）系统已成为实现高精度定位与建图的核心技术。FAST-LIO2作为其中的佼佼者，以其紧耦合的激光-惯性里程计框架和高效的IKD-Tree数据结构，在学术界和工业界获得了广泛认可。然而，将FAST-LIO2部署到实际的ROS2项目中时，许多开发者会发现，仅仅完成算法复现还远远不够——真正的挑战在于如何让系统在资源受限的硬件平台上稳定高效地运行，并实现从建图到重定位的全流程优化。

本文将深入探讨FAST-LIO2在ROS2环境中的性能调优策略与实战技巧，面向已有FAST-LIO2基础但希望进一步提升系统性能与稳定性的工程师和研究人员。我们将跳过基础安装与配置，直接聚焦于高级优化技术，包括线程模型调整、动态链接库优化、实时性能提升技巧，以及建图、回环检测、重定位和地图优化的全流程实战经验。

1. 环境准备与依赖库深度优化

在开始性能调优之前，确保基础环境的正确配置至关重要。虽然许多教程已经介绍了Ubuntu 22.04和ROS2 Humble的基础安装，但针对FAST-LIO2的性能优化需要更细致的依赖库处理。

1.1 关键依赖库的编译优化

Sophus库的特殊配置：最新版本的Sophus库默认依赖fmt库，这可能引入不必要的兼容性问题。建议使用1.22.10版本并通过编译选项禁用高级日志功能：

git clone https://github.com/strasdat/Sophus.git
cd Sophus
git checkout 1.22.10
mkdir build && cd build
cmake .. -DSOPHUS_USE_BASIC_LOGGING=ON
make -j$(nproc)
sudo make install

GTSAM库的编译与路径配置：GTSAM是FAST-LIO2后端优化的核心依赖，但其编译过程常遇到动态链接库问题。以下是优化后的编译流程：

git clone https://github.com/borglab/gtsam.git
cd gtsam/gtsam/3rdparty/cephes

# 创建必要的dllexport.h文件
cat > dllexport.h << 'EOF'
#ifndef DLLEXPORT_H
#define DLLEXPORT_H
#ifdef _WIN32
  #ifdef GTSAM_SHARED_LIB
    #define DLLEXPORT __declspec(dllexport)
  #else
    #define DLLEXPORT __declspec(dllimport)
  #endif
#else
  #define DLLEXPORT
#endif
#endif
EOF

cd ../..
mkdir build && cd build
cmake .. -DGTSAM_BUILD_WITH_MARCH_NATIVE=ON
make -j$(nproc)
sudo make install

编译完成后，务必确认libgtsam.so.4的安装路径并将其添加到系统库路径中：

# 查找动态链接库位置
find /usr/local/lib -name "libgtsam.so.4*"

# 永久添加到环境变量
echo 'export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH' >> ~/.bashrc
source ~/.bashrc

提示：如果遇到地图保存时找不到libgtsam.so.4的问题，除了设置LD_LIBRARY_PATH外，还可以考虑使用ldconfig更新系统库缓存：sudo ldconfig /usr/local/lib

1.2 ROS2工作区配置优化

为提升编译效率和运行时性能，建议对ROS2工作区进行如下优化：

# 在colcon构建时启用更优化的编译选项
echo "export CMAKE_CXX_FLAGS='-O3 -march=native -mtune=native'" >> ~/.bashrc
echo "export CMAKE_C_FLAGS='-O3 -march=native -mtune=native'" >> ~/.bashrc

# 配置colcon构建系统以使用并行编译
echo "export COLCON_EXTRA_CMAKE_ARGS=\"-DCMAKE_BUILD_TYPE=Release\"" >> ~/.bashrc
source ~/.bashrc

2. FAST-LIO2核心线程模型优化

FAST-LIO2的默认配置可能无法充分发挥多核处理器的性能，特别是在处理高频率激光雷达数据时。通过调整线程模型，可以显著提升系统实时性能。

2.1 回调组与多线程执行器配置

ROS2提供了灵活的回调组机制，允许开发者精细控制回调函数的执行方式。以下是如何优化LocalizerNode的线程模型：

// 在LocalizerNode构造函数中创建独立的回调组
m_timer_callback_group = this->create_callback_group(
  rclcpp::CallbackGroupType::MutuallyExclusive);
m_subscriber_callback_group = this->create_callback_group(
  rclcpp::CallbackGroupType::MutuallyExclusive);
m_service_callback_group = this->create_callback_group(
  rclcpp::CallbackGroupType::MutuallyExclusive);

// 为订阅者设置回调组
rclcpp::SubscriptionOptions subscription_options;
subscription_options.callback_group = m_subscriber_callback_group;

// 创建多线程执行器
int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  
  // 创建多线程执行器，使用3个线程
  rclcpp::executors::MultiThreadedExecutor executor(
    rclcpp::ExecutorO