matlab SLAM(Simultaneous Localization and Mapping) example(1)

한 6개월 전부터 SLAM에 관심이 생겨서 여기저기 유튜브강의와 Probabilistic Robotics 책을 통해 독핵을 해보았다.
뭔가 기본지식을 쌓아나가면서도 내가 제대로 이해한 것이 맞나 의문이 한켠에 남는 느낌을 지울 수가 없었다.
왜 why? 첫째로는 이쪽 분야를 처음 접했으니 여러 수식들과 개념들이 바로바로 이해가 안된다. 기본기 부족이랄까...
둘째로는 부족한 이해를 실습으로 보충하면 좋겠지만 일단 로봇이 없다. 그래서 인터넷이 돌아다니는 dataset을 활용해볼까 했지만 역시 기본기 부족으로 제대로 활용하기 어려웠다.
이렇게 처음으로 시도한 C++로 가장 기본적인 Kalman Filtering 구현이 실패해버렸다

MATLAB

SLAM에 대해 배우고 많은 실험을 해보고 싶기 때문에 뒤적거리다 보니 matlab에서 SLAM 을 simulation 할 수 있는 예제코드가 있었다.
나는 matlab을 써본적도 없고 문법도 모르지만 일단 학교 라이센스를 이용해 설치후 예제를 돌려본다.
이 포스팅 자체가 해당 예제에 대한 포스팅이 될 예정..
matlab과 SLAM 둘다 맨땅에 해딩하는 것이나 마찬가지기 때문에 험난한 여정이 예상되지만 어떻게든 되겠지라는 마인드로 시작한다.
하지만 그 만큼 포스팅 자체의 퀄리티는 일기장 수준일 듯 하다.

Perform SLAM Using 3-D Lidar Point Clouds

matlab slam을 검색하면 처음으로 나오는 예제이다.
일단 여기서 다루는 데이터부터 한번 이해해보자.

load pCloud.mat

맨 처음에 하는것이 바로 pCloud.mat이라는 데이터를 로드하는 것인데 이 데이터가 정확히 어떤 데이터를 담고 있는지 궁금하다.
실제 데이터 파일을 열게되면 다음과 같이 1x240 의 cell을 가진 행벡터로 보여진다.

그리고 그 한개의 cell에는 대량 50000x3 의 데이터가 존재하는데 로봇이 움직일때 lidar 센서로 들어온 좌표정보가 아닐까 추측한다(예제 페이지 설명보니 아마 맞는듯).

즉 240x50000개의 x,y,z 정보가 있는셈이다.

referenceVector = [0, 0, 1];    % normal vector
maxDistance = 0.5;              % for removing ceiling/ground
maxAngularDistance = 15;        %

% sampling (sampling ratio : 0.25)
% point cloud downsampled
randomSampleRatio = 0.25;

% voxel grid size in NDT(Normal Distribution Tranformation)
gridStep = 2.5;
distanceMovedThreshold = 0.3;

데이터 로드 후 이것저것 변수들을 초기화 한다. 일단 randomSampleRatio는 말 그대로 point cloud를 sampling 하는 비율이다. 모든 point cloud들을 전부 보기엔 굉장히 오래걸리니까 샘플링을 하는 듯 하다. point cloud registration의 accuracy도 올려준다는데 샘플링을 안하는게 제일 정확한 것이 아닌가??? 일단 넘어가보자. 또 gridStep이라는 변수가 있는데 내가 알기로는 point 하나하나를 이용하기 보다 여러 point들을 묶어 하나의 voxel로 만들어 이용하는 것이 효율적이라 들었다 이 gridStep이 voxelization에 관여하는 변수인듯 하다. Voxelization에 대해 자세하게 알기 위해선 Normal Distribution Transformation에 대해 알아봐야겠지만 이부분도 일단 스킵하자. 마지막으로 distanceMovedThreshold는 0.3 이상 움직인 후 scan을 수행한다는 뜻.

% Loop Closure Estimation Algorithm?
loopClosureSearchRadius = 3;

nScansPerSubmap = 3;
subMapThresh = 50;

annularRegionLimits = [-0.75,0.75];

rmseThreshold = 0.26;

loopClosureThreshold = 150;
optimizationInterval = 2;

Loop Closure가 뭔가 검색해봤다. Loop Closure(루프결합)은 정확한 지도를 작성함에 있어 중요한 역할을 한다. 루프결합이란 로봇이 지도상에 이미 등록한 위치를 다시 방문하였을 때, 현재 측정된 sensor data와 이미 작성된 map data 사이의 일치를 찾고 누적된 에러를 감소시켜 정확도를 향상하는 과정이다. 시간이 지날 수록 error가 축적되는 SLAM 알고리즘과 같은 문제에서 필수적으로 필요한 프로세스로 보인다. 여기까지 알아보고 다시 진행해보자 loopClosureSearchRadius는 동일 위치에 재방문했다는 decision을 내릴 때 현재 나의 추정위치와 지도상 위치의 거리 Threshold 역할을 하는 듯 하다. 나 자신의 이해를 돕기위해 그림을 그려보도록 하자.

로봇이 파란 곡선을 따라 이동했다고 생각해보자. 사실 저 파란 경로 역시 로봇이 자신이 이동했다고 '추정'하는 경로일 뿐이다. 현재 로봇이 파란지점에 도달했다고 해보자. 이때 저 노란 별을 관측한 데이터와 지도상 빨간점에서 관측한 데이터가 유사할 때 로봇은 '아 내가 사실은 아까 그 빨간 점까지 돌아왔구나' 라고 생각하며 경로와 지도를 수정할 수 있다.

그리고 갑자기 Submap 이란 것이 튀어나오는데 일단 정확히 파악한 것은 아니지만 이해한대로 적어본다. 로봇이 움직이면서 지도를 그리고 submap이라는 local information을 유지한다. loop closure decision을 할때 유지한 submap와 비교를 하는데 바로 직전 subMapThresh 개의 submap은 고려하지 않는다 라는 뜻인듯 하다.

어제에 이어 포스팅을 계속해보자. 일단 어제 코드를 보며 생각했던 것은 일단 SLAM 알고리즘 자체가 크게 3개로 이루어져있다.
1. Kalman filter SLAM
2. Particle filter SLAM
3. Graph SLAM
그런데 일단 이 셋중에 어떤 알고리즘이 이용되어있는지 감을 잡기 힘들다는 것. 코드를 계속 보면 감이올지 안올지 모르겠으나 일단 어제 코드에 이어서 계속 봐보도록 하자.

% 3D Posegraph object for storing estimated relative poses
% poseGraph3D object stores information for 3D pose containing nodes
% connected by edges
pGraph = poseGraph3D;

% Default serialized upper-right triangle of 6-by-6 Information Matrix
infoMat = [1,0,0,0,0,0,1,0,0,0,0,1,0,0,0,1,0,0,1,0,1];

% Number of closure edges added since last pose graph optimization and map
% refinement
numLoopClosuresSinceLastOptimization = 0; 
% True after pose graph optimization until the next scan
mapUpdated = false;
% Equals to 1 if the scan is accepted
scanAccepted = 0;

% 3D Occupancy grid object for creating and visualizing 3D map

mapResolution = 2; % cells per meter
omap = occupancyMap3D(mapResolution);

일단 실질적으로 SLAM에서 사용되는 것으로 보이는 poseGraph3D의 객체를 생성하고 information matrix 의 upper triangular matrix(상삼각행렬)을 초기화한다. 그리고 scanAccepted 라는 flag 변수가 있는 것을 보니 모든 scan이 accept되지 않는다는 것도 알 수 있다.

pcProcessed = cell(1,length(pClouds));
lidarScans2d = cell(1,length(pClouds)); 
submaps = cell(1,length(pClouds)/nScansPerSubmap);

pcsToView = cell(1,length(pClouds)); 

% Set to 1 to visualize created map and posegraph during build process
viewMap = 1; 
% Set to 1 to visualize processed point clouds during build process
viewPC = 0;

rng(0);

그 후 메모리를 미리 할당해놓는데 이때 눈에 띄는 것은 submaps 를 할당할 때 length(pClouds)/nScansPerSubmap 의 사이즈로 할당하는데 그럼 이전에 언급했던 240 x 50000 이라고 생각했던 x,y,z 좌표 정보는 사실 240번의 scan 정보에 지나지 않았던 것 아닐까? 대략 1번의 scan에 50000개의 좌표가 들어오나보다. 실제로 몇번의 scan이 add 되었나 count 하는 변수가 229까지 카운팅 된것을 보면 240개의 scan중 11개가 drop되고 229개가 accept 된거같다. 이따 확인해봐야겠다. 이제 실제로 루프를 돌며 실제로 SLAM 알고리즘이 어떻게 돌아가는지 봐야하니 이후내용은 다음 포스팅에 올린다.