matlab SLAM(Simultaneous Localization And Mapping) example(2)

SLAM Algorithm

저번 포스팅에 이어서 실제 SLAM이 어떻게 수행되는지 바로 알아보자.

for i=1:length(pClouds)
    % Read point clouds in sequence
    pc = pClouds{i};

반복문의 시작. 매회 point clouds 1개의 cell을 로드한다. 즉 simulation이 아니라 실제 로봇이었다면 1회 scan을 진행하는 것.

% Remove invalid points outside the max range and unnecessary points
    ind = (-maxLidarRange < pc(:,1) & pc(:,1) < maxLidarRange ...
        & -maxLidarRange  < pc(:,2) & pc(:,2) < maxLidarRange ...
        & (abs(pc(:,2))>abs(0.5*pc(:,1)) | pc(:,1)>0));
    
    
    pcl = pointCloud(pc(ind,:));

주석을 보면 Lidar sensor의 range 밖의 point등과, 불필요한 point들을 필터링한다고 되어있다. matlab을 처음 접한 나로선 저 :(콜론)을 포함한 코드가 뭘 뜻하는지 찾아봐야한다. 콜론은 matlab에서 가장 유용한 연산자 중 하나로, 벡터나 첨자 배열을 만드는 데 쓴다고 한다. 이런 문법같은 것은 사실 코드를 여러번 작성하면서 익숙해지는 것이 답인데 일단 matlab 공부하는 것이 아니니 코드 뜻만 알아보도록 하겠다. pc(:,1) 은 행렬 A의 1번째 열을 뜻한다. 그런데 지금보니까 matlab은 C/C++에서와 같이 인덱싱을 0부터 하지 않는듯 하다. 그리고 지금보니 Z축이 땅에서 수직이다. 코드를 해석보면 이렇게 그릴 수 있지 않을까.

이 코드는 시뮬레이션 코드이기 때문에 Lidar sensor 범위를 조절하고 편의를 위해 정사각형 영역을 사용하고 있지만 사실은 오른쪽과같이 원과 같은 영역을 탐지할 것이다. 다만 왜 2번째 행이 1번째 행의 두배보다 커야하는걸까? 나는 단순히 1,2,3 열이 x,y,z 좌표를 의미한다고 생각했는데 좀더 말이 되는쪽으로 생각해보니 Camera Coordinate 상의 좌표인듯 하다. 하지만 그래도 2행이 1행보다 2배이상 커야 하는 이유는 모르겠다. 보류하고 넘어가자. 결국 pcl 에는 1회 filtered scan point cloud가 배열로 저장될 것이다.

% Remove the points on the ground
    [~, ~, outliers] = ...
        pcfitplane(pcl, maxDistance,referenceVector,maxAngularDistance);
    pcl_wogrd = select(pcl,outliers,'OutputSize','full');
    
    % Remove the points on the ceiling
    [~, ~, outliers] = ...
        pcfitplane(pcl_wogrd,0.2,referenceVector,maxAngularDistance);
    pcl_wogrd = select(pcl_wogrd,outliers,'OutputSize','full');

솔직히 다른 언어들을 접하는 것에 대해 크게 생각해본적 없는데 생각보다 다르니 찾아봐야하는 것이 많다. ~ 연산자는 논리적 not 이나 출력 제한을 의미한다는데 일단 처음 pcfitplane은 outlier[변수]에 outlier index를 저장한다. 그리고 논리적으로는 pcl_wogrd(설마 pcl without ground??)에 기존 pcl에서 outlier들을 제거하는게 맞는거같다. select함수는