벨로다인 LiDAR로 SLAM 만들기

이 글은 LiDAR(Light Detection and Ranging) 기반으로 SLAM 하는 방법을 간략히 설명한다. 슬램은 임의의 위치에서 상대적 거리를 측정할 수 있는 센서를 이용해 실시간으로 지도를 생성하는 기술이다. 이 기술은 무인자율차 등 실시간으로 2, 3차원 지도를 생성해야할 때 사용한다.

실시간 SLAM(Simultaneous localization and mapping) 기반 3D 맵(LOAM)

모바일 SLAM 결과(한국건설기술연구원 일산 건물)

이 글에서 SLAM을 활용해 보기 위해서 저가의 VLP16 벨로다인 센서를 사용한다. 참고로, 이 방법은 우분투 ROS 환경에서 실행된다. 여기서 사용하는 환경은 ROS(Robot Operating System) melodic 버전을 사용하였으며, NVIDIA TX2, 오드로이드 우분투 18.04 기반으로 TEST 하였다(SLAM은 많은 계산시간이 필요하므로, NVIDIA TX와 같이 가능한 성능 좋은 컴퓨터를 사용하는 것이 좋다. 이 경우에는 장치 소형화를 위해 부득이하게 오드로이드를 사용했다). 우분투, 벨로다인 LiDAR 드라이버, ROS, RViz 설치 방법은 다음 링크를 참고하길 바란다.

• 오드로이드 우분투 기반 벨로다인 라이다와 ROS 설치, 연결 및 설정

SLAM 동작 방식
SLAM은 다음과 같은 단계로 스캔된 포인트 클라우드 장면(Scene)을 정합(registration)한다.

1. 각 장면 별 특징(feature) 계산
보통 각 장면에서 스캔된 데이터 PCD에는 불변인 특징점이 존재한다. 예를 들어, 특정 곡률을 가진 포인트, 평면 포인트 클라우드 등이 그것이다. 포인트 클라우드 스캔 특징이 있다면 이를 특징으로 사용하면 된다.

2. 각 장면 별 유사 특징 매칭
각 장면 별 유사한 특징을 서로 매칭한다. 서로 같은 최소 3개 특징을 찾아 매칭할 수 있으면 다음과 같은 매칭 좌표변환행렬 MCT를 만들 수 있다. 각 장면별 획득된 MCT는 Odometry 데이타를 포함한다.

3. 장면 정합
각 장면의 매칭 좌표변환행렬을 얻으면, 이를 장면의 포인트 클라우드에 적용한다. 이로써 한 좌표계로 각 장면의 데이터가 정합된다. 이를 실시간으로 처리해야 하기 때문에 계산 성능이 좋은 장치를 사용해야 한다.

다음은 SLAM의 대표적 알고리즘 중 하나인 ICP를 이용한 실내 공간 스캔 데이터 정합 알고리즘이다.

ICP 기반 정합 행렬 계산 알고리즘(Yun-Ting Wang etc, 2018)

이 글에서는 SLAM의 원리를 깊게 다루지는 않는다. SLAM의 이론적 설명은 다음 링크를 참고한다.

• What is SLAM
• 3DTK - The 3D SLAM 6D
• Large-Scale 3D Point Cloud Processing Tutorial 2013

SLAM은 대부분 ROS 패키지를 사용하므로, 이전 글 참고해 미리 설치해 놓어야 한다. ROS에 대한 상세 설명은 아래 글을 참고 하길 바란다.

• ROS 개념 및 명령어

LOAM SLAM
우선 SLAM 에서 잘 알려진 LOAM(Laser Odometry and Mapping) 방식 SLAM을 만들어보자. 다음과 같이 github에서 소스코드를 받고, 순서대로 빌드해 본다(링크1, 링크2 참고).

$ cd ~/catkin_ws/src/
$ git clone https://github.com/laboshinl/loam_velodyne.git
$ cd ~/catkin_ws
$ catkin_make -DCMAKE_BUILD_TYPE=Release
$ source ~/catkin_ws/devel/setup.bash

참고로, 오드로이드, 라즈베리파이 같이 램 용량이 적은 컴퓨터에서는 컴파일 에러가 발생한다. 이 경우, 부록에 설명된 방법대로 스왑 용량을 설정해 컴파일 빌드하면 된다(컴파일이 몇시간 걸릴 수 있다).

이제 roscore를 실행하고, 다음 명령을 실행한다.
roslaunch loam_velodyne loam_velodyne.launch

실행된 ROS node 토픽(rqt_graph)

LOAM SLAM을 이용해 회사 사무실을 LiDAR로 스캔해 본 결과는 다음과 같다. 점군 밀도가 밀집되어 있는 벽체가 진하게 표시되어 구분되는 것을 확인할 수 있다. 

SLAM 스캔 결과(건설연 본관 2동 3층)

점군이 모여있는 부분은 회사에 설치된 파티션이고, 그 외에 점들이 흩어져 있는 부분들은 천장 및 바닥이다. 파티션의 경계선은 뚜렷하게 표시된 맵이 생성되지만, 아직 좌표계, 캘리브레이션, IMU 등을 제대로 설정하지 않은 상황이라, SLAM 맵 정확도가 높지는 않아 보인다. 또한, 창쪽 유리가 반사되어 노이즈가 생기는 것을 확인할 수 있다. 

SLAM 점군과 측량장비(GLM100C)와 측정값 간의 차이는 높이 2미터에서 1-0.5미터정도이다. 정확도는 좀 더 다양한 방법으로 확인할 필요가 있어보인다.

1차 SLAM 테스트

2차 SLAM 테스트

3차 SLAM 테스트(ODROID)

4차 SLAM 테스트(NVIDIA TX2로 테스트. IMU연결안함. SLAM 맵 깨지는 현상이 덜함)

SLAM은 IMU 센서 유무에 따라 테스트해보았다. 두 경우 모두 급격한 회전이나 이동에 오차가 발생하는 것을 확인할 수 있다(이 문제는 성능 낮은 저가 임베디드 보드 사용과 관련있을 수 있다). 가능한 SLAM이 예측 가능한 방향으로 부드럽게 천천히 이동하는 것이 제일 품질이 좋다. 단, IMU 센서가 없는 경우 급격한 회전이나 이동시 오차가 상대적으로 더 많았으며, 정합 대상을 잃어버리면서 스캐닝되는 문제가 좀 더 빈번히 발생한다. 이렇게 정합이 안되고 발산하는 문제는 SLAM 기술의 일반적인 문제로 알려져 있다(상용 SLAM 측량 기술도 동일한 문제가 발생한다). 그러므로 현재 기술은 아직 SLAM을 이용해 스캔하는 방법이 결과물에 나쁜 영향을 줄 수도 있다.

참고로, VLP16의 스캔 수직범위는 크지 않아, 360도 스캔을 위해서는 스캐너를 회전시켜야 한다. 아래 영상은 LiDAR를 회전시켜 360도로 점군을 획득해 맵을 생성하는 예이다.

LOAM SLAM

SLAM 결과를 저장하려면 다음과 같이 토픽명을 지정하고 ROSBAG 명령어로 토픽 데이터를 저장하면 된다.
rosbag record -o out /laser_cloud_surround

만약, PCD 포맷으로 변환하려면, 아래 명령을 이용해 [input bag]에 저장된 .bag파일명을 지정해 준다.
rosrun pcl_ros bag_to_pcd [input bag] /laser_cloud_surround pcd

생성된 pcd파일은 다음과 같이 CloudCompare 프로그램으로 확인할 수 있다.

저장된 SLAM 정합 결과(CloudCompare)

참고로, 다음 명령을 이용해 미리 획득된 데이터를 플레이하거나 읽을 수 있다.

rosbag play ~/Downloads/velodyne.bag 

roslaunch velodyne_pointcloud VLP16_points.launch pcap:="$HOME/Downloads/velodyne.pcap"

LOAM동작을 이해하기 위해 소스코드를 UML로 역설계한다.

LOAM SLAM UML

아울로 ROS NODE간 토픽 그래프를 RQT_GRAPH 명령을 이용해 확인해 본다.

ROS NODE 토픽 그래프

구조와 소스코드를 분석해 보면 다음과 같은 순서로 SLAM데이터를 계산하고 있는 것을 알 수 있다.
1. velodyne node: 스캔 후 point cloud 데이터 획득
2. multi scan registration node: 스캔된 데이터에서 곡률 기반 특징점(모서리, 평면) 생성
3. laser odometry: 이전 스캔 데이터에서 생성된 특징점을 기반으로 현재 스캔 데이터의 매특징점과 비교해 주행괘적(odometry)을 계산
4. laser mapping: 주행괘적을 이용해 스캔 데이터를 보정해 정합함. 만약, IMU 센서가 있는 경우, /imu/data에서 얻은 데이터를 이용해 데이터 보정함

LOAM은 특징점 계산에 많은 계산 시간이 필요하다. 이런 이유로 원칙적으로 NVIDIA TX2 이상의 임베디드 보드가 필요하다. LeGO LOAM은 이 계산 시간을 좀 더 줄인것이다.
좀 더 자세한 내용은 관련 논문을 참고하길 바란다.

LeGO LOAM
LeGO(lightweight and ground optimized lidar odometry and mapping) LOAM은 LOAM에 비해 계산 시간은 줄이고, 루프백(LOOP BACK)을 지원하여 SLAM 안전성을 높인 방법이다. Levenberg–Marquardt algorithm 을 이용해 해 계산 방법을 개선한다. 참고로 LM 알고리즘은 특정 함수에 적합한 커브를 피팅(curve fitting)할 때도 많이 사용된다.
LeGO LOAM 링크를 방문해, 다음과 같이 소스를 다운로드후 빌드를 준비한다. 참고로, LeGO LOAM을 빌드하기 전에 GTSAM(Georgia Tech Smoothing and Mapping library) 패키지를 미리 설치해 놓아야 한다.

wget -O ~/Downloads/gtsam.zip https://github.com/borglab/gtsam/archive/4.0.0-alpha2.zip
cd ~/Downloads/ && unzip gtsam.zip -d ~/Downloads/
cd ~/Downloads/gtsam-4.0.0-alpha2/
mkdir build && cd build
cmake ..
sudo make install

다음과 같이 소스를 빌드한다.
cd ~/catkin_ws/src
git clone https://github.com/RobustFieldAutonomyLab/LeGO-LOAM.git
cd ..
catkin_make -j1

다음과 같이 ROS 패키지 실행해 보았다.
roslaunch lego_loam run.launch

참고로 lego 패키지의 노드 중 일부는 ROS의 시뮬레이션 time 을 이용한다. 이 기능은 ros::time 패키지에 있는 데, 이를 사용하는 노드의 ROS loop가 동작되지 않는 경우가 발생할 수 있다. 이 상황에는 실제 스캔된 포인트 클라우드 및 처리된 결과물을 rviz에서 확인할 수 없다. 이때는 해당 노드의 main() 함수 내에서 ROS loop가 실행되기 전에 다음과 같은 시간 초기화 함수를 호출해 빌드한 후 실행해 보면 잘 동작될 것이다. 

ros::time::Init();

일부 TF(Transform coordinate Frame) 토픽이 동작되지 않는 문제가 있을 수 있다. 이 경우 센서 데이터 좌표계가 설정되지 않아 rviz에 스캔 데이터가 표시되지 않고 에러 메시지가 발생한다. 이 경우, lego_loam 의 run.launch 파일 내 TF 토픽 발생하는 노드를 터미널에서 강제로 실행하면 잘 동작된다. 

문제가 없을 경우 LeGO SLAM 실행된 결과는 다음과 같다.

LeGO SLAM 실행 결과

github에 링크되어 있는 pcap파일로 실행하면 미리 스캔한 데이터로 다음과 같은 데모를 확인할 수 있다고 한다. 만약, 벨로다인에서 저장된 ROS bag 파일이 있다면, 다음 명령을 이용해 재생하여 rviz에 데이터를 표시할 수 있다. 참고로, LeGO 웹사이트에서 샘플 데이터 bag 파일을 다운로드 할 수 있다.

rosbag play *.bag --clock --topic /velodyne_points /imu/data

LeGO LOAM

LOAM이나 LeGO이외에도 여러가지 SLAM방식이 존재한다. 그러므로, 응용 목적에 따라 적합한 것을 사용해야 한다.

HDL graph SLAM

HDL GRAPH SLAM

HDL 그래프 스램은 3차원 라이다를 이용한 슬램 기술 중 하나로 NDT(Normal Distributions Transformation) 스캔 매칭 및 루프 검출 방식을 알고리즘으로 사용한다. 이 방식은 다음 그림과 같이 odometry 예측, RANSAC을 이용한 바닥 검출 등을 이용해 스캔 데이터 매칭을 수행한다.

HDL GRAPH SLAM 프로세스

HDL 슬램을 수행하려면, 다음 링크를 방문해 소스코드를 다운로드 받고, 빌드해야 한다.

• HDL GRAPH SLAM 깃허브

실행을 위해 필요한 패키지는 다음과 같이 설치한다.

# for indigo
sudo apt-get install ros-indigo-geodesy ros-indigo-pcl_ros ros-indigo-nmea-msgs
# for kinetic
sudo apt-get install ros-kinetic-geodesy ros-kinetic-pcl_ros ros-kinetic-nmea-msgs ros-kinetic-libg2o
# for melodic
sudo apt-get install ros-melodic-geodesy ros-melodic-pcl-ros ros-melodic-nmea-msgs ros-melodic-libg2o

sudo pip install ProgressBar2

컴파일은 ROS cakin으로 빌드한다. 만약 intel 계열 보드가 아니면 OpenMP 사용하는 소스코드에서 에러가 발생한다. ARM계열 보드의 경우, openmp 패키지를 설치하고, 에러 나는 소스 코드를 일부 수정해야 한다.

관련 에러를 수정한 후 벨로다인 노드와 슬램을 실행다면 다음과 같은 결과를 확인할 수 있다.

실내 SLAM 결과

깃허브에 포함된 예제를 보면, 다음과 같은 실행 결과를 확인할 수 있다.

HDL GRAPH SLAM 결과

NDT(Normal Distributions Transformation) SLAM
NDT는 SLAM에서 사용하는 기술 중 하나로 PCL(point cloud library)에 구현되어 있는 알고리즘이다.

PCL NDT 지원 클래스

이 방법은 이동하는 장치에서 스캔한 점군 PCD1, PCD2가 있을 경우, 두 점군이 겹칠때 각 점들의 오차를 정규분포로 계산해 최소화하는 알고리즘으로 정합을 한다.
이 방법을 실시간 SLAM에 직접 사용할 때는 계산 성능에 문제가 있어, OCTREE 등 공간 인덱싱 기법을 사용해 속도를 개선해 사용하는 기법을 적용한다.
다음은 PCL 및 공간 인덱싱을 사용한 버전의 소스이다.

• PCL 버전 NDT
• 공간 인덱싱 적용 버전 NDT

이 중 공간 인덱싱 적용된 NDT 코드를 다운로드받고 ROS CATKIN으로 빌드해 보았다. 그리고 다음과 같이 실행해 보았다(각 폴더는 본인 환경에 맞게 변경하길 바란다).

rosrun ndt_omp align /home/nvidia/catkin_ws/src/ndt_omp/data/251370668.pcd /home/nvidia/catkin_ws/src/ndt_omp/data/251371071.pcd

그럼 다음과 같이 두개의 점군이 정합된 결과를 확인할 수 있다.

NDT 결과

참고로, NDT는 GICP(Generalized Iterative Closest Point) 방식의 하나이다.

GICP 결과(좌: 정합되지 않은 두개 점군. 우: 정합된 점군)(PCL)

부록: 오드로이드 cc1plus internal compile 에러 솔류션
오드로이드는 내부 램 용량이 작은 임베디드 컴퓨터이기 때문에 컴파일 시 cc1plus internal compile error 가 발생될 수 있다. 이 경우, 터미널에서 다음과 같이 설정한다.

dd if=/dev/zero of=/swapfile bs=1024 count=4194304
chown root.root /swapfile
chmod 0600 /swapfile
mkswap /swapfile
swapon /swapfile

설정된 스왑 디스크 크기를 확인해 본다.
free -m

이렇게 해도 안된다면, 컴파일 시 make -j 옵션 대신 make 를 사용한다(관련 링크).

참고로, 임베디드 장치는 메모리 크기가 2~4G이며 스왑은 구성되어 있지 않다. 이 경우, 컴파일 프로세스에서 g++: internal compiler error: Killed (program cc1plus) 오류가 발생한다. 이 오류는 메모리가 부족하여 발생하므로, 스왑을 추가하여 문제를 해결할 수 있다(링크 참고).

참고: NVIDIA TX2기반 테스트

ODROID보다 TX2기반에서 안정적인 SLAM 처리가 가능하다. ROS 특성상 컴퓨팅 처리 속도가 늦으면, 메시지 동기화가 문제가 되며, 이로 인해 SLAM 맵 생성이 제때 처리되지 않는 문제가 발생할 수 있다. 참고로 TX2는 보통 개발 보드와 함께 조립되어 있는 데, 다음과 같은 캐리어 보드를 이용하면, 손바닥 보다 작은 크기로 패키징 가능하다.

 캐리어 보드

NVIDIA TX2와 개발보드

레퍼런스

• Yun-Ting Wang etc, 2018, A Single LiDAR-Based Feature Fusion Indoor Localization Algorithm, Sensors
• Velodyne ROS SLAM
• Ji Zhang, 2013, LOAM SLAM, Carnegie Mellon University(paper)
• LOAM velodyne, ROS.org
• Ji Zhang, 2013, NDT omp, Carnegie Mellon University
• ROS, ROS SLAM. GMapping
• Sebastian Hening, 2017, 3D LiDAR SLAM Integration with GPS/INS for UAVs in Urban GPS-Degraded Environments, ICP based SLAM
• Tixiao Shan, Brendan Englot, 2018, lightweight and ground optimized lidar odometry and mapping, LeGO LOAM
• Kenji Koide etc, 2019, A Portable 3D LIDAR-based System for Long-term and Wide-area People Behavior Measurement, Advanced Robotic Systems, HDL graph SLAM
• B(erkeley) L(ocalization) A(nd) M(apping), 2016, BLAM SLAM
• Christoph Schütte, 2019, CARTOGRAPHER SLAM, google.com
• Raul Mur-Artal, 2017, ORB-SLAM2
• VeloView, 2017, VeloView: LiDAR SLAM capabilities with LOAM
• Pierre Guilbert, 2019, Point-cloud processing using VeloView: Automatic LiDAR-IMU Calibration and Object Recognition, kitware
• Rihards Gailums, 2016, HACKATHON: Journey to creating Drivenet Maps - Open Data real-time road maps for Autonomous Driving from 3D LiDAR point clouds
• Surfel-based Mapping using 3D Laser Range Data
• Advanced implementation of LOAM
• NDT based LiDAR SLAM 3D
• 3D Lidar Inertial Odometry and Mapping (LIO-mapping)
• SLAM-capable robot using the RoboPeak LIDAR sensor
• VeloSLAM
• SLAM system that employs 2D and 3D LIDAR measurements
• Ceros Solver, google

기타 임베디드 보드에서 소스코드 빌드하며 에러가 발생할 때 구글링해 도움받은 레퍼런스는 아래와 같다. 

• C++: INTERNAL COMPILER ERROR: KILLED (PROGRAM CC1PLUS)
• internal compile error: Killed (program cc1plus), Bolog(odroid cc1plus "c++: internal compiler error")
• ODROID-XU3/XU4
• OpenMP on Ubuntu
• cmake error with tx2(arm) #2
• How to add -fpermissive option to kdevelop?
• CMakeLists.txt 주요 명령과 변수 정리
• github
• Build Error (Jetson TX1 - ARM64): c++: error: unrecognized command line option '-msse' #479