[SLAM] Ouster 데이터셋으로 LIO-SAM 구현하기

환경: Ubuntu 22.04

ROS2 humble

 

1. driver 설치

본인이 사용할 센서에 맞게 드라이버를 설치해야 한다. 이 논문은 Ouster lidar와 Xsens IMU, SBG Systems IMU를 기반으로 테스트되었기 때문에 드라이버를 설치해준다.

 

Ouster Driver

https://github.com/ros-drivers/ros2_ouster_drivers

GitHub - ros-drivers/ros2_ouster_drivers: ROS2 Drivers for the Ouster OS-0, OS-1, and OS-2 Lidars

 

cd ~/ros2_ws/src 
git clone https://github.com/ros-drivers/ros2_ouster_drivers
colcon build

 

 

Xsens Driver 설치

https://github.com/bluespace-ai/bluespace_ai_xsens_ros_mti_driver/tree/ros2_0_galactic

GitHub - bluespace-ai/bluespace_ai_xsens_ros_mti_driver: xsens ros2 driver

 

cd ~/ros2_ws/src
git clone https://github.com/bluespace-ai/bluespace_ai_xsens_ros_mti_driver/tree/ros2_0_galactic
colcon build

 

 

SBG-Systems Driver

https://github.com/SBG-Systems/sbg_ros2_driver

GitHub - SBG-Systems/sbg_ros2_driver: ROS 2 driver for SBG Systems IMU/AHRS/INS units such as ELLIPSE or QUANTA.

 

sudo apt-get install ros-galactic-sbg-driver
cd ~/ros2_ws/src
git clone https://github.com/SBG-Systems/sbg_ros2_driver.git
cd sbg_ros2_driver
rosdep update
rosdep install --from-path .
cd ../..
colcon build
source install/setup.bash

 

 

2. Dependencies

ROS2

sudo apt install ros-humble-perception-pcl \
  	   ros-humble-pcl-msgs \
  	   ros-humble-vision-opencv \
  	   ros-humble-xacro

 

gtsam

sudo add-apt-repository ppa:borglab/gtsam-release-4.1
sudo apt install libgtsam-dev libgtsam-unstable-dev

 

 

3. LIO-SAM 빌드

cd ~/ros2_ws/src
git clone https://github.com/TixiaoShan/LIO-SAM.git
cd LIO-SAM
git checkout ros2
cd ..
colcon build

 

 

4. lidar data 주의점

imageProjection.cpp에 사용될 포인트 클라우드를 준비할 때 주의할 점이 있다. LIO-SAM은 포인트 클라우드 deskew(비뚤어진 이미지를 고쳐줌)하기 위해서 IMU 센서 데이터를 사용한다. 그래서 한 scan에서의 상대적인 포인트 시간 정보를 알아야 한다. imageProjection.cpp의 deskewPoint() 함수에서 시간 데이터를 사용해서 스캔 시작점 대비 포인트의 변환 정보를 계산한다. 참고로 Velodyne의 최신 ROS driver는 정보를 제공하고 있어 따로 수정할 필요가 없다. 다른 센서의 경우에는 time channel을 수정해주어야 한다.

 

LIO-SAM은 포인트를 행렬 형태로 저장하는데 포인트 ring number이 필요하다. 마찬가지로 Velodyne driver는 자동으로 제공하는데, 다른 센서의 경우, ring number를 확인해 알맞게 수정해야 한다.

 

본인은 ros1 환경에서 Ouster사의 OS1-64 센서 + Xsens IMU(9 axis)로 측정된 bag 파일을 사용했다. 주의할 점은 Ouster 내부에 부착된 IMU는 6축이기 때문에 LIO-SAM을 사용할 수 없다. 9축 IMU 센서를 추가로 설치하거나 여기에서 데이터를 받아 사용할 수 있다.(Ouster 데이터는 rooftop_ouster_dataset.bag)

 

5. IMU data 주의점

 

6. 센서에 맞게 코드 수정

params.yaml

가지고 있는 bag 파일을 실행시켜 어떤 토픽을 발행하는지 확인하고, 토픽에 맞게 params.yaml을 수정할 거다.

ros2 topic list

 

내가 가진 센서는 /os1_points와 /imu/data_raw 토픽을 발행하고 있어 각각 바꾸어 주었다.

그리고 # Sensor Setting 부분에서 sensor 명, N_SCAN, Horizon SCAN을 변경해야 한다. 본인은 os1-64이기 때문에 제품 상세 설명을 보고 변경했다. 구글링하면 제품 상세 스펙이나 다른 알고리즘에 사람들이 os1-64 스펙에 맞게 적용해놓은 걸 찾을 수 있다.

# params.yaml

# Topics
    pointCloudTopic: "/os1_points"                   # Point cloud datas
    imuTopic: "/imu/data_raw"                        # IMU data
    # odomTopic: "/os1_imu"                    # IMU pre-preintegration odometry, same frequency as IMU
    gpsTopic: "odometry/gpsz"                    # GPS odometry topic from navsat, see module_navsat.launch file

    # Frames
    lidarFrame: "lidar_link"
    baselinkFrame: "base_link"
    odometryFrame: "odom"
    mapFrame: "map"

    # GPS Settings
    useImuHeadingInitialization: false           # if using GPS data, set to "true"
    useGpsElevation: false                       # if GPS elevation is bad, set to "false"
    gpsCovThreshold: 2.0                         # m^2, threshold for using GPS data
    poseCovThreshold: 25.0                       # m^2, threshold for using GPS data

    # Export settings
    savePCD: false                               # https://github.com/TixiaoShan/LIO-SAM/issues/3
    savePCDDirectory: "/Downloads/LOAM/"         # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation

    # Sensor Settings
    sensor: ouster                               # lidar sensor type, either 'velodyne', 'ouster' or 'livox'
    N_SCAN: 64                                   # number of lidar channels (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6)
    Horizon_SCAN: 1024                            # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000)
    downsampleRate: 1                            # default: 1. Downsample your data if too many
    # points. i.e., 16 = 64 / 4, 16 = 16 / 1
    lidarMinRange: 1.0                           # default: 1.0, minimum lidar range to be used
    lidarMaxRange: 1000.0

 

imageProjection.cpp

앞서 말했던 것처럼 Velodyne이 아닌 이상 센서에 맞게 time과 ring을 변경해야 한다. 다행이게도 Ouster 구조체가 작성되어 있어 PintXYZIRT를 Velodyne에서 Ouster 구조체로 변경해주면 된다.

// Use the Velodyne point format as a common representation
using PointXYZIRT = OusterPointXYZIRT; // changed into Ouster

 

구조체가 변경되었기 때문에, PointXYZIRT로 선언된 변수의 멤버도 바꾸어 줘야 한다.

catchCloudPoint() 함수 내부에 PointXYZIRT로 선언된 dst가 time으로 설정되어 있는데, t로 바꿔준다.

dst.t = src.t * 1e-9f;

 

 

찾아보니 laserCloudIn으로 선언된 것들은 모두 time이라는 멤버를 참조하고 있어 Ctrl+F로 laserCloudIn을 검색해 time으로 참조한 경우 모조리 t로 바꾸어 주면 된다.

# 266번째 줄
timeScanEnd = timeScanCur + laserCloudIn->points.back().t;

# 612번째 줄
thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].t);

 

7. 실행

ros2 launch lio_sam run.launch.py

 

ros2 bag play your-bag