SLAM은 IMU 센서 유무에 따라 테스트해보았다.

두 경우 모두 급격한 회전이나 이동에 오차가 발생하는 것을 확인할 수 있다 (이 문제는 성능 낮은 저가 임베디드 보드 사용과 관련 있을 수 있다).

가능한 SLAM이 예측 가능한 방향으로 부드럽게 천천히 이동하는 것이 제일 품질이 좋다.

단, IMU 센서가 없는 경우 급격한 회전이나 이동시 오차가 상대적으로 더 많았으며, 정합 대상을 잃어버리면서 스캐닝되는 문제가 좀 더 빈번히 발생한다.

이렇게 정합이 안되고 발산하는 문제는 SLAM 기술의 일반적인 문제로 알려져 있다 (상용 SLAM 측량 기술도 동일한 문제가 발생한다).

그러므로 현재 기술은 아직 SLAM을 이용해 스캔하는 방법이 결과물에 나쁜 영향을 줄 수도 있다. 

참고로, VLP-16의 스캔 수직범위는 크지 않아, 360도 스캔을 위해서는 스캐너를 회전시켜야 한다.

아래 영상은 LiDAR를 회전시켜 360도로 점군을 획득해 맵을 생성하는 예이다. 

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

https://github.com/RobustFieldAutonomyLab/LeGO-LOAM

LeGO(lightweight and ground optimized lidar odometry and mapping) LOAM은 LOAM에 비해 계산 시간은 줄이고, 루프백(LOOP BACK)을 지원하여 SLAM 안전성을 높인 방법이다. 

Levenberg–Marquardt algorithm 을 이용해 해 계산 방법을 개선한다. 

참고로 LM 알고리즘은 특정 함수에 적합한 커브를 피팅(curve fitting)할 때도 많이 사용된다. 

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 

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

LeGO LOAM 

LOAM이나 LeGO이외에도 여러가지 SLAM방식이 존재한다. 

그러므로, 응용 목적에 따라 적합한 것을 사용해야 한다. 

HDL GRAPH SLAM 

https://github.com/koide3/hdl_graph_slam

HDL 그래프 슬램은 3차원 라이다를 이용한 슬램 기술 중 하나로 NDT(Normal Distributions Transformation) 스캔 매칭 및 루프 검출 방식을 알고리즘으로 사용한다. 

이 방식은 다음 그림과 같이 odometry 예측, RANSAC을 이용한 바닥 검출 등을 이용해 스캔 데이터 매칭을 수행한다.

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)에 구현되어 있는 알고리즘이다. 

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

- PCL 버전 NDT
- 공간 인덱싱 적용 버전 NDT (https://github.com/koide3/ndt_omp) 

참고로, 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) 오류가 발생한다.

이 오류는 메모리가 부족하여 발생하므로, 스왑을 추가하여 문제를 해결할 수 있다(링크 참고).

ODROID보다 TX2기반에서 안정적인 SLAM 처리가 가능하다. 

ROS 특성상 컴퓨팅 처리 속도가 늦으면, 메시지 동기화가 문제가 되며, 이로 인해 SLAM 맵 생성이 제때 처리되지 않는 문제가 발생할 수 있다. 

참고로 TX2는 보통 개발 보드와 함께 조립되어 있는 데, 다음과 같은 캐리어 보드를 이용하면, 손바닥 보다 작은 크기로 패키징 가능하다. 

 캐리어 보드 

NVIDIA TX2와 개발보드 

개요

LIO SAM은 LOAM개발자가 기존 방법을 개선해 개발한 슬램 기술이다. 

LIO-SAM은 라이다 관성 주행 거리 측정법을 개발해, 루프 클로징을 포함한 다양한 상대 및 절대 포인트 측정을 여러 소스에서 통합한다.

관성 측정 장치 (IMU) 통합에서 추정된 모션은 포인트 클라우드의 왜곡을 제거하고 라이다 주행 거리 측정 최적화를 위한 초기 추측 데이터를 생성한다.

라이다 주행 거리 측정 솔루션은 IMU의 편향을 추정하는 데 사용된다.

이 결과로 다음과 같이 기존 LOAM보다 좋은 결과를 얻을 수 있다.

시스템 아키텍처는 다음과 같다. 

이 기술은 다음과 같이 IMU, LiDAR가 미리 준비되어 있어야 한다.

소스 빌드 및 실행

다음과 같이 실행해 소스코드를 빌드한다.

실행은 다음과 같다. 

roslaunch lio_sam run.launch

미리 준비된 데이터로 스캔 데이터를 시뮬레이션할 수 있다.

LIO SAM은 LOAM과 유사한 방식(참고)으로 개발되었다. 특징점을 계산하기 위해, GSTAM 패키지를 사용한다.

-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- 

  https://sites.google.com/site/bimprinciple/

  http://daddynkidsmakers.blogspot.kr/