SLAM 算法复现记录：Co-LRIO

• uwb 多机slam

1. 项目基本信息

• 系统：Ubuntu 20.04
• ROS 版本：Noetic
• 项目地址：https://github.com/PengYu-Team/Co-LRIO
• 这是我的复现过程记录，并非教程，因此会有一些不必要的操作

---

2. 本地复现

安装gtsam4.2a8

• gtsam4.2a8下载地址
• 安装 参考官方安装教程 https://github.com/borglab/gtsam

  mkdir build
  cd build
  cmake .. -DGTSAM_USE_SYSTEM_TBB=ON -DCMAKE_INSTALL_PREFIX=/home/sd10/third_party/gtsam_4.2a8  #（安装到自己设置的路径下，不唯一）
  make check (optional, runs unit tests)
  make install
  

  修改项目的 cmakelists.txt

cmakelists.txt：

cmake_minimum_required(VERSION 3.5)
project(co_lrio)

if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
  set(CMAKE_BUILD_TYPE Release)
endif()

# Find dependencies
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclpy REQUIRED)
find_package(std_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(pcl_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_sensor_msgs REQUIRED)
find_package(tf2_eigen REQUIRED)
find_package(libstatistics_collector REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(OpenCV REQUIRED)
find_package(OpenMP)
# 添加 Boost 依赖（GTSAM需要）
find_package(Boost REQUIRED COMPONENTS system filesystem serialization)
find_package(TBB REQUIRED)

if (OPENMP_FOUND)
  set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
  set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
endif()
find_package(PCL REQUIRED)
#find_package(GTSAM REQUIRED)
find_package(Eigen REQUIRED)

# --------------------------------------------------
# GTSAM配置
# --------------------------------------------------
# 清理旧缓存
unset(GTSAM_DIR CACHE)
unset(GTSAM_INCLUDE_DIR CACHE)
unset(GTSAM_LIBRARIES CACHE)
# 强制指定GTSAM的安装路径
set(GTSAM_DIR "/home/sd10/third_party/gtsam_4.2a8/lib/cmake/GTSAM")
find_package(
  GTSAM 
  4.2.0 EXACT 
  REQUIRED 
  NO_MODULE 
  NO_CMAKE_PATH          # 禁止搜索默认路径
  NO_CMAKE_SYSTEM_PATH   # 禁止搜索系统路径
)
# 验证是否找到正确版本
message(STATUS "Found GTSAM ${GTSAM_VERSION} at ${GTSAM_INCLUDE_DIR}")

# Include directories
include_directories(
	include
  include/third_parties
  ${TBB_INCLUDE_DIRS}
  ${EIGEN_INCLUDE_DIRS}
  ${PCL_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
  ${GTSAM_INCLUDE_DIR}  # 使用find_package找到的路径
  ${Boost_INCLUDE_DIRS}
)

# Defined message and service
set(msg_files
  msg/OptimizationRequest.msg
  msg/LoopClosure.msg
  msg/OptimizationResponse.msg
  srv/SaveFiles.srv
)

rosidl_generate_interfaces(${PROJECT_NAME} 
  ${msg_files}
  DEPENDENCIES std_msgs nav_msgs sensor_msgs geometry_msgs
)

# Maximum Clique Solver
add_library(fast_max_clique_finder
  include/third_parties/fast_max-clique_finder/src/findClique.h
  include/third_parties/fast_max-clique_finder/src/graphIO.h
  include/third_parties/fast_max-clique_finder/src/findClique.cpp
  include/third_parties/fast_max-clique_finder/src/findCliqueHeu.cpp
  include/third_parties/fast_max-clique_finder/src/utils.cpp
  include/third_parties/fast_max-clique_finder/src/graphIO.cpp
)
install(
  TARGETS fast_max_clique_finder
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
)

# nabo library
add_library(nabo_lib
  include/third_parties/nabo/nabo.h
  include/third_parties/nabo/nabo.cpp
  include/third_parties/nabo/nabo_private.h
  include/third_parties/nabo/index_heap.h
  include/third_parties/nabo/kdtree_opencl.cpp
  include/third_parties/nabo/brute_force_cpu.cpp
  include/third_parties/nabo/kdtree_cpu.cpp
)
install(
  TARGETS nabo_lib
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
)

# geo graphic library
add_library(geographic_lib
  include/third_parties/geo_graphic/Config.h
  include/third_parties/geo_graphic/Constants.hpp
  include/third_parties/geo_graphic/Geocentric.cpp
  include/third_parties/geo_graphic/Geocentric.hpp
  include/third_parties/geo_graphic/LocalCartesian.cpp
  include/third_parties/geo_graphic/LocalCartesian.hpp
  include/third_parties/geo_graphic/Math.cpp
  include/third_parties/geo_graphic/Math.hpp
)
install(
  TARGETS geographic_lib
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
)

# risam library
add_library(risam_lib
  include/third_parties/risam/DoglegLineSearch.h
  include/third_parties/risam/GraduatedFactor.cpp
  include/third_parties/risam/GraduatedFactor.h
  include/third_parties/risam/GraduatedKernel.cpp
  include/third_parties/risam/GraduatedKernel.h
  include/third_parties/risam/RISAM2.cpp
  include/third_parties/risam/RISAM2.h
)
target_link_libraries(risam_lib PRIVATE gtsam)
install(
  TARGETS risam_lib
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
)

# gicp library
add_library(gicp_lib
  include/third_parties/nabo/nabo.h
  include/third_parties/nabo/nabo.cpp
  include/third_parties/nabo/nabo_private.h
  include/third_parties/nabo/index_heap.h
  include/third_parties/nabo/kdtree_opencl.cpp
  include/third_parties/nabo/brute_force_cpu.cpp
  include/third_parties/nabo/kdtree_cpu.cpp

  include/third_parties/flann/flann.cpp
  include/third_parties/flann/flann.h

  include/third_parties/ikd_tree/ikd_Tree.h
  include/third_parties/ikd_tree/ikd_Tree.cpp

  include/third_parties/gicp/so3/so3.hpp
  include/third_parties/gicp/impl/fast_gicp_impl.hpp
  include/third_parties/gicp/impl/fast_vgicp_impl.hpp
  include/third_parties/gicp/impl/lsq_registration_impl.hpp
  include/third_parties/gicp/gicp_settings.hpp
  include/third_parties/gicp/fast_vgicp_voxel.hpp
  include/third_parties/gicp/fast_gicp.hpp
  include/third_parties/gicp/fast_gicp.cpp
 include/third_parties/gicp/fast_vgicp.hpp
 include/third_parties/gicp/fast_vgicp.cpp
  include/third_parties/gicp/lsq_registration.hpp
  include/third_parties/gicp/lsq_registration.cpp
)
install(
  TARGETS gicp_lib
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
)

# Lidar odometry
add_executable(${PROJECT_NAME}_lidar_odometry src/scanContextDescriptor.cpp src/lidarIrisDescriptor.cpp  src/lidarOdometry.cpp)
ament_target_dependencies(${PROJECT_NAME}_lidar_odometry
  rclcpp rclpy
  std_msgs nav_msgs sensor_msgs geometry_msgs visualization_msgs pcl_msgs
  tf2 tf2_ros tf2_geometry_msgs tf2_eigen tf2_sensor_msgs
  OpenCV PCL GTSAM Eigen)
#target_link_libraries(${PROJECT_NAME}_lidar_odometry gtsam gtsam_unstable gicp_lib #geo-graphic_lib)
target_link_libraries(${PROJECT_NAME}_lidar_odometry 
    ${TBB_LIBRARIES}
    ${GTSAM_LIBRARIES}
    ${Boost_LIBRARIES}
    "/home/sd10/third_party/gtsam_4.2a8/lib/libgtsam.so"
    "/home/sd10/third_party/gtsam_4.2a8/lib/libgtsam_unstable.so"
    gtsam_unstable
    gicp_lib 
    geographic_lib
    tbb
)

rosidl_target_interfaces(${PROJECT_NAME}_lidar_odometry ${PROJECT_NAME} "ro-sidl_typesupport_cpp")

# Concentrated mapping
add_executable(${PROJECT_NAME}_concentrated_mapping src/scanContextDescriptor.cpp src/lidarIrisDescriptor.cpp src/concentratedMapping.cpp)
ament_target_dependencies(${PROJECT_NAME}_concentrated_mapping
  rclcpp rclpy
  std_msgs nav_msgs sensor_msgs geometry_msgs visualization_msgs pcl_msgs
  tf2 tf2_ros tf2_geometry_msgs tf2_eigen tf2_sensor_msgs
  OpenCV PCL GTSAM Eigen)
#target_link_libraries(${PROJECT_NAME}_concentrated_mapping gtsam risam_lib #fast_max_clique_finder nabo_lib gicp_lib geographic_lib)
target_link_libraries(${PROJECT_NAME}_concentrated_mapping 
    ${TBB_LIBRARIES}
    ${GTSAM_LIBRARIES}
    ${Boost_LIBRARIES}
    "/home/sd10/third_party/gtsam_4.2a8/lib/libgtsam.so"
    risam_lib 
    fast_max_clique_finder 
    nabo_lib 
    gicp_lib 
    geographic_lib
    tbb
)

rosidl_target_interfaces(${PROJECT_NAME}_concentrated_mapping ${PROJECT_NAME} "ro-sidl_typesupport_cpp")

install(
  DIRECTORY launch config
  DESTINATION share/${PROJECT_NAME}/
)

install(
  TARGETS ${PROJECT_NAME}_lidar_odometry
  DESTINATION lib/${PROJECT_NAME}
)

install(
  TARGETS ${PROJECT_NAME}_concentrated_mapping
  DESTINATION lib/${PROJECT_NAME}
)

install(
  DIRECTORY "include/"
  DESTINATION include
)

ament_export_include_directories(include)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

编译

mkdir -p ~/cslam_ws/src
cd ~/cslam_ws/src
git clone https://github.com/PengYu-Team/Co-LRIO.git
cd ../
colcon build --symlink-install

修改 launch 文件

• 加入S3E相关的话题重映射

from launch import LaunchDescription
from launch.actions import SetEnvironmentVariable
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from ament_index_python.packages import get_package_share_directory
import os.path
import yaml
from launch.substitutions import TextSubstitution, LaunchConfiguration, Command

def generate_launch_description():

	ld = LaunchDescription()

	parameters_file_path = os.path.join(get_package_share_directory('co_lrio'), 'config', 'co_lrio_params.yaml')
	xacro_path = os.path.join(get_package_share_directory('co_lrio'), 'config', 'robot.urdf.xacro')

	# Set env var to print message to stdout immediately
	# arg = SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '0')
	# ld.add_action(arg)

	# single robot mapping node
	robot_1_static_node = Node(
		package='tf2_ros',
		namespace='robot_1',
		executable='static_transform_publisher',
		arguments='0.0 0.0 0.0 0.0 0.0 0.0 world robot_1/odom'.split(' '),
		parameters=[parameters_file_path]
	)
	ld.add_action(robot_1_static_node)

	robot_1_xacro_node = Node(
		package='robot_state_publisher',
		executable='robot_state_publisher',
		namespace='robot_1',
		name='robot_state_publisher',
		output='screen',
		parameters=[{'robot_description': Command(['xacro', ' ', xacro_path])}, {'frame_prefix' : 'robot_1/'}]
	)
	ld.add_action(robot_1_xacro_node)

	robot_1_lidar_odometry_node = Node(
		package = 'co_lrio',
		namespace = 'robot_1',
		executable = 'co_lrio_lidar_odometry',
		name = 'co_lrio_lidar_odometry',
		parameters = [parameters_file_path],
		arguments = ['--ros-args', '--log-level', 'INFO',
			   # 添加话题重映射
        '-r', '__ns:=/robot_1',  # 强制命名空间
        '-r', 'velodyne_points:=/Bob/velodyne_points',  # 映射原始话题
        '-r', 'imu/data:=/Bob/imu/data'],
		output='screen'
	)
	ld.add_action(robot_1_lidar_odometry_node)

	# robot_1_feature_to_map_matching_node = Node(
	# 	package = 'co_lrio',
	# 	namespace = 'robot_1',
	# 	executable = 'co_lrio_feature_to_map_matching',
	# 	name = 'co_lrio_feature_to_map_matching',
	# 	#parameters = [parameters_file_path],
	# 	arguments = ['--ros-args', '--log-level', 'INFO'],
	# 	output='screen'
	# )
	# ld.add_action(robot_1_feature_to_map_matching_node)

	# single robot mapping node
	robot_0_static_node = Node(
		package='tf2_ros',
		namespace='robot_0',
		executable='static_transform_publisher',
		arguments='0.0 0.0 0.0 0.0 0.0 0.0 world robot_0/odom'.split(' '),
		parameters=[parameters_file_path]
	)
	ld.add_action(robot_0_static_node)

	robot_0_xacro_node = Node(
		package='robot_state_publisher',
		executable='robot_state_publisher',
		namespace='robot_0',
		name='robot_state_publisher',
		output='screen',
		parameters=[{'robot_description': Command(['xacro', ' ', xacro_path])}, {'frame_prefix' : 'robot_0/'}]
	)
	ld.add_action(robot_0_xacro_node)

	# robot_0_feature_to_map_matching_node = Node(
	# 	package = 'co_lrio',
	# 	namespace = 'robot_0',
	# 	executable = 'co_lrio_feature_to_map_matching',
	# 	name = 'co_lrio_feature_to_map_matching',
	# 	arguments = ['--ros-args', '--log-level', 'INFO'],
	# 	output='screen'
	# )
	# ld.add_action(robot_0_feature_to_map_matching_node)

	robot_0_lidar_odometry_node = Node(
		package = 'co_lrio',
		namespace = 'robot_0',
		executable = 'co_lrio_lidar_odometry',
		name = 'co_lrio_lidar_odometry',
		parameters = [parameters_file_path],
		arguments = ['--ros-args', '--log-level', 'INFO', 
			   # 添加话题重映射
        		'-r', '__ns:=/robot_0',  # 强制命名空间
        		'-r', 'velodyne_points:=/Alpha/velodyne_points',  # 映射原始话题
       			'-r', 'imu/data:=/Alpha/imu/data'],
		output='screen'
	)
	ld.add_action(robot_0_lidar_odometry_node)

	# single robot mapping node
	robot_2_static_node = Node(
		package='tf2_ros',
		namespace='robot_2',
		executable='static_transform_publisher',
		arguments='0.0 0.0 0.0 0.0 0.0 0.0 world robot_2/odom'.split(' '),
		parameters=[parameters_file_path]
	)
	ld.add_action(robot_2_static_node)

	robot_2_xacro_node = Node(
		package='robot_state_publisher',
		executable='robot_state_publisher',
		namespace='robot_2',
		name='robot_state_publisher',
		output='screen',
		parameters=[{'robot_description': Command(['xacro', ' ', xacro_path])}, {'frame_prefix' : 'robot_2/'}]
	)
	ld.add_action(robot_2_xacro_node)

	robot_2_lidar_odometry_node = Node(
		package = 'co_lrio',
		namespace = 'robot_2',
		executable = 'co_lrio_lidar_odometry',
		name = 'co_lrio_lidar_odometry',
		parameters = [parameters_file_path],
		arguments = ['--ros-args', '--log-level', 'INFO',
			   # 添加话题重映射
        '-r', '__ns:=/robot_2',  # 强制命名空间
        '-r', 'velodyne_points:=/Carol/velodyne_points',  # 映射原始话题
        '-r', 'imu/data:=/Carol/imu/data'],
		output='screen'
	)
	ld.add_action(robot_2_lidar_odometry_node)

	# robot_2_feature_to_map_matching_node = Node(
	# 	package = 'co_lrio',
	# 	namespace = 'robot_2',
	# 	executable = 'co_lrio_feature_to_map_matching',
	# 	name = 'co_lrio_feature_to_map_matching',
	# 	arguments = ['--ros-args', '--log-level', 'INFO'],
	# 	output='screen'
	# )
	# ld.add_action(robot_2_feature_to_map_matching_node)

	# centrailized mapping node
	centrailized_mapping_node = Node(
		package = 'co_lrio',
		namespace = '',
		executable = 'co_lrio_concentrated_mapping',
		name = 'co_lrio_concentrated_mapping',
		parameters = [parameters_file_path],
		arguments = ['--ros-args', '--log-level', 'INFO'],
		output='screen'
	)
	ld.add_action(centrailized_mapping_node)

	# centrailized rviz
	rviz_config_path = os.path.join(get_package_share_directory('co_lrio'), 'config', 'rviz2_three_robot.rviz')
	rviz_node = Node(
		package = 'rviz2',
		namespace = '',
		executable = 'rviz2',
		name = 'co_lrio_rviz',
		respawn=True, 
		arguments = ['-d' + rviz_config_path]
	)
	ld.add_action(rviz_node)

	# rviza_config_path = os.path.join(get_package_share_directory('co_lrio'), 'config', 'rviz2_a.rviz')
	# rviza_node = Node(
	# 	package = 'rviz2',
	# 	namespace = '',
	# 	executable = 'rviz2',
	# 	name = 'a_rviz',
	# 	arguments = ['-d' + rviza_config_path]
	# )
	# ld.add_action(rviza_node)

	# rvizb_config_path = os.path.join(get_package_share_directory('co_lrio'), 'config', 'rviz2_b.rviz')
	# rvizb_node = Node(
	# 	package = 'rviz2',
	# 	namespace = '',
	# 	executable = 'rviz2',
	# 	name = 'b_rviz',
	# 	arguments = ['-d' + rvizb_config_path]
	# )
	# ld.add_action(rvizb_node)

	# rvizc_config_path = os.path.join(get_package_share_directory('co_lrio'), 'config', 'rviz2_c.rviz')
	# rvizc_node = Node(
	# 	package = 'rviz2',
	# 	namespace = '',
	# 	executable = 'rviz2',
	# 	name = 'c_rviz',
	# 	arguments = ['-d' + rvizc_config_path]
	# )
	# ld.add_action(rvizc_node)

	return ld

3. docker 复现

• 确保docker网络没有问题，在对应目录下创建Dockerfile文件，内容为：

#使用 ROS 2 Foxy 官方镜像（基于 Ubuntu 20.04）
FROM osrf/ros:foxy-desktop

# 设置环境变量，避免交互安装
ENV DEBIAN_FRONTEND=noninteractive

# 更新系统软件包并安装基础工具
RUN apt-get update && \
    apt-get install -y \
    git \
    build-essential \
    curl \
    cmake \
    lsb-release \
    gnupg2 \
    sudo \
    python3-colcon-common-extensions \
    python3-pip \
    python3-rosdep \
    libeigen3-dev \
    libpcl-dev \
    ros-foxy-pcl-msgs \
    ros-foxy-pcl-ros \
    ros-foxy-xacro \
    unzip \
    libusb-1.0-0-dev \
    libpcap-dev \
    libpng-dev \
    libopenni-dev \
    libopenni2-dev \
    ros-foxy-ament-cmake && \
    apt-get clean && \
    rm -rf /var/lib/apt/lists/*

# 复制解压后的 GTSAM 文件到 Docker 内部
COPY ./gtsam-4.2a8 /tmp/gtsam-4.2a8

# 编译并安装 GTSAM
RUN cd /tmp/gtsam-4.2a8 && \
    mkdir build && cd build && \
    cmake .. -DCMAKE_INSTALL_PREFIX=/usr/local && \
    make -j$(nproc) && make check && make install && \
    cd / && rm -rf /tmp/gtsam-4.2a8

# 6. 初始化 rosdep 并更新源
#RUN rosdep init && \
#    sed -i 's|http://|https://|g' /etc/ros/rosdep/sources.list.d/20-default.list && \
#    rosdep update


# 创建工作空间并克隆 Co-LRIO 代码
RUN mkdir -p /workspace/cslam_ws/src && \
    cd /workspace/cslam_ws/src 
COPY ./Co-LRIO /workspace/cslam_ws/src

# 设置默认工作目录
WORKDIR /workspace/cslam_ws

# 设置 ROS 2 环境并构建工作空间
RUN . /opt/ros/foxy/setup.sh && \
    colcon build --symlink-install

# 运行容器时配置 ROS 2 环境
CMD ["/bin/bash", "-c", "source /opt/ros/foxy/setup.bash && source /workspace/cslam_ws/install/setup.bash && exec bash"]

• 如果docker build是出现FROM osrf/ros:foxy-desktop 失败，确定docker pull osrf/ros:foxy-desktop是否成功，若失败则配置docker网络。
• 若自己在宿主机上实验 docker pull osrf/ros:foxy-desktop 可以成功，则在 docker pull osrf/ros:foxy-desktop 成功后，构建时使用下面命令：docker build --network=host --pull=false -t colrio .
• 构建后检查：docker images
• 运行容器：实现可视化，挂载宿主机上的数据集目录，路径不唯一，根据自己的实际目录修改，或者从宿主机上复制数据集（太大的数据集不推荐）

xhost +local:root
docker run -it --gpus all --network host -e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix -v /home/sd10/Downloads/S3E/:/home/dataset --name colrio colrio

---