이런식으로 어떤 point type에 대해 갑자기 applyFilter가 없다는 에러가 뜨면 CMakeLists.txt에서 target_link_libraries에서 pcl을 링크하는 부분에 -lpcl_filters를 추가해주면 해결됨.

import functools
self.cam1_enter_b.set_on_clicked(functools.partial(self.enter_img_idx, 1))

저 set_on_clicked 함수에 인자로 들어가는 콜백함수를 이렇게 input param과 바인딩해서 넣지 않고 아래처럼 input param과 함께 콜 하면 input param도 제대로 안들어가고 콜백이 작동을 안함

self.cam1_enter_b.set_on_clicked(self.enter_img_idx(1)) # 오답

참고: https://stackoverflow.com/questions/2347388/python-passing-a-function-with-parameters-as-parameter

$ sudo apt-get purge pulseaudio  
$ rm -rf ~/.config/pulse && sudo killall pulseaudio  
$ sudo apt-get install pulseaudio  
$ pulseaudio --start

The above to ensure pulse is working first. Then, this one, which is a bit weird that doesn't come by default with pulseaudio itself:

$ sudo apt-get install pulseaudio-module-bluetooth  
$ pactl load-module module-bluetooth-discover  

더도 말고 덜도말고 딱 이것만 하면 지금까지는 항상 해결되었다. reboot해줄 필요도 없음. 마지막줄 $ pactl load-module module-bluetooth-discover 했을때 module initialization failed인가 그런 에러 뜨면 reboot 해야 정상작동하더라..

📝 개요 8개의 open-source LiDAR/visual SLAM 시스템을 동일 환경에서 실험해 성능 비교 : LOAM, Lego LOAM, LIO SAM, HDL Graph SLAM, ORB SLAM3, Basalt VIO, SVO2 실내외에서 아래의 여러 컨디션에 따른 성능과 계산자원 비교를 수행하였다.

1) 센서의 마운트 포지션 2) 지형, 진동 3) 모션 (다양한 선속도와 각속도) 마지막으로 각 컨디션 별 최적의 시스템에 대한 제안으로 끝맺음.

🔎 결론

• 전반적으로 실외, dynamic object가 있는 환경에서는 라이다 기반이 우수함. 카메라보다 넓은 FOV를 이유로 듦.
• LiDAR SLAM의 경우,
  • LIO SAM w/ IMU가 베스트
  • LEGO LOAM은 경량이 중요하다면 굿, 대신 센서가 땅에 가까울 수록 잘 작동
  • LOAM도 경량 조건 시 괜찮은 선택지지만, 복잡한 환경에선 성능이 떨어짐
• visual SLAM의 경우,
  • ORB-SLAM3가 dynamic&복잡한 환경에서 잘 작동
  • Basalt VIO는 빠른 속도 변화에 상대적으로 강건한 모습을 보임
  • SVO2는 빠른 프론트엔드 덕에 빠른 모션에 강함

SLAM 알고리즘

배경지식

• SLAM 시스템은 보통 위 그림과 같이 frontend / backend 두 부분으로 구성됨.
• Frontend
  • visual SLAM 시각적 센서로부터 들어오는 매 프레임에서 포즈 추정에 쓰일 주요 정보를 추출 (data association) 카메라 이미지를 수신해 각 프레임에서의 키포인트를 추출하고, 이 키포인트들을 프레임간에 track한다. 로봇 포즈의 초기추정 또한 PnP 등의 방법으로 구할 수 있다. 프레임간에 공통되는 키포인트들을 랜드마크라고 한다. 이 랜드마크를 결정하는데는 여러 다른 제약이 사용될 수 있다.
  • LiDAR SLAM 라이다의 프론트엔드는 보통 다음 세 부분으로 구성된다. (1) 계산을 줄이기 위해 우선 다운샘플링 (2) 키포인트 추출- 보통 복셀의 smoothness 값을 많이 사용 (3) ICP와 같은 스캔 매치 알고리즘을 사용해 포즈 변환의 초기추정을 얻음 측위 에러를 줄이기 위해 루프 폐쇄를 포함하기도 한다 맵은 보통 랜드마크 위치에 희소한 정보만을 담고 있거나, 아니면 그냥 dense한 포인트클라우드일수도 있다.
• Backend
  • visual SLAM 프론트엔드의 초기추정 포즈와 랜드마크 association은 백엔드에서 로봇포즈의 MAP(maximum-a-poseteriori) 추정을 위해 사용된다. 카메라 이미지를 수신해 각 프레임에서의 키포인트를 추출하고, 이 키포인트들을 프레임간에 track한다. 로봇 포즈의 초기추정 또한 PnP 등의 방법으로 구할 수 있다. 프레임간에 공통되는 키포인트들을 랜드마크라고 한다. 이 랜드마크를 결정하는데는 여러 다른 제약이 사용될 수 있다.
  • LiDAR SLAM 점군 스캔 매칭을 통해 얻은 포즈간 transformation을 랜드마크 association 대신 사용한다.
    • 추가적으로 IMU integration를 사용하기도 함.
    • G2O나 GTSAM같은 오픈소스 라이브러리들을 통한 factor/pose graph 최적화 방식이 백엔드에 많이 쓰인다. 이는 여러 다양한 센서 모달리티를 통합하여 강인한 상태추정이 가능하도록 도와준다.

LiDAR SLAM 알고리즘

SOTA이며, 널리 쓰이고, ROS를 지원하는 오픈소스가 존재하는 아래 8가지 SLAM/odometry 시스템을 선정- LiDAR 기반 4개, 카메라 기반 4개.

💡 odometry와 full SLAM의 차이 odometry: frame-by-frame 추정 + 가끔 local window 최적화. full SLAM: 추가로 루프 디텍션이 있어 global consistency를 챙긴다. 따라서 full SLAM이 odometry systam보다 무겁다.

1) LOAM

• LIO SAM이나 LEGO LOAM 등 많은 시스템의 모태
• scan match 시 변형 ICP인 point-to-line, point-to-plane을 사용

  2) LEGO-LOAM

• LOAM과 매우 비슷한데 점군을 엣지와 평면 피쳐로 나눠서 처리함으로써 효율화.
• 평면 피쳐(땅)으로부터 높이, roll, pitch를 뽑고, 엣지로부터는 X,Y-coord와 yaw를 뽑는다.

  3) HDL graph SLAM:

• factor graph 기반, 여러개의 엣지 제약 정의 가능- GPS, IMU, 땅바닥 평면 검출, 루프 폐쇄 등..
• 노드들은 NDT나 ICP를 통한 포즈 초기추정 이후 그래프에 추가된다.

  4) LIO SAM

• full SLAM
• factor graph 기반, 라이다 오돔과 IMU integration을 tightly couple하여 상태추정

Visual SLAM 알고리즘

1) SVO2

• 프론트엔드에서 키포인트 검출기를 사용하는 다른 visual SLAM과 다르게, 얘는 direct visual slam과 유사한 프론트엔드를 가짐_(정확히는 direct slam 맞다. sparse direct slam) _
• local intensity gradient의 방향을 통해 모션을 추정하는 sparse model 기반 image alignment 방식.
• 다른 특징점 기반 방법보다 빠른 프론트엔드가 특장점 (track lost가 잘 안뜸)

  2) ORB-SLAM3

• 특징점 기반, SOTA 중 하나,
• 프론트엔드는 ORB feature detector/descriptor matching으로 이루어져있음
• DBOW2 포즈그래프 기반 루프폐쇄로 강건한 시스템 설계

3) Basalt VIO

• 특징점 베이스 VI 알고리즘
• 키프레임간 IMU reading의 비선형성을 복원(일반적인 IMU pre-integration 단계는 비선형성을 잃는다)
• 비선형 factor들을 따라 FAST 특징점과 KLT 트래커로 수행된 local BA 이후 factor 그래프에서 GBA 수행되어 또 최적화된다(문장이 왜이래..)

4) Kimera VIO

• semantic 3D mesh reconstruction을 위해 나온 라이브러리 내의 서브 모듈
• 프론트엔드는 Shi-Tomasi corner를 이용해 특징을 추출하고 KLT 트래커 이용해 트랙한다.
• five-point, three-point RANSAC을 이용해 키프레임간 상대포즈가 계산되고, pre-integrated IMU도 키프레임간에 계산된다. 랜드마크, 포즈 초기추정, IMU pre-integration은 factor graph 제약으로 사용되어 최종 상태 추정에 쓰인다.

실험

셋팅

• 라이다: Velodyne VLP 16
• 카메라: Realsense T265, ZED
• 4 core 8 thread Intel i7 7700HQ CPU
• calibrated using Kalibr toolbox for camera-imu calibration and LI-Calib for Lidar-imu calibration

메트릭

RMS of APE, RPE. w/ STD Umeyama alignment를 사용했다. (아니 alignment 왜하냐고..제발 그냥 initial pose에서 transform 구해서 그걸로 맞추라고.. 그래도 RPE는 alignment로부터 자유롭다.) evo_rpe 패키지 쓴 듯한 느낌이 강하게 듦.

실외 실험

실험1- 마운트 포지션 & 지형

센서의 마운트 위치 변경, 아스팔트/자갈에서 실험 수행

• LEGO LOAM은 센서가 땅에서 멀어질수록 정확도가 떨어졌음
• LIO SAM은 IMU와 같이 사용했을때 가장 좋은 APE를 달성했음. 근데 흔들리는경우 성능이 떨어짐.
• LOAM은 가장 넓은 FOV를 보장하는 side mount position에서 성능이 가장 좋았음.
• ORB SLAM3는 boom low에서 visual SLAM 중 가장 낮은 오차를 가짐.
• 대부분의 visual SLAM 알고리즘이 카메라가 땅에서 멀어질수록 성능이 내려가는건 주목할만한 부분.
• 사이드 마운트 포지션에서는, 카메라가 모션에 평행함에따라 키포인트들이 너무 빨리 움직이고 모션 블러가 컸다. 프레임간 데이터 어소시에이션이 거의 불가능했고, Basalt VIO나 SVO2는 트래킹에 실패했다. 하지만 orb slam3는 리로칼라이즈에 성공했다. 리얼센스 T265는 탑마운트 데이터셋에서는 하드웨어적으로 콜렉션이 안됐고(??뭔소리야) 그래서 basalt VIO랑 SVO2가 실패했다.

실험2- 선속도, 각속도

모션은 라이다에는 점군 skew 왜곡을 발생시키며, 카메라에는 모션블러를 가져온다.

• HDL graph SLAM이 가장 낮은 에러를 보였고, 루프폐쇄가 정확도를 유지할 수 있도록 해주었다.
• LIO SAM이 초기화 에러로 인해 최악의 오차를 보였다.
• ORB SLAM3와 Basalt VIO는 비슷한 오차를 보였고, 둘 다 라이다 중 리오샘과 레고로암보다 나았다.

실내 실험

실험1

• 한 변 2m의 사각형으로 도는 루프. 총 72m
• 코너에서 직각회전이라 피쳐 트래커에게 챌린징
• 네 변 중 두 변은 유리라 라이다에게 챌린징 (라이다에게 너무한거 아니니)
• 로암이 라이다 기반 중 가장 작은 RPE를 보임, 그 다음은 리오샘
• 비주얼들이 라이다보다 나았는데 Basalt가 가장 작은 오차를 가짐
• 표준편차를 비교해보면, 라이다 슬램이 비주얼 슬램보다 노이지함.
• APE로는 이제 global ocnsistency를 확인해볼 수 있는데, orb3가 가장 좋았음.
• 비슷하게, HDL은 최악의 rpe였지만 괜찮은 루프 폐쇄 덕에 괜찮은 ape를 가짐
• 라이다 기반의 ape가 매우 비슷비슷하다는게 흥미롭다

실험2

직선 구간, 360도 회전

• 360도 회전은 라이다의 skew문제를 끌고오기위함
• 또한 폭 좁은 실내라 비주얼쪽에서도 키포인트가 빨리 움직여 롤링셔터 왜곡이나 모션블러 등을 끌어들일것.
• visual SLAM이 rpe에선 라이다보다 나았다.
• Basalt가 가장 작은 오차, 그 다음이 svo2
• orb slam3의 프론트엔드가 제일 느린데, 계속해서 피쳐 트랙 로스가 떴다 회전에서. 근데 relocal이 계속 돼서 성능이 괜찮게 나온다
• LIO-SAM은 imu pre-integration이랑, 점군 deskewing(by imu)를 통해 라이다 슬램중 가장 낮은 오차를 달성했다.

실험3

로봇은 정지해있고 앞에 다이내믹 오브젝트가 왔다갔다거림 그래서 초기 위치로부터 얼마나 벗어났는지로 오차를 측정함

• 라이다들이 드리프트가 매우 적었고, 키메라빼고는 모든 카메라를 압도했다.
• HDL에서는 포즈그래프 노드들이 초기화되지않아(문턱 이동값을 넘지못해) 그냥 계속 0이었다.
• 카메라들은 20~30cm 가량의 드리프트가 있었다. 이는 라이다에 비해 작은 FOV로 인해 움직이는 물체에 취약할 수 밖에 없기 때문으로 보인다.
• 라이다는 근데 accumulated distance가 높았다. 이는 노이지한 추정치를 의미함. 그니까 제자리에서 엄청 흔들린거임.

계산 자원 비교

LEGO LOAM, LOAM이 가장 낮은 CPU 자원을 필요로한다. 포즈 그래프 베이스인 LIO SAM과 HDL의 CPU사용량은 훨씬 높았다. LIO SAM은 80프로의 피크를 찍기도 했다. (추가적인 imu integration 시 발생) 피쳐 디텍터가 cpu를 많이 잡아먹기때문에 orb3나 basalt, kimera 모두 많이 높았다.

모든 실외 실험 중에서 Lidar 위치가 상단에 장착된 LIO SAM이 우리 실험에 사용된 로더 유형 기계에 가장 적합했으며 APE의 RMS가 1.142m로 가장 적었습니다. 이동 중 진동이 심한 붐의 로더 버킷에 센서를 고정했을 때 성능이 저하된 IMU 종속 LIO SAM에는 안정성이 더 중요했습니다. LeGO LOAM은 가장 효율적이면서도 센서가 1.316m의 APE로 지면에 더 가까울 때 상대적으로 좋은 성능을 보였습니다. 임의의 LIDAR 위치를 사용하면 더 간단한 LOAM이 합리적인 결과를 얻었으므로 센서 위치가 고정되지 않은 경우 적합한 선택입니다. 시각적 알고리즘은 카메라가 지면에 가까울수록 더 잘 수행되었습니다.

이제 완전히 feature grid embedding + octree가 기본 구조로 자리잡은 듯 하다. spatial한 구조가 나름 최적화 된 것 같으니 여러 센서와의 퓨전 (특히 라이다와의 찰떡궁합을 예상)으로 쭉쭉 영역을 넓혀 나갈거라고 예상했는데 아니나 다를까 블로그에 포스팅 하는 지금 벌써 NeRF-LOAM이라는 애가 선발로 치고 나왔다.. 미묘한 기분이 든다. 바로 NeRF-LOAM 리뷰를 연이어 해야겠다.

• dense tracking&mapping system
• fuses neural implicit representations with traditional volumetric fusion methods
• use mlp to encode and optimize the scene inside each voxel
• octree-based structure for dynamic expansion (it enables the model to work without knowing the environment like in previous works)
• traditional visual slam method → they are incapable of rendering novel views as they cannot hallucinate the unseen parts of the scene.
• previous SLAM work with NeRF focusing to encode and compress the scene in terms of memory usage → pre-trained networks used in these systems generalize poorly to different types of scenes, making them less useful in practical scenarios

similar to NICE-slam, difference is:

(1) dynamically allocate sparse voxels on the fly (improves usability, reduces memory consumption)

(2) all learned on-the-fly (no pre-trained)

(3) keyframe strategy suitable for sparse voxels

NeRF is not the best for surface reconstruction, which takes main place for AR tasks. We force the network to learn more details near the surface within a distance.

~ explenations about the pros of explicit feature grid

encodes 3D scenes with neural networks and local embeddings just as Vox-Surf

SYSTEM

(0) initialize the global map by running a few mapping iterations for the first frame.

(1) under fixed network, estimate cam pose and send every frame with pose to mapper to construct global map

(2) The mapping process first takes the estimated camera poses and allocates new voxels and appropriately transformed 3D point cloud from the input depth map. Then it fuses the new voxel-based scene into the global map and applies the joint optimization.

(appropriately transformed 3D point cloud?)

(3) In order to reduce the complexity of optimization, we only maintain a small number of keyframes, which are selected by measuring the ratio of observed voxels. 음. 꽤 직관적이네.

(4) long-term map consistency is maintained by constantly optimizing a fixed window of keyframes.

voxel embeddings are attached to the vertices of each voxel and are shared by neighboring voxels → trilerp

efficient point sampling (inside voxel grid)-

• For each sampled pixel, we first check if it has hit any voxel along the visual ray by performing a fast ray-voxel intersection test. Pixels without any hit are masked out since they do not contribute to rendering
• enforce a limit M_h on how many voxels a single pixel is able to see

optimization

4 losses:

RGB loss, depth loss, free-space loss, SDF loss on the sampled points

• RGB/depth - MAE loss
• free-space/SDF loss - works with a truncation distance within which the surface is defined → similar to regularizer

tracking

• optim under lie algebra
• zero motion model
• simple back-propagation
• use pixel whose ray has intersection with existing voxel

mapping

Key-frame selection

Unlike previous works where key-frames are only inserted based on

• heuristically chosen metrics
• fixed interval

by an intersection test

This is risky for loopy camera motions → we also enforce a maximum interval between adjacent frames

Joint mapping and pose update

randomly select N keyframes(including the recently tracked frame), jointly optimize the scene network and feature embeddings

dynamic grid

dynamic Octree + morton

EXPERIMENT

datasets:

• all seq. of Replica dataset
• ScanNet
• custom (with iPhone)

metric:

(1) reconstruction quality

• accuracy, completion, completion ratio

(2) ATE RMSE

implementation detail:

• 0.2m voxel length

we use 0.07 length for room0 (reso=128)

pose accuracy only on ScanNet.

(single NVidia RTX 3090, on Replica set) tracking and mapping are profiled on a per-iteration

Depending on the scene complexity, our method can take around 150-200ms to track a new frame and 450-550ms for the joint frame and map optimization.

5hz for tracking, 2hz for optimization → quite slow (on the Replica office-0 scene) very small!!! (but I have to check for office0)

(for room0, 2.6MB under reso=64)

현재 프레임의 포즈를 보상 없이 추정한 후, 맵을 쌓을 때 이전 포즈와 현재포즈의 차이를 이용해 보상한 프레임을 사용해보기로 했다.

Pandar 128 채널 라이다를 사용해 실험했다. 프레임 하나를 띄워보았다. 민트 화살표가 차량 주행 방향이다. 이 데이터는 후방이 +y, 왼쪽이 +x, 위가 +z인 좌표계를 사용함을 확인했다.

판다는 앵글 스캔이 +y축을 기준으로 시계방향으로 이루어진다. 즉 +y축이 앵글컷 지점이다. 앵글컷 시작점에서부터 종료점까지 point의 angle을 이용해 포즈 변화량에 곱해줄 상수를 구할 것이다.

pcl::PointCloud<pcl::PointXYZI>::Ptr original_frame(new pcl::PointCloud<pcl::PointXYZI>());
for (size_t i = 0; i < scan.points.size(); i++)
{
    int ang = constrainAngle(RAD2DEG(atan2(scan.points[i].y, scan.points[i].x)));
    ang = (90 - ang) % 360;
    if (ang < 0)
        ang += 360;

    pcl::PointXYZI pt;
    pt_.x = scan.points[i].x;
    pt_.y = scan.points[i].y;
    pt_.z = scan.points[i].z;
    pt_.intensity = static_cast<float>(ang)/360*255;
    original_frame->points.push_back(pt_);
}
original_frame->width    = original_frame->size();
original_frame->height   = 1;
pcl::io::savePCDFile<pcl::PointXYZI>("/home/yeonsoo/original_frame.pcd", *original_frame);

위 코드를 사용하여 확인해보니 +y축을 앵글컷으로 하여 상수가 잘 들어갔다. ang/360이 이전 프레임과 현재 프레임 사이의 포즈 변화량을 반영하는 상수가 된다. (위 그림에서 파란색=0, 빨간색=255)

정면으로 3m 이동했을 때 상황을 구현해보자. 헤딩은 아이덴티티, (0,1,0)에서 (0,-2,0)로 움직였다.

Eigen::Matrix4f cur_pose, prev_pose;
prev_pose << 1, 0, 0, 0, 
             0, 1, 0, 1,
             0, 0, 1, 0,
             0, 0, 0, 1;

cur_pose << 1, 0, 0, 0, 
            0, 1, 0, -2,
            0, 0, 1, 0,
            0, 0, 0, 1;

Eigen::Vector3f del_pose_t; //translation 변화량
del_pose_t << cur_pose(0,3) - prev_pose(0,3),
              cur_pose(1,3) - prev_pose(1,3),
              cur_pose(2,3) - prev_pose(2,3);
del_pose_t /= 360.;
for (size_t i = 0; i < scan.points.size(); i++)
{
    Eigen::Matrix<float, 4, 1> ori_pt, cur_pt, cor_pt, next_pt;
    int ang = constrainAngle(RAD2DEG(atan2(scan.points[i].y, scan.points[i].x)));
    ang = (90 - ang) % 360;
    if (ang < 0)
        ang += 360;

    ori_pt << scan.points[i].x, scan.points[i].y, scan.points[i].z, 1;
    Eigen::Matrix4f part_pose;
    part_pose = prev_pose;
    part_pose(0, 3) += del_pose_t(0, 3) * ang;
    part_pose(1, 3) += del_pose_t(1, 3) * ang;
    part_pose(2, 3) += del_pose_t(2, 3) * ang;

    Eigen::Matrix4f prev_pose_inv;
    prev_pose_inv.block(0,0,3,3) = prev_pose.block(0,0,3,3).transpose();
    prev_pose_inv.col(3).topRows(3) = -prev_pose_inv.block(0,0,3,3)*prev_pose.col(3).topRows(3);
    cor_pt =  prev_pose_inv * part_pose * ori_pt;
    cor_pt =  part_pose * ori_pt;

    pt_compen.x = cor_pt(0,3);
    pt_compen.y = cor_pt(1,3);
    pt_compen.z = cor_pt(2,3);
    pt_compen.intensity = static_cast<float>(ang)/360*255;
    compen_frame->points.push_back(pt_compen);  
}
original_frame->width    = original_frame->size();
original_frame->height   = 1;
pcl::io::savePCDFile<pcl::PointXYZI>("/home/yeonsoo/original_frame.pcd", *original_frame);
compen_frame->width    = compen_frame->size();
compen_frame->height   = 1;
pcl::io::savePCDFile<pcl::PointXYZI>("/home/yeonsoo/compen_frame.pcd", *compen_frame);

짠.. 파란 점들은 prev_pose가 적용되었고 빨간 점들은 cur_pose가 적용되었다. 그 사이는 등속으로 보간되었다. scan: 라이다 프레임 part_pose: 월드 프레임 (월드 프레임 상에서 각이 ang_i일때의 시간 t_i에서의 prev_pose->cur_pose 보간 위치) compen_frame: 백 투 라이다 프레임

x, y를 한번에 움직여보자.

Eigen::Matrix4f cur_pose, prev_pose;
prev_pose << 1, 0, 0, 2, 
            0, 1, 0, 4,
            0, 0, 1, 0,
            0, 0, 0, 1;

cur_pose << 1, 0, 0, 7, 
            0, 1, 0, -2,
            0, 0, 1, 0,
            0, 0, 0, 1;

이번에는 prev_pose에서의 스캔 점군과 cur_pose에서의 스캔 점군, 그 사이 보상이 적용된 스캔 점군을 모두 월드 프레임 상에서 띄워보았다. 흰색: prev_pose 초록색: cur_pose

앵글컷 시작점인 파란점은 prev_pose와 잘 물려있고 빨간점은 cur_pose와 잘 물려있다.

rotation을 적용해보자. yaw로 60도를 돌려본다. 좌단: prev_pose 우단: yaw로 60도 돌린 cur_pose rotation은 quaternion 보간을 사용한다. 참고로 회전변환 보간의 경우 다양한 방법이 존재한다. 회전행렬에서 회전축과 각을 추출해 각만 1/n때려도 되고.. 쿼터니언 보간시에도 선형보간이나 2차보간 등 다 가능하지만 가장 많이 쓰이는 것은 slerp이다. Eigen 라이브러리에서도 지원한다. 아무튼 나는 항상 쿼터니언을 무조건적으로 선호하기때문에 쿼터니언 slerp 보간을 하도록 한다. 회전행렬 보간에 대한 이론을 더 알아보고 싶은 사람은 아래 링크의 ppt를 추천함. https://mycourses.aalto.fi/pluginfile.php/1120229/mod_resource/content/1/06_quaternions.pdf

 Eigen::Matrix4f cur_pose, prev_pose;
 prev_pose << 1, 0, 0, 10, 
              0, 1, 0, 10,
              0, 0, 1, 0,
              0, 0, 0, 1;

cur_pose << 1, 0, 0, 10, 
            0, 1, 0, 10,
            0, 0, 1, 0,
            0, 0, 0, 1;

cur_pose.block(0, 0, 3, 3) = map_builder_.eul2RotMatF(0, 0, -60*3.14159/180);

Eigen::Matrix3f prev_R = prev_pose.block(0, 0, 3, 3);
Eigen::Matrix3f cur_R = cur_pose.block(0, 0, 3, 3);
Eigen::Quaternionf v0, v1, vt;
v0 = prev_R;
v1 = cur_R;

pcl::PointXYZI pt;
pcl::PointCloud<pcl::PointXYZI>::Ptr original_frame(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr compen_frame(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr next_frame(new pcl::PointCloud<pcl::PointXYZI>());

for (size_t i = 0; i < scan.points.size(); i++)
{
    Eigen::Matrix<float, 4, 1> ori_pt, cur_pt, cor_pt, next_pt;
    int ang = map_builder_.constrainAngle(RAD2DEG(atan2(scan.points[i].y, scan.points[i].x)));
    ang = (90 - ang) % 360;
    if (ang < 0)
        ang += 360;

    ori_pt << scan.points[i].x, scan.points[i].y, scan.points[i].z, 1;
    cur_pt =  prev_pose * ori_pt;

    pcl::PointXYZI pt_, pt_compen, pt_next;
    pt_.x = cur_pt(0,3);
    pt_.y = cur_pt(1,3);
    pt_.z = cur_pt(2,3);
    pt_.intensity = static_cast<float>(ang)/360*255;
    original_frame->points.push_back(pt_);

    Eigen::Matrix4f part_pose;
    part_pose = prev_pose;
    vt = v0.slerp(static_cast<float>(ang)/360., v1);
    part_pose.block(0,0,3,3) = vt.toRotationMatrix();

    Eigen::Matrix4f prev_pose_inv;
    prev_pose_inv.block(0,0,3,3) = prev_pose.block(0,0,3,3).transpose();
    prev_pose_inv.col(3).topRows(3) = -prev_pose_inv.block(0,0,3,3)*prev_pose.col(3).topRows(3);
    cor_pt =  prev_pose_inv * part_pose * ori_pt;
    cor_pt =  part_pose * ori_pt;

    pt_compen.x = cor_pt(0,3);
    pt_compen.y = cor_pt(1,3);
    pt_compen.z = cor_pt(2,3);
    pt_compen.intensity = static_cast<float>(ang)/360*255;
    compen_frame->points.push_back(pt_compen);  

    next_pt =  cur_pose * ori_pt;
    pt_next.x = next_pt(0,3);
    pt_next.y = next_pt(1,3);
    pt_next.z = next_pt(2,3);
    pt_next.intensity = static_cast<float>(ang)/360*255;
    next_frame->points.push_back(pt_next);  
}
original_frame->width    = original_frame->size();
original_frame->height   = 1;
pcl::io::savePCDFile<pcl::PointXYZI>("/home/yeonsoo/original_frame.pcd", *original_frame);
compen_frame->width    = compen_frame->size();
compen_frame->height   = 1;
pcl::io::savePCDFile<pcl::PointXYZI>("/home/yeonsoo/compen_frame.pcd", *compen_frame);
next_frame->width    = next_frame->size();
next_frame->height   = 1;
pcl::io::savePCDFile<pcl::PointXYZI>("/home/yeonsoo/next_frame.pcd", *next_frame);

평행이동과 회전을 같이 적용해보자.

Eigen::Matrix4f cur_pose, prev_pose;
prev_pose << 1, 0, 0, 10, 
            0, 1, 0, 10,
            0, 0, 1, 0,
            0, 0, 0, 1;

cur_pose << 1, 0, 0, 14, 
            0, 1, 0, 2,
            0, 0, 1, 0,
            0, 0, 0, 1;

prev_pose.block(0, 0, 3, 3) = eul2RotMatF(0, 0, -30*3.14159/180);
cur_pose.block(0, 0, 3, 3) = eul2RotMatF(0, 0, -60*3.14159/180);

Eigen::Vector3f del_pose_t;
del_pose_t << cur_pose(0,3) - prev_pose(0,3),
                cur_pose(1,3) - prev_pose(1,3),
                cur_pose(2,3) - prev_pose(2,3);
del_pose_t /= 360.;

Eigen::Matrix3f prev_R = prev_pose.block(0, 0, 3, 3);
Eigen::Matrix3f cur_R = cur_pose.block(0, 0, 3, 3);
Eigen::Quaternionf v0, v1, vt;
v0 = prev_R;
v1 = cur_R;

pcl::PointCloud<pcl::PointXYZI> scan;
pcl::fromROSMsg(*point_cloud_msg, scan);

pcl::PointXYZI pt;
pcl::PointCloud<pcl::PointXYZI>::Ptr original_frame(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr compen_frame(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr next_frame(new pcl::PointCloud<pcl::PointXYZI>());

for (size_t i = 0; i < scan.points.size(); i++)
{
    Eigen::Matrix<float, 4, 1> ori_pt, cur_pt, cor_pt, next_pt;
    int ang = constrainAngle(RAD2DEG(atan2(scan.points[i].y, scan.points[i].x)));
    ang = (90 - ang) % 360;
    if (ang < 0)
        ang += 360;

    ori_pt << scan.points[i].x, scan.points[i].y, scan.points[i].z, 1;
    cur_pt =  prev_pose * ori_pt;

    pcl::PointXYZI pt_, pt_compen, pt_next;
    pt_.x = cur_pt(0,3);
    pt_.y = cur_pt(1,3);
    pt_.z = cur_pt(2,3);
    pt_.intensity = static_cast<float>(ang)/360*255;
    original_frame->points.push_back(pt_);

    Eigen::Matrix4f part_pose;
    part_pose = prev_pose;
    part_pose(0, 3) += del_pose_t(0, 3) * ang;
    part_pose(1, 3) += del_pose_t(1, 3) * ang;
    part_pose(2, 3) += del_pose_t(2, 3) * ang;
    vt = v0.slerp(static_cast<float>(ang)/360., v1);
    part_pose.block(0,0,3,3) = vt.toRotationMatrix();

    Eigen::Matrix4f prev_pose_inv;
    prev_pose_inv.block(0,0,3,3) = prev_pose.block(0,0,3,3).transpose();
    prev_pose_inv.col(3).topRows(3) = -prev_pose_inv.block(0,0,3,3)*prev_pose.col(3).topRows(3);
    cor_pt =  prev_pose_inv * part_pose * ori_pt;
    cor_pt =  part_pose * ori_pt;

    pt_compen.x = cor_pt(0,3);
    pt_compen.y = cor_pt(1,3);
    pt_compen.z = cor_pt(2,3);
    pt_compen.intensity = static_cast<float>(ang)/360*255;
    compen_frame->points.push_back(pt_compen);  


    next_pt =  cur_pose * ori_pt;
    pt_next.x = next_pt(0,3);
    pt_next.y = next_pt(1,3);
    pt_next.z = next_pt(2,3);
    pt_next.intensity = static_cast<float>(ang)/360*255;
    next_frame->points.push_back(pt_next);  
}
original_frame->width    = original_frame->size();
original_frame->height   = 1;
pcl::io::savePCDFile<pcl::PointXYZI>("/home/yeonsoo/original_frame.pcd", *original_frame);
compen_frame->width    = compen_frame->size();
compen_frame->height   = 1;
pcl::io::savePCDFile<pcl::PointXYZI>("/home/yeonsoo/compen_frame.pcd", *compen_frame);
next_frame->width    = next_frame->size();
next_frame->height   = 1;
pcl::io::savePCDFile<pcl::PointXYZI>("/home/yeonsoo/next_frame.pcd", *next_frame);

예쁘게 잘 나온다.

그럼 이걸 이용해 slam을 한 번 해보자. raw pointcloud frame이 들어왔을 때, (0) NDT matching으로 cur_pose를 추정한다. (1) 이전 프레임의 cur_pose 였던 prev_pose와, (0)에서 얻은 cur_pose를 이용해 위와 같이 프레임을 보상한다. (2) 보상한 프레임으로 GICP를 돌려 최종 cur_pose를 얻고, 보상한 프레임을 키프레임으로 저장해 맵에 축적한다.

비교를 위해 라이다 보상파트를 넣지 않고 매 프레임별로 NDT -> GICP를 했을 때의 결과를 먼저 확인한다. 왼쪽 그림에서 파란색으로 표시된 부분의 가로등을 보면, 기둥이 두 개로 생긴다.

그 다음은 보상을 해줬을 때.

음.. 기대와 다르게 별 효과가 없다. 오히려 더 더럽게 보인다.

아! 혹시 이게 문제인가? 현재는 라이다 스캔의 시작점이 prev_pose이고 끝점이 cur_pose라고 가정하고 있는데 사실은 prev_pose와 cur_pose의 중점을 스캔의 시작점으로 놓고, 끝점을 cur_pose를 prev_pose<->cur_pose 간의 속도를 이용해 1.5만큼 연장한 지점으로 잡는게 맞나?

그래서 아래와 같이 바꿔보았다.

Eigen::Matrix4f cur_pose, prev_pose, real_prev_pose;
cur_pose = pose_;
prev_pose = getEPose(previous_pose_);

Eigen::Vector3f del_pose_t;
del_pose_t << cur_pose(0,3) - prev_pose(0,3),
            cur_pose(1,3) - prev_pose(1,3),
            cur_pose(2,3) - prev_pose(2,3);



Eigen::Matrix3f prev_R = prev_pose.block(0, 0, 3, 3);
Eigen::Matrix3f cur_R = cur_pose.block(0, 0, 3, 3);
Eigen::Quaternionf v0, v1, vt;
v0 = prev_R;
v1 = cur_R;
vt = v0.slerp(0.5, v1);

real_prev_pose = prev_pose;
real_prev_pose.col(3).topRows(3) += del_pose_t/2;
real_prev_pose.block(0, 0, 3, 3) = vt.toRotationMatrix();

del_pose_t /= 360.;
Eigen::Matrix3f real_prev_R = real_prev_pose.block(0, 0, 3, 3);
v0 = real_prev_R;

Eigen::Matrix4f prev_pose_inv;
prev_pose_inv.block(0,0,3,3) = real_prev_pose.block(0,0,3,3).transpose();
prev_pose_inv.col(3).topRows(3) = -prev_pose_inv.block(0,0,3,3)*real_prev_pose.col(3).topRows(3);

pcl::PointXYZI pt;
for (size_t i = 0; i < input_cloud->points.size(); i++)
{
    Eigen::Matrix<float, 4, 1> ori_pt_f, compen_frame_w, compen_frame_f;
    ori_pt_f << input_cloud->points[i].x, input_cloud->points[i].y, input_cloud->points[i].z, 1;

    int ang = constrainAngle(RAD2DEG(atan2(input_cloud->points[i].y, input_cloud->points[i].x)));
    ang = (90 - ang) % 360;
    if (ang < 0)
        ang += 360;

    Eigen::Matrix4f part_pose; //on world frame
    part_pose = real_prev_pose;
    part_pose(0, 3) += del_pose_t(0, 3) * ang *2;
    part_pose(1, 3) += del_pose_t(1, 3) * ang *2;
    part_pose(2, 3) += del_pose_t(2, 3) * ang *2;

    vt = v0.slerp(static_cast<float>(ang)/360.*2, v1);
    part_pose.block(0,0,3,3) = vt.toRotationMatrix();

    compen_frame_w = part_pose * ori_pt_f; //on world frame
    compen_frame_f = prev_pose_inv * compen_frame_w; //back to prev frame

    pt.x = compen_frame_f(0);
    pt.y = compen_frame_f(1);
    pt.z = compen_frame_f(2);
    pt.intensity = input_cloud->points[i].intensity;

    output_cloud->points.push_back(pt);
}

여기 real_prev_pose가 prev_pose와 cur_pose의 중점이고, real_prev_pose와 prev_pose 사이의 포즈 변화량에 ang*2를 반영해줌으로써 구현하였다. 결과는 첫번째 시도와 비슷하게 안좋음.. 그냥 imu 쓰자.

• 멀티프로세서에 더 많은 쓰레드를 올리자
• 블록에 더 많은 쓰레드를 올리자 왜냐면 이게 레이턴시를 숨기는 유일한 방법이니까!

하지만 아래 두 개념은 잘못되었음

• 멀티쓰레딩이 GPU에서 레이턴시를 숨기는 유일한 방법이다
• 공유메모리가 레지스터만큼 빠르다

레이턴시 숨김 (Hide latency) 란 레이턴시 동안 다른 연산을 수행하여 빠르게 작업을 마치는 것을 말한다.

1. 더 적은 쓰레드로 산술 레이턴시를 숨겨보자 산술 레이턴시는 메모리 레이턴시보다 (I/O) 매우 작음. 대략 20사이클/400사이클. 아래와 같이 dependent한 연산은 앞의 연산이 끝나야 수행될 수 있다.

Thread-level parprallelism (TLP) 과 Instruction-leel parallelism (ILP): 왼: TLP, 오: ILP 위 예시는 ILP 없이 TLP만을 사용했을 때. 한 스레드에서 처리하기에는 너무너무 가벼운 연산이라 사이클의 낭비임 (idle cycle). 이렇게 ILP=2 를 사용해주면 TLP만 쓸 때보다 (점선) 더 적은 쓰레드 개수로 100% 유틸라이즈 가능 ILP를 더 늘리면?

idle cycle인지 어떻게 알지.. 직접 계산하는건가

2. 메모리 레이턴시를 숨겨보자 Needed parallelism = Latency x Throughput

음 일단 쓰레드별로 연산을 사이클에 꽉차게 쓰는게 좋다는거 Note, threads don’t stall on memory access – Only on data dependency

Note, local arrays are allocated in registers if possible 이렇게 float을 카피하는 연산이 있을 때, 한 쓰레드에서 많은 float을 한번에 copy하면 훨씬 유리함 memory intensive kernel 을 만들자!

3. 더 적은 쓰레드를 이용해 더 빠르게 돌리기 적은 쓰레드는 곧 각 쓰레드 별로 더 많은 레지스터가 할당됨을 의미. 공유메모리보다 훠어얼씬 빠름 (거의 6배 이상) 워프 내 쓰레드에서 공유메모리에 접근하는건 bank conflicts가 없는 한 거의 레지스터만큼 빠르다는건 잘못된 상식임. 즉 레지스터 활용을 잘 해야 피크에 근접한 성능을 낼 수 있음. 근데 레지스터를 많이 할당하는건 쓰레드의 낮은 점유를 의미. 근데 괜찮음. 이게 더 빠름.

이는 개별 쓰레드에서 여러개의 아웃풋을 계산함으로써도 달성될 수 있다. 예를 들어 근데 이제 각 쓰레드에서 최대로 쓸 수 있는 레지스터 용량도 정해져있으니까 이건 참고하기

출처: https://www.nvidia.com/content/gtc-2010/pdfs/2238_gtc2010.pdf

그리드 라는건 독립적인 블록들로 이루어짐 블록은 그 블록 내에서 서로 교류 가능한 쓰레드 들로 이루어짐 32개의 쓰레드는 워프 라는 단위를 이룸 인스트럭션은 워프 단위로 issued 피연산자가 준비되지 않을 경우 워프 전체가 정지됨

• 워프가 정지되면 다른 워프로 context switch가 일어나는데, 이는 매우 빠르게 실행되어야함

레지스터와 공유메모리는 블록이 활성화되어있는 동안은 그 블록에 묶여있음 한번 블록이 활성화되면 그 블록 내 모든 쓰레드가 완료될때까지 살아있다

최상의 전역 메모리 대역폭을 달성하기 위해서는 지연을 감추기 위한 transaction in flight를 충분히 가져야 함 이는

• 점유를 높이거나
• instruction 레벨의 병렬을 늘림으로써

달성 가능

*메모리 대역폭이란? 프로세서가 반도체 메모리에서 데이터를 읽거나 쓰는 속도. byte/s 단위.

cuda occupancy calculator와 visual profiler를 사용하면 메모리 대역폭/점유율을 확인할 수 있다.

점유율 = 활성 워프 / 최대 활성 워프 전체 블록 하나에 리소스가 할당됨

• 리소스는 유한함
• 각 쓰레드당 너무 많은 리소스를 쓰면 점유율을 제한할 수 있음 점유는
• 레지스터 사용량
• 공유메모리 사용량
• 블록 크기 에 따라 제한될 수 있다.

점유율 제한 요인:

• 레지스터 사용량

• -ptxas-options=-v 와 함께 컴파일하면 확인 가능 SM 당 32k 레지스터 (Fermi의 경우) 예시1) 커널은 각 쓰레드별 20개의 레지스터를 사용 (+1 임시) 활성 쓰레드 = 32k/21 = 1560 쓰레드 (가능)

• 1536 쓰레드 (in SM) 사용시 1의 점유율 예시2) 커널이 각 쓰레드별 63개의 레지스터를 사용 (+1 임시) 활성 쓰레드 = 32k/64 = 512 쓰레드 가능

• 512/1536 = .3333 점유율 Can control register usage with the nvcc flag: --maxrregcount

• 공유메모리 마찬가지로 --ptxas-options=-v 로 컴파일시 각 블록당 공유메모리를 확인 가능 Fermi의 경우 16K 또는 48K의 공유메모리 있음 예시1) 48K 공유메모리의 경우 커널이 쓰레드별로 32바이트의 공유메모리 사용시 48K/32 = 1536 쓰레드 가능

• occupancy=1 예시2) 16K 공유메모리의 경우 16K/32=512 쓰레드. 이때 1536 쓰레드 할당 시 occ=.333 공유메모리를 너무 크게 잡지 말 것.

• 블록 사이즈 각 SM은 최대 8개의 활성 블록을 가짐 작은 블록 크기는 전체 쓰레드의 개수를 제한함 블록 개수를 많게 하고, 쓰레드 개수는 일반적으로 128-256으로 하도록 한다

점유율은 보통의 경우 66%정도면 최대 대역폭을 만족함 점유율을 높이려고 애쓰는것보다 instruction level parallelism (ILP)를 살펴보는게 훨씬 효과가 크다 Vasily Volkov’s GTC2010 talk “Better Performance at Lower Occupancy” https://www.nvidia.com/content/gtc-2010/pdfs/2238_gtc2010.pdf

출처: https://on-demand.gputechconf.com/gtc-express/2011/presentations/cuda_webinars_WarpsAndOccupancy.pdf

https://github.com/dginev/ar5iv 이게 해당 프로젝트인 것 같은데 설명을 보니 rXiv.org의 논문들을 같이 제공되는 latexml을 이용해 HTML5 페이지로 변환해주는 웹서비스라고 한다.

현재 버전은 3.10.2

sudo apt remove cmake

이걸로 이전 버전 cmake를 지우라는 얘기가 있는데 지울 필요 없음. 지우면 ROS 다 날라감. https://cmake.org/files/ 여기에서 대충 아무 버전이나 선택, 안에 들어가면 그 중 제일 높은 버전의 tar.gz 다운.

cd Downloads/
tar -xvzf [cmake-3.xx.x.tar.gz]
cd cmake-3.xx.x/
./bootstrap --prefix=/usr/local
make
sudo make install

다 하고 cmake --version으로 확인.

TBA

아래는 Xiang Gao와 Tao Zhang의 <Introduction to Visual SLAM>을 읽고 번역정리+추가적인 코멘트를 단 내용이다. 문장은 가능하면 간소화하고, 이론적으로 더 참고할만한 부분은 추가적인 코멘트를 넣었음. 저자에 의해 모든 자료가 무료 배포되었음. 아래 링크 참고 📘 영어판 도서 파일: https://github.com/gaoxiang12/slambook-en 📂 practice 코드: https://github.com/gaoxiang12/slambook2

2장. 3D rigid Body Motion

🔎 학습 목표

1. 3D 공간 상의 rigid body 기하학- 회전행렬, 변환행렬, 쿼터니언, 오일러각- 을 익힌다.
2. Eigen 라이브러리의 행렬&기하 모듈 사용법을 익힌다.

지난 시간에 우리는 visual SLAM의 전반적인 프레임워크와 내용을 살펴보았다. 이번 강의에서는 visual SLAM의 가장 기초적인 이론을 다룬다. 바로 3D 공간에서 어떻게 강체의 움직임을 나타낼 것인지에 대한 내용이다. (계속 등장할 "강체" (rigid body)는 단순히 찌그러지거나 늘어나지 않는 단단한 덩어리의 개념이라고 생각하면 됨, 즉 움직임이나 외력에 의해 물체의 형태가 변하지 않는다는 가정을 담아 "물체"보다 좀 더 specific한 표현을 사용)

이 움직임은 회전과 평행이동의 조합으로 표현될 수 있다는 것은 직관적으로 느낌이 올 텐데, 평행이동 쪽은 상당히 간단하지만 회전 쪽은 생각보다 복잡한 문제들이 얽혀있다. 회전행렬과 쿼터니언, 오일러각의 의미와 그들이 어떻게 계산되고 변환되는지를 살펴보며 회전에 대한 개념을 정확히 잡아보도록 하자. 실습 파트에서는 Eigen이라는 가장 흔히 쓰이는 선형대수 라이브러리를 소개한다. Eigen은 C++ 행렬연산을 제공하고, 그 안의 기하학 모듈은 쿼터니언과 같은 기본적인 데이터 구조나 연산을 지원한다. Eigen은 고도로 최적화 되어있지만, 여전히 몇가지 이슈를 가지고 있다. 실습 파트에서 자세히 살펴보자!

2.1 회전행렬

2.1.1 점, 벡터, 좌표시스템

우리는 3차원 공간에서 살기 때문에 태생적으로 3D 움직임에 익숙하다. 3차원 공간은 3개의 축으로 이루어져있기 때문에 공간상의 한 점의 위치는 3개의 좌표로 특정 가능하다. 하지만 우리는 이제 한 점이 아닌, 위치(position)와 방향(orientation)을 가진 강체를 고려해야한다. 카메라 또한 3차원 공간상의 강체로 볼 수 있으므로 visual SLAM에서 우리가 구하려는 것 중 하나-카메라의 포즈- 역시 강체의 위치와 방향이 된다. "현재 카메라의 위치는 (0,0,0)이고, 앞쪽을 보고 있어" 라는 말을 어떻게 수학적인 언어로 표현할 수 있을까? 가장 기본적인 개념인 점과 벡터에서부터 시작해보자. 길이와 부피가 없는 '점'은 공간의 가장 기본적인 성분이다. 두 점을 이으면 벡터가 된다. 벡터는 한 점에서 다른 한 점을 가리키는 화살표로 이해하면 된다. 여기서 잠깐 짚고 넘어갈 점은 벡터를 벡터 좌표와 혼동하면 안된다. 기본 선형대수 지식을 잠깐 떠올려본다면, 3D 공간 상 한 점의 좌표는 $\mathbb{R}^3$로 표현될 수 있다. 선형 공간에서 우리는 공간의 기저(base) 벡터 $(\textbf{e}_1 , \textbf{e}_2, \textbf{e}_3)$를 잡을 수 있다.

<기저벡터> 공간 상에서 선형 독립 (linearly independent)인 벡터의 집합 일반적으로 서로 orthogonal하고 단위길이를 가짐 (보통 계산하기 편하니까 그렇게 잡는다는거지 필수조건 아님)

그러고 나면, 임의의 벡터 $\textbf{a}$는 이 기저벡터에 대한 좌표를 가지게 된다. $$ \textbf{a} = [\textbf{e}_1, \textbf{e}_2, \textbf{e}_3] \left[\begin{matrix} a_1 \ a_2 \ a_3 \end{matrix}\right] = a_1\textbf{e}_1 + a_2\textbf{e}_2 + a_3\textbf{e}_3 $$ 여기서 $(a_1, a_2, a_3)^T$를 우리는 벡터 $\textbf{a}$의 좌표라고 부른다.

즉 벡터의 좌표값은 벡터가 공간에 어떻게 뉘어져있는지 그 벡터 자체뿐만 아니라 기저벡터를 어떻게 잡냐에 따라 달라진다. $\mathbb{R}^3$에서 좌표 시스템은 일반적으로 3개의 orthogonal한 좌표축으로 이루어진다. 예를 들어 $\textbf{x}, \textbf{y}$ 축이 주어지면 $\textbf{z}$축은 오른손의 법칙 또는 왼손의 법칙에 따라 $\textbf{x} \times \textbf{y}$로 얻을 수 있다. 정의하기에 따라 좌표시스템은 왼손좌표계/오른손좌표계로 나뉘는데, 3번째 축은 두 시스템 간에 반대방향으로 정의된다. 대부분의 3D 라이브러리는 (OpenGL, 3DS Max 등..) 오른손좌표계를 사용하고, Unity나 Direct3D 등은 왼손좌표계를 사용한다.

여러분이 기본적인 선형 대수 지식은 있다고 가정하고 벡터나 스칼라 사이의 더하기-빼기와 같은 기본 연산에 관한 설명은 생략한다. 내적과 외적은 다시 한번 살펴본다. 두 벡터 $\textbf{a}, \textbf{b} \in \mathbb{R}^3$ 에 대해 내적은 다음과 같이 정의된다: $$ \textbf{a} \cdot \textbf{b} = \textbf{a}^T\textbf{b} = \sum^{3}_{i=1}{a_ib_i} = |\textbf{a}||\textbf{b}|\cos\left<\textbf{a},\textbf{b}\right> $$ 여기서 $\left<\textbf{a},\textbf{b}\right>$는 두 벡터 간 각도를 의미한다. 내적은 두 벡터 간의 투영(projection) 관계를 나타내기도 한다.

외적의 경우: $$ \textbf{a} \times \textbf{b} = \Bigg|\begin{matrix} \textbf{e}1 & \textbf{e}_2 & \textbf{e}_3 \ a_1 & a_2 & a_3 \ b_1 & b_2 & b_3 \end{matrix} \Bigg| = \left[\begin{matrix} a_2b_3 - a_3b_2\ a_3b_1 - a_1b_3 \ a_1b_2 - a_2b_1 \end{matrix}\right]= \left[\begin{matrix} 0 & -a_3 & a_2 \ a_3 & 0 & -a_1\ -a_2 & a_1 & 0 \end{matrix}\right]\textbf{b} \triangleq \textbf{a}^\land \textbf{b} $$ 두 벡터를 외적하면 결과는 두 벡터에 수직인 방향을 가지는 벡터가 된다. 길이는 $|\textbf{a}||\textbf{b}\sin\left<\textbf{a},\textbf{b}\right>$이고, 이는 두 벡터가 이루는 사변형의 면적과 같다. 위의 $^\land$라는 연산자는 벡터 $\textbf{a}$를 skew-symmetric 행렬로 변환해주는 연산이다. 이를 이용하면 외적 $\textbf{a} \times \textbf{b}$를 $\textbf{a}^\land \textbf{b}$라는 행렬과 벡터의 곱-선형 연산-으로 표현할 수 있다. 이 기호는 앞으로 자주 쓰일 것이니 기억해놓길 바란다. 이 연산은 일대일 매핑이라 임의의 벡터는 유일한 anti-symmetric 행렬을 가진다. _(skew-symmetric, anti-symmetric 다 같은 말, 그냥 대각선을 기준으로 대칭인 성분끼리 부호 다르고 크기 같은 행렬을 뜻한다고만 알면 됨) $$ \textbf{a}^\land = \left[\begin{matrix} 0 & -a_3 & a_2 \ a_3 & 0 & -a_1\ -a_2 & a_1 & 0 \end{matrix}\right] $$

덧셈이나 뺄셈, 내/외적과 같은 모든 벡터 연산은 우리가 그들의 좌표를 모르더라도 (즉, 기저벡터를 특정해 벡터좌표를 구하지 않더라도) 계산될 수 있다는 것을 알아두라. 예를 들어 두 벡터의 좌표를 모르지만 내적을 하고 싶으면 두 벡터들의 길이와 각도를 통해 내적값을 계산할 수 있다. 내적 결과는 좌표시스템을 어떻게 선택하느냐와는 독립적이라는 뜻이다.

2.1.2 좌표계 간 유클리디안 변환

우리는 종종 하나의 환경에서 여러 좌표계를 정의한다- 로보틱스에서는, 매 링크와 관절마다 각각의 좌표계를 정의한다. 3D 매핑에서는, 각 cuboid나 cylinder마다 좌표계를 정의한다. 움직이는 로봇의 경우에는 고정 관성 좌표계(stationary inertial coordinate system, = 월드좌표계)를 사용하는 것이 일반적이다 (아래 그림의 $x_W, y_W, z_W$). 한편, 카메라나 로봇은 위 그림의 $x_C, y_C, z_C$에 해당하는, 움직이는 좌표계이다.

공간 상의 어떤 벡터 $\textbf{p}$를 생각해보자. 이 벡터는 카메라 좌표계에서는 $\textbf{p}c$라는 좌표를 가지고, 월드 좌표계에서 보면 $\textbf{p}_w$라는 좌표를 가지는데, 이 두 좌표 간 변환을 어떻게 해주면 될까? 우선은 카메라 좌표계 상에서의 좌표값을 구하고, 그 다음에 적절한 변환을 해주면 될 것 같은데, 이 변환과정을 수학적으로 표현해보자. 이제 설명하겠지만 이 과정은 변환행렬 $\textbf{T}$로 표현이 가능하다. 직관적으로 생각해보면 위 그림의 두 좌표계는 하나를 조금 돌리고(회전) 평행이동 시키면 겹칠 수 있는데, 이 회전과 평행이동을 강체 운동 (rigid body motion)이라고 한다. 직관적으로, 두 좌표계 간의 motion은 rotation과 translation으로 이루어져있는데, 이를 rigid body motion이라고 한다. 당연히 카메라 모션도 강체운동이다. 강체운동에서는 벡터의 길이나 각도가 변하지 않는다. 공중으로 핸드폰을 던지는 경우를 생각해보라. 이 때는 공간적인 위치와 방향만이 바뀌고, 길이나 각 면의 각도 등은 바뀌지 않는다. 핸드폰이 모션 중에 지우개처럼 뭉개지거나 쭉 늘어나지는 않으니까! 이 상황에서 우리는 핸드폰의 움직임이 유클리디안이라고 말할 수 있다. (강체 변환과 유클리디안 변환은 동의어임)_ 반복해 말하지만 유클리디안 변환은 회전과 평행이동으로 이루어져있는데, 우선 회전을 살펴보자. 여기 단위길이의 orthogonal한 기저벡터 $(\textbf{e}1, \textbf{e}_2, \textbf{e}_3)$가 있다. 회전 후에 이것들은 $(\textbf{e}'_1, \textbf{e}'_2, \textbf{e}'_3)$가 된다. 그러면 공간 상에 그냥 가만히 누워있는 벡터 $\textbf{a}$의 좌표는 $[a_1, a_2, a_3]^T$ 에서 $[a'_1, a'_2, a'_3]^T$로 바뀌게 되는데, 벡터 자체는 변하지 않았기 때문에 벡터 좌표의 정의에 따라 다음의 등식이 성립한다. $$ [\textbf{e}_1, \textbf{e}_2, \textbf{e}_3] \left[\begin{matrix} a_1 \ a_2 \ a_3 \end{matrix}\right] = [\textbf{e}'_1, \textbf{e}'_2, \textbf{e}'_3] \left[\begin{matrix} a'_1 \ a'_2 \ a'_3 \end{matrix}\right] $$ 양쪽에 $\left[\begin{matrix} \textbf{e}^T_1 \ \textbf{e}^T_2 \ \textbf{e}^T_3 \end{matrix}\right]$ 를 곱하면 우변의 왼쪽 행렬이 단위행렬이 되면서 아래와 같은 식을 얻을 수 있다. $$ \left[\begin{matrix} a_1 \ a_2 \ a_3 \end{matrix}\right] = \underbrace{ \left[\begin{matrix} \textbf{e}^T_1\textbf{e}_1 & \textbf{e}^T_1\textbf{e}_2 & \textbf{e}^T_1\textbf{e}_3\ \textbf{e}^T_2\textbf{e}_1 & \textbf{e}^T_2\textbf{e}_2 & \textbf{e}^T_2\textbf{e}_3\ \textbf{e}^T_3\textbf{e}_1 & \textbf{e}^T_3\textbf{e}_2 & \textbf{e}^T_3\textbf{e}_3\ \end{matrix}\right]}{\text{rotation matrix}}

\left[\begin{matrix} a'_1 \ a'_2 \ a'_3 \end{matrix}\right] \triangleq \textbf{R}\textbf{a}' $$ 여기서 저 중간의 행렬을 행렬 $\textbf{R}$로 정의한다. 이 행렬은 두 좌표계의 기저행렬들간의 내적으로 구성되어있으며, 회전 전후 동일 벡터의 좌표에 대한 변환관계를 담고있다. 같은 회전에 대해서는 어떤 벡터든 동일한 $\textbf{R}$을 통해 변환할 수 있다. 이 행렬 $\textbf{R}$를 회전행렬이라고 부르자. 한편, 행렬의 성분들은 두 좌표계간 기저행렬들의 내적으로 이루어진다. 기저행렬의 길이가 1이므로 이건 사실 기저행렬간 각도의 코사인을 취한 값과 동일하다. 따라서 이 행렬을 다른 말로 방향 코사인 행렬 (Direction Cosine Matrix, DCM)이라고 부르기도 하는데, 우리는 그냥 회전행렬이라고 쭉 부르는걸로 하자. 이 회전행렬은 몇가지 특수한 성질을 가지고 있다. 사실 이 행렬은 determinant가 1인 orthogonal 행렬이다. 역으로, orthogonal하면서 determinant가 1인 모든 행렬은 회전행렬이다. 따라서, 우리는 n차원의 회전 행렬 집합을 다음과 같이 정의할 수 있다: $$ \text{SO}(n) = {\textbf{R}\in \mathbb{R}^{n\times n}|\textbf{R}\textbf{R}^T=\textbf{I}, \det(\textbf{R})=1 } $$ $\text{SO}(n)$은 _special orthogonal group_을 나타내는 용어로, n차원 공간에서의 회전행렬들로 이루어져있다. $\text{SO}(3)$라고 하면 3차원 공간에서의 회전행렬 그룹을 의미하는 것이다.

여기서 orthogonal과 회전변환의 관계에 대한 정리를 잠깐 해보자. 사실 정확히 말하자면 모든 회전행렬은 직교행렬(orthogonal matrix)이고, 따라서 직교행렬의 성질을 계승한다. 직교행렬의 성질 중 하나가 벡터에 곱해도 그 벡터의 크기를 변화시키지 않는다는 것인데, 즉, 직교행렬 Q와 어떤 열벡터 v가 있으면 ||Qv|| = ||v||이다. 유클리드 공간 상에서 어떤 벡터의 크기는 변화하지 않았는데 좌표만 바뀌었다면 그건 회전 또는 반전이 일어났음을 의미하고, 따라서 사실 모든 직교행렬은 회전행렬이거나(det=1), *부적절한 회전 행렬이다(det=-1) *영어로 improper rotation이라 부적절한 회전행렬이라고 번역하긴 했지만 그냥 회전+반사를 같이 행하는 변환을 저렇게 표현함. *직교행렬의 성질 중 하나- det은 1이거나 -1이다(증명은 매우 간단함) 참고)

• Why are orthogonal matrices generalizations of rotations and reflections? https://math.stackexchange.com/questions/612936/why-are-orthogonal-matrices-generalizations-of-rotations-and-reflections
• Orthogonal Matrix (직교 행렬) 이란? https://m.blog.naver.com/PostView.naver?isHttpsRedirect=true&blogId=sw4r&logNo=221358626240
• improper rotation & 수도스케일러(pseudoscalar) 수도벡터(pseudovector) 수도텐서(pseudotensor) https://sciphy.tistory.com/537

회전행렬의 역행렬은 반대방향으로의 회전을 의미하고, 역행렬=전치행렬(직교행렬의 특성)이므로 반대방향으로의 회전은 전치행렬과 같다. $$ \textbf{a}' = \textbf{R}^{-1}\textbf{a} = \textbf{R}^{\text{T}}\textbf{a} $$

유클리디안 변환은 회전과 평행이동을 함께 고려한다. 월드좌표계에 있는 벡터 $\textbf{a}$를 $\textbf{R}$로 회전하고 $\textbf{t}$만큼 평행이동을 하면 $\textbf{a}’$라는 벡터가 되고, 이걸 식으로 표현하면 $\textbf{a}’ = \textbf{R}\textbf{a} + \textbf{t}$이다. 여기서 $\textbf{t}$는 평행이동(translation) 벡터이다. 회전과 비교했을 때, 평행이동 부분은 그냥 회전 후의 좌표에 평행이동 벡터를 단순히 더하면 끝으로 매우 간단한 것을 알 수 있다. 위 식을 통해 우리는 회전행렬 $\textbf{R}$과 평행이동 벡터 $\textbf{t}$를 이용해 좌표변환관계를 완전히 나타낼 수 있다. 좀 더 일반화를 해보자면, 서로 다른 좌표계 1과 2가 있을 때 두 좌표계 상에서 벡터 $\textbf{a}$의 좌표를 $\textbf{a}1$, $\textbf{a}_2$라고 하면 아래와 같이 두 좌표계 간 관계를 나타낼 수 있다. $$ \textbf{a}_1 = \textbf{R}{12}\textbf{a}2 + \textbf{t}{12} $$ 여기서 $\textbf{R}{12}$는 “벡터를 좌표계 2에서 1로 바꿀때의 회전" _(벡터를 돌리는게 아니고 벡터의 좌표를 변환해주는거임)_을 의미한다. 벡터가 회전행렬의 오른쪽에 곱해지기 때문에 아래첨자는 오른쪽에서 왼쪽 순서로 읽어주도록 쓴다. 좌표변환은 원체가 엄청 헷갈리는데다가 특히나 여러 좌표계를 왔다갔다 하게되면 큰 혼란이 올 수 있으므로 일단 앞으로 우리가 쓸 표기법을 명확하게 하도록 하자. 1에서 2로의 회전행렬은 $\textbf{R}{21}$로 쓴다. 정의하기에 따라 다르기때문에 책마다 표기법이 다를 수 있는데, 우리 책에서는 앞으로 이렇게 쓰기로 한다.

R12의 “벡터를 좌표계2에서 1로 바꿀때의 회전” 이라는게 기하학적으로 와닿지 않는? 불명확한 부분이 있을 수 있는데 이건 따로 정리

$\textbf{t}_{12}$의 기하학적 의미를 살펴보면, 이건 좌표계1의 시각에서 본 좌표계 1의 원점에서 좌표계2의 원점을 향하는 벡터를 의미한다. (즉 좌표계1 상에서 좌표계2의 원점의 좌표) 따라서 이걸 “1에서 2로의 벡터”라고 받아들이는 것을 추천한다.

벡터의 개념... 항상 원점이 꼬리 아닌가? 그러면 결국 좌표계 1의 원점에서 좌표계 2의 원점을 향하는 벡터의 월드좌표 = 좌표계 1 상에서 동일 벡터의 좌표 아닌가?? 원래 꼬리를 항상 원점으로 당겨와야하지않나? 확인 필요

좌표계 2의 원점에서 좌표계 1의 원점을 향하는 벡터의 좌표계 2 기준 벡터좌표인 $\textbf{t}{21}$는 $-\textbf{t}{12}$와 같지 않다. 두 좌표계간 회전이 존재하기 때문이다.

오.. 정말? 아 하긴 좌표계는 평행이동이 아니고 로테이션도 포함되어있으니 당연한거구나

“지금 내 카메라의 좌표가 뭐냐”라고 한다면 일반적으로는 월드좌표계 $\text{W}$ 상에서 카메라계 $\text{C}$의 원점을 가리키는 벡터 좌표를 의미하므로 $t_{\text{WC}}$로 표현할 수 있다. 이건 방금 말했듯이 $-t_{\text{CW}}$와 같지 않고, 뒤에서 배우겠지만 $-R_{\text{CW}}t_{\text{CW}}$와 같다.

2.1.3 변환행렬과 동차좌표계

위 식을 통해 우리는 유클리디안 공간 상에서의 회전과 평행이동을 완벽하게 표현할 수 있게 되었지만, 작은 문제가 남아있다. 바로 위 변환식은 선형이 아니라는 것! (지금 보여주겠지만 이건 상당히 끔찍한 문제이다) 우리가 $\textbf{R}_1$, $\textbf{t}_1$, 그리고 $\textbf{R}_2$, $\textbf{t}_2$ 두 개의 변환을 수행한다고 해보자: $$ \textbf{b} = \textbf{R}_1\textbf{a} + \textbf{t}_1, \quad\quad \textbf{c} = \textbf{R}_2\textbf{b} + \textbf{t}_2 $$

그럼 $\textbf{a}$에서 $\textbf{c}$로의 변환은 이렇게 표현된다: $$ \textbf{c} = \textbf{R}_2(\textbf{R}_1\textbf{a} + \textbf{t}_1) + \textbf{t}_2 $$

즉, 여러번 변환을 하게되면 전혀 우아하지 않은 더러운 식이 나오게 된다. 이건 너무 보기 싫으니까(단순히 보기 싫어서만은 아니지만) 동차좌표(homogeneous coordinates)를 이용해 변환행렬이라는 걸 다음과 같이 정의해보자. $$ \left[\begin{matrix} \textbf{a}' \ 1 \end{matrix}\right] = \left[\begin{matrix} \textbf{R} & \textbf{t}\ \textbf{0}^{\text{T}} & 1 \end{matrix}\right] \left[\begin{matrix} \textbf{a} \ 1 \end{matrix}\right] \triangleq \textbf{T}\left[\begin{matrix} \textbf{a} \ 1 \end{matrix}\right] $$

이건 일종의 수학적인 트릭이라고 볼 수 있는데, 3차원 벡터의 끝에 1을 추가해 동차좌표라 부르는 4D 벡터로 바꿔준다. 이 4차원 벡터에 대해서는 회전과 평행이동을 한번에 단일 행렬로 표현할 수 있고, 이러면 전체 변환과정이 선형이 된다. 여기의 $\textbf{T}$를 변환행렬이라고 부른다.

$\textbf{a}$의 동차좌표를 $\textbf{~a}$로 나타내도록 하자. 그러면 동차좌표와 변환행렬을 사용해서 표현한 $\textbf{a}$에서 $\textbf{c}$로의 변환은 이런 예쁜 모양을 갖게된다. $$ \textbf{~b} = \textbf{T}_1\textbf{~a},\quad \textbf{~c} = \textbf{T}_2\textbf{~b} \quad \quad \Rightarrow \textbf{~c} = \textbf{T}_1\textbf{T}_2\textbf{~a} $$

그런데 끝에 1을 붙였다 뗐다 하는 차이밖에 없는데 항상 기호로 동차좌표와 비동차좌표 벡터를 구별해서 표현하긴 귀찮으니까 여기서는 앞으로 그냥 구분하지않고 같은 표현으로 쓰도록 하겠다. 그냥 앞에 곱해지는 행렬이 열이 4개다 하면 아 이 벡터는 동차좌표구나 하면 된다. 예를들어 $\textbf{T}\textbf{a}$라고 썼으면 $\textbf{a}$는 동차좌표인거고(아니라면 계산이 안됨), $\textbf{R}\textbf{a}$라고 썼으면 비동차좌표인 걸로 여기면 된다.

변환행렬 $\textbf{T}$는 특수한 구조를 가진다. 좌상단이 회전행렬이고, 가장 오른쪽 열은 평행이동 벡터이고, 가장 밑 행은 0 0 0 1이다. 이 변환행렬의 집합은 special euclidean group_에서 첫자를 따 SE로 표현된다. $$ \text{SE}(3) = {\textbf{T} = \left[\begin{matrix} \textbf{R} & \textbf{t} \ \textbf{0}^{\text{T}} & 1\end{matrix}\right] \in \mathbb{R}^{4 \times 4} | \textbf{R} \in \text{SO}(3), \textbf{t} \in \mathbb{R}^3} $$ $\text{SO}(3)$와 마찬가지로 변환행렬의 역행렬은 역변환을 의미한다. 앞에서와 같이, 우리는 $\textbf{T}{12}$를 2에서 1로의 변환을 나타내도록 정의한다.

자, 복습해보자. 첫번째로 우리는 벡터와 벡터의 좌표 표현을 살펴보았고 벡터 간 연산을 소개했다. 좌표계간의 이동은 유클리디안 변환으로 설명되고, 이는 회전과 평행이동으로 이루어져있다. 회전은 회전행렬 $\text{SO}(3)$로 묘사되고, 평행이동은 $\mathbb{R}^3$ 벡터로 나타나진다. 마지막으로, 평행이동과 회전을 하나의 행렬로 한번에 나타내면 그게 변환행렬 $\text{SE}(3)$이다.

2.2 실습: Eigen 사용하기

이번 강의의 실습 파트는 두 부분으로 이루어진다. 첫번째 파트에서는 어떻게 Eigen을 사용해 행렬과 벡터를 나타내는지, 그리고 회전행렬과 변환행렬을 어떻게 계산하는지를 설명할 것이다.

이번 실습의 코드는 “slambook2/ch3/useEigen”에서 찾아볼 수 있다.

Eigen (http://eigen.tuxfamily.org/index.php?title=Main_Page)은 C++ 오픈소스 선형대수 라이브러리로, 행렬에 대한 선형대수 연산을 최적화된 빠른 속도로 수행할 수 있도록 지원하며 선형 방정식을 푸는 것과 같은 여러 함수들을 제공한다. 많은 상위 소프트웨어 라이브러리(g2o, Sophus, etc..) 들도 행렬 연산을 위해 Eigen을 사용한다. PC에 Eigen이 깔려있지 않다면 다음의 커맨드로 설치를 해보자.

sudo apt-get install libeigen3-dev

우리 책에서 쓰이는 대부분의 라이브러리들은 우분투 소프트웨어 센터에 등록되어있으므로 apt 명령어를 사용해 손쉽게 설치 가능하다. 저번 강의에서 우리는 라이브러리가 헤더 파일과 라이브러리 파일로 이루어져있다는 것을 배웠다. Eigen은 오로지 헤더파일들로만 빌드된 특이한 라이브러리이다. 이말은, .so나 .a같은 바이너리 파일 없이 헤더 파일만 위치시키면 가져다 쓸 수 있다는 말이다. 즉 라이브러리 파일을 링크시킬 필요 없다.

그럼 이제 아래의 코드조각을 통해 Eigen을 연습해보자!

2.3 회전 벡터와 오일러각

2.3.1 회전 벡터

이제 다시 이론적인 부분으로 넘어가보자. 회전 행렬을 사용한 4X4 변환 행렬만으로 6DoF의 3D 강체의 움직임을 나타내기 충분할까? 이 행렬 표현은 다음의 명확한 단점을 가지고 있다.

1. SO(3)는 9개의 값을 가진 회전 행렬이지만, 3D 회전은 사실 3자유도 운동이기 때문에 이 행렬 표현은 불필요하게 많은 값을 사용한다는 것을 알 수 있다. 변환행렬 또한 6자유도의 움직임을 16개의 값으로 표현하는 낭비를 하고 있다. 뭔가 더 콤팩트한 표현이 없을까?
2. 회전 행렬은 determinant가 1인 orthogonal matrix여야 한다는 존재 자체에 대한 기본적인 constraints를 가진다. 변환 행렬도 마찬가지 (R 부분이 위 constraints를 만족해야한다는걸 말하는건가?). 이 constraints는 회전/변환 행렬을 추정하거나 최적화하기 어렵게 만든다.

따라서, 회전이나 평행이동을 표현할 수 있는 좀 더 간단하고 압축적인 표현을 찾아볼 필요가 있다. 예를 들자면.. 회전을 3차원 벡터로, 변환을 6차원 벡터로 표현할 순 없을까? 회전을 회전축과 회전각으로 표현할 수 있다는 것은 명백하다. 따라서, 우리는 벡터-회전축과 평행한 방향이면서 길이가 회전각과 같은-를 사용할 수 있다. 이를 회전 벡터 (또는 angle-axis/axis-angle)이라 부른다. 이렇게 하면 3차원 벡터만으로 회전을 나타낼 수 있다. 비슷하게, 우리는 회전 벡터와 평행이동 벡터를 합쳐서 변환 행렬을 표현할 수 있다. 이렇게 되면 6차원이 된다. $\textbf{R}$이라고 표현되는 회전을 생각해보자. 이게 회전 벡터로 표현된거라면, 회전축이 단위길이의 벡터 $\textbf{n}$이고, 회전각이 $\theta$라고 가정했을 때, 벡터 $\theta\textbf{n}$또한 이 회전을 나타낼 수 있다(?). 그럼, 이 두 표현 간의 연결고리가 무엇인가? 사실, 이 둘 간의 관계를 유도하는 것은 매우 간단하다. 로테이션 벡터-행렬로의 변환은 로드리게스 수식 (Rodrigues' formula)을 이용하여 수행할 수 있다. 자세한 유도는 살짝 복잡하니까 생략하고, 변환 수식만 알려주겠다. (자세한 유도는 링크 참고: https://en.wikipedia.org/wiki/Rodrigues%27_ rotation_formula, 근데 어차피 뒤에서 리 대수 관점에서 유도 한번 해줄거) $$ \textbf{R} = \cos{\theta}\textbf{I} + (1-\cos{\theta})\textbf{n}\textbf{n}^T + \sin{\theta}\textbf{n}^{\wedge} $$ $^\wedge$ 기호는 벡터를 skew-symmetric으로 변환해주는 연산자이고, 앞에서 설명했다. 반대로 우리는 회전 행렬로부터 회전 벡터를 계산할 수도 있다. 코너 $\theta$에 대해, 양 변에 trace를 취하면, (뭔소리야?) $$ \text{tr}(\textbf{R}) = \cos{\theta}\text{tr}(\textbf{I})

• (1-\cos{\theta})\text{tr}(\textbf{n}\textbf{n}^T)
• \sin{\theta}\text{tr}(\textbf{n}^{\wedge}) \ = 3 \cos{\theta} = (1-\cos{\theta}) \quad\quad\quad\quad\quad\quad\quad\quad \ = 1+2\cos{\theta} \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad $$ 따라서, $$ \theta = \arccos\left({{\text{tr}(\textbf{R})-1}\over{2}}\right) $$ 축 $\textbf{n}$에 따라, 왜냐면 회전축은 회전 후에도 바뀌지 않으므로, $$ \textbf{R}\textbf{n} = \textbf{n} $$ 따라서, 축 $\textbf{n}$는 회전행렬 $\textbf{R}$의 고유값(eigenvalue) 1(??)에 해당하는 고유벡터이다. (왜?) 고유값, 고유벡터의 기하학적 의미 복습(아 그냥 정의상 당연한거지 참) 이 식을 풀고 노말라이즈 해주면 그 솔루션이 바로 회전축이다. 이렇게 알아본 두 변환 수식은 뒷장에서도 나올 것이고, 이는 $\text{SO}(3)$ 상에서 리 그룹과 리 대수 간의 간계와 정확히 일치한다는 것을 발견하게 될 것이다!

2.3.2 오일러각

그럼 이제 오일러각 얘기를 좀 해보자. 그게 회전 행렬이든 회전 벡터읻ㄴ, 그들이 회전을 묘사할 수 있다고 해도, 그 숫자들 만으로 회전을 상상하는 것은 어려운 일이다. 그 값들이 바뀌었을 때, 우리는 물체가 어떤 방향으로 돌지 모른다. 오일러각은 회전을 굉장히 직관적인 방식으로 묘사해준다. 오일러각은 3개의 기본축(primal axis)을 사용하여 회전을 각 축을 중심으로 한 3개의 회전으로 분해한다. 사람은 한 축 회전 프로세스는 쉽게 상상할 수 있으므로. 하지만, 분해 방법은 매우 다양하기 때문에 오일러각에 대해서는 매우 다양한 대체들과 헷갈리는 정의들이 존재한다. 예를 들어 우리는 일단 첫번째로 X축에 대해서 회전을 하고, 그 다음 Y축, 그리고 Z축 순- 즉 XYZ 순서로- 회전을 할 수도 있는데, 근데 ZYZ나 ZYX 등등도 가능하다. 그리고 우리는 회전이 완전 고정축에서 이루어지는지, 아니면 회전 이후의 축을 기준으로 이루어지는지도 구별해야한다.

이렇게 불명확한 축 순서로 인해 사용에 상당한 문제점들이 발생하는데, 다행히도 특정 연구 분야 끼리는 공통된 오일러각 정의를 사용한다. 여러분도 아마 "피치 각 (pitch angle)"이나 "요 각 (yaw angle)"과 같은 항공용어를 들어본 적이 있을텐데, 이것이 오일러각 중 가장 일반적으로 쓰이는 yaw-pitch-roll 각이다. 이건 ZYX축 순서의 회전을 사용하는데, 따라서 ZYX 오일러각을 예시로 들어보자. 강체의 정면 (우리를 향한)이 X축이라고 가정하고, 오른쪽 방향이 Y, 위쪽 방향이 Z축이라고 해보자. (그림 2-2를 보면 됨) 그러면, ZYX 각은 다음의 세 축을 따라 회전을 분해하는 것이다.

1. 우선은 z축을 기준으로 회전해서 요 각 $\theta_{yaw}=y$이도록 맞춰줌(?)
2. 그 다음 회전된 상태의 Y축을 기준으로 다시 회전해줘서 피치 각 $\theta_{pitch}=p$을 만들어줌
3. 마지막으로 회전된 상태의 X축을 기준으로 $\theta_{roll}=r$을 만들어주면 끝

이렇게 하면, 우리는 $[y, p, r]^T$와 같은 3차원의 벡터를 이용해 어떤 회전이든 표현할 수 있다. 그리고 이 벡터는 굉장히 직관적이다. 우리는 이 벡터를 보면 회전과정을 상상할 수 있다. 다른 오일러각도 순서에 따라 다양하지만, 위에서 본 ypr각이 제일 널리 쓰인다. 다른 오일러각은 축순서와 함께 언급된다. 예를들어 ypr각은 ZYX이고, XYZ, ZYZ와같은 오일러각도 있을 수 있지만 얘네는 ypr각 같은 별도의 이름은 없다. 각 분야마다 고유의 좌표방향과 오일러각 습관이 있다. 오일러각의 제일 큰 단점은 짐벌락(Gimbal lock)이라는 아주 유명한 문제이다. ypr 케이스에서, 만약 피치각이 +-90도이면, 처음 회전과 세번째 회전은 같은 축을 이용하게 되고, 그러면 시스템의 자유도가 3에서 2로 줄어들게 된다. 이를 singularity problem이라고 하고

SPH..파트너사

아래는 Xiang Gao와 Tao Zhang의 <Introduction to Visual SLAM>을 읽고 번역정리+추가적인 코멘트를 단 내용이다. 말은 가능하면 간소화하고, 이론적으로 더 참고할만한 부분은 추가적인 코멘트를 넣었음. 저자에 의해 모든 자료가 무료 배포되었음. 아래 링크 참고 📘 영어판 도서 파일: https://github.com/gaoxiang12/slambook-en 📂 practice 코드: https://github.com/gaoxiang12/slambook2

1장. Introduction to SLAM

🔎 학습 목표

1. Visual SLAM 프레임워크가 어떤 모듈들로 이루어지고, 각 모듈이 어떤 역할을 하는지 이해한다.
2. 프로그래밍 환경을 셋팅하고, 실험을 위한 준비를 완료한다.
3. 리눅스에서 프로그램을 어떻게 컴파일하고 실행하는지 이해한다. 만일 컴파일 단계에서 문제가 생긴다면 어떻게 디버깅하는지 배운다.
4. CMake의 기본 사용법을 배운다.

이번 강의에서는 따라오는 챕터에서 다룰 내용들의 개요를 잡는 느낌으로 visual SLAM 시스템의 구조를 요약 설명할 것이다. 실습 파트에서는 환경 설정과 프로그램 개발에 대한 기초를 설명한다. 깜찍한 "Hellow SLAM" 프로그램을 만들어보는 것으로 마무리!

1.1 "Little Carrot"을 만나보자

위는 Little Carrot이라는 이름을 가진 귀여운 로봇이다. 이 친구가 방 안에서 자유롭게 돌아다닐 수 있도록 자율주행 기능을 탑재해보자. 바퀴만으로도 돌아다니는 것은 가능하지만, 적절한 네비게이션 시스템 없이는 여기저기 부딫혀 물건을 망가뜨릴 수도 있다. 머리에 카메라 두개를 달아주어 Little Carrot이 두 눈과 머리, 사지가 있는 사람의 형상을 갖추도록 디자인해보자. 이제 Little Carrot은 사람처럼 자유롭게 돌아다니면서 환경을 탐색할 수 있을까? 그러기 위해서는 Little Carrot은 최소한 아래 두 가지를 스스로 알 수 있어야 한다.

• 내 위치가 지금 어디쯤인지 (=localization)
• 현재 나를 둘러싼 환경이 어떻게 생겼는지 (=map building)

이 두 문제를 풀기 위해선 바닥에 가이드 레일을 깔아 그 위만 주행하도록 한다든가, QR 코드같은 마커를 벽에 덕지덕지 붙인다든가, radio localization 기기를 테이블에 부착한다든가 하는 여러가지 접근 방법들이 있을 수 있다. 실외환경이라면 Little Carrot의 머리에 휴대폰이나 차량에 부착하는 GNSS 수신기를 설치해줄 수도 있을 것이다. 위 그림에서 보이는 여러가지 센서들은 침입형(intrusive)/비침입형(non-intrusive) 두 가지 카테고리로 나뉜다. 비침입형 센서는 로봇 몸체에 부착하기만 하면 환경과는 관계없이 활용 할 수 있는 휠 인코더, 카메라, 레이저 센서, IMU 등을 의미한다. 침입형 센서는 로봇이 아닌 환경에 설치해야하는 가이드 레일이나 QR 코드 등을 말한다. 침입형의 경우 설치가 가능하기만 하다면 localization 문제를 매우 간단하고 정확하게 해결할 수 있지만 범용성이나 사용성의 한계로 인해 활용에 제약이 생길 수 밖에 없다. 만약 GPS 수신을 받을 수 없는 상황이라거나 가이드 레일을 깔 수 없는 상황이라면 어떡할 것인가? 반대로 비침입형 센서의 경우 직접적인 위치 파악이 불가하고 간접적인 물리적 측정치만을 얻을 수 있다. 예를 들어 휠 인코더는 휠 회전각을 측정하고, IMU는 각속도와 각가속도를 측정한다. 카메라나 레이저 스캐너는 점군이나 이미지와 같은 특정 형식으로 외부환경을 관측한다. 우리는 이러한 간접적인 관측치로부터 위치를 추정하기 위해 여러 복잡한 알고리즘들을 적용해야 한다. 이렇게만 들으면 침입형 센서를 이용한 방식에 비해 엄청 돌아가는 것처럼 보이지만, 이 방식의 분명한 이점은 환경에 대한 요구사항이 없기 때문에 어떤 미지의 환경에서도 적용이 가능하다는 것이다. 이런 특성으로 인해 여러 연구에서 이 비침입형 센서를 이용한 localization 방식을 self-localization이라고 명명하기도 한다.

다시 앞에서 얘기한 SLAM의 정의로 돌아가보면, 우리는 사전 정보가 없는 미지의 환경을 탐색하는 것이 SLAM의 핵심이라고 말한 바 있다. 이론적으로는, Little Carrot이 놓일 환경이 어떤 환경일지 미리 추정하지 않아야 하므로 (물론 실제로는 실외인지 실내인지 정도의 대략적인 범위는 주어질 것이다) 우리는 GPS 같은 외부 센서가 잘 작동할 것이라고 가정해서는 안된다. 따라서 이 책에서는 비침입형 센서를 사용해 SLAM을 수행하는 것에 초점을 맞추며, 그 중에서도 우리는 visual SLAM에 대해서 다루고 있으므로 Little Carrot의 두 눈으로 무엇을 할 수 있는지를 중점적으로 알아 볼 것이다.

SLAM에서 쓰이는 카메라는 보통의 SLR (single-lens reflex) 카메라와는 차이가 있다. 훨씬 저가이며, 비싼 렌즈를 사용하지 않는다. 또한 일정 속도(보통의 경우 30fps)로 연사하여 연속적인 비디오 스트림 형태의 데이터를 생성한다. 카메라는 아래 그림과 같이 크게 세 가지 카테고리로 나뉜다; monocular (단안), stereo (양안), RGB-D. 이름에서 직관적으로 알 수 있듯이, 단안 카메라는 하나의 카메라만을 가지고 있고, 양안 카메라는 카메라 두 개로 이루어져있다. RGB-D 카메라의 원리는 좀 더 복잡한데, 컬러 이미지를 찍는 것 뿐만 아니라 각 픽셀에 대해 씬과 카메라 사이의 거리를 측정해준다. 자세한 원리는 lecture 5에서 다룰 것이다. 이 외에도 파노라마 카메라, 이벤트 카메라 등의 특별한 카메라 타입이 사용되기도 한다. 여러분은 우리가 Little Carrot의 머리에 박아준 카메라가 바로 양안 카메라임을 눈치챘을 것이다. 그럼 각 카메라 타입의 장단점을 살펴보자.

monocular (단안) 카메라

• 하나의 카메라만을 사용하는 SLAM 시스템을 monocular SLAM이라고 부른다. 적용이 단순하고, 또 저렴하기 때문에 자주 선택되는 옵션이다. 단안 카메라의 아웃풋 데이터는 한장의 사진이다. 이것으로 무엇을 할 수 있을까?
• 사진은 본질적으로 장면을 카메라의 이미지 평면에 투사한 projection이다. 사진을 찍는다는 것은 3차원의 세계를 2차원의 형태로 투영하는 것인데, 이 투영 과정에서 한 차원의 소실이 일어난다. 이때 잃어버리는 1차원은 depth(또는 distance) 정보를 담고 있는 차원이다.
• 따라서 단안 케이스에서 우리는 장면 내 물체와 카메라 사이의 거리값을 알 수 없는데, 뒤에서 보겠지만 이 거리값은 SLAM에서 매우 중요한 정보이다. 사람의 경우 내재된 사전 지식- 물체의 예상 크기 등-을 통해 보통 이미지 내에 있는 여러 물체 간의 거리 관계 파악이 가능하다. 하지만 아래 그림과 같은 특정 경우에서는 사람도 객체의 거리와 크기를 특정할 수 없다. 아래 사진에 보이는 작은 사람들이 멀리 떨어져 있는 큰 물체(인간)인지, 가까이 있는 작은 물체(장난감)인지는 보는 각도를 바꿔 3차원 구조에 대한 추가적인 정보를 더 얻기 전에는 알 수 없다.
• 단안 카메라를 통해 얻은 이미지는 3D 공간에 대한 2D projection 이기 때문에 3D 구조를 복원하고자 한다면 우리는 카메라의 view angle을 바꿔야한다. monocular SLAM은 이 원리를 사용한다. 카메라를 움직이고, 그 움직임을 추정하고, 그러면서 장면 내 객체의 거리와 크기-즉, 장면의 구조-를 파악하는 것이다. 그렇다면 움직임과 구조를 어떻게 추정할 수 있을까?
• 우리는 일상 속의 관찰을 통해 그 방법을 이미 알고있다. 카메라를 오른쪽으로 이동하면, 이미지 내의 객체는 왼쪽으로 이동한다. 카메라에서 가까운 물체는 카메라의 움직임에 따라 이미지 내에서 빠르게 이동하고, 먼 물체는 느리게 이동한다. 따라서, 카메라가 움직일 때 이미지 내의 물체들의 이동은 픽셀 시차(pixel disparity)를 형성한다. 이 시차를 측정함으로써, 우리는 어느 물체가 가깝고 먼지를 정량적으로 결정할 수 있게 된다. 하지만 만약 우리가 어떤 물체가 가깝고 먼지 안다고 해도 그 크기는 상대적인 값에 머무른다. 예를 들어 우리가 영화를 볼 때, 영화의 한 장면에서 어떤 물체가 다른 물체보다 크다고는 말할 수 있지만 그 물체의 실제 사이즈를 정확히 알 수는 없다. 직관적으로 생각해보면, 만약 카메라의 움직임과 장면의 크기를 동시에 두 배씩 키워도 단안 카메라는 똑같은 이미지를 찍을 것이다. 이는 monocular SLAM을 통해 추정한 경로(trajectory)와 맵은 실제 경로 또는 맵과 알려지지 않은 인수배의 차이가 있음을 의미하고, 우리는 이 인수를 스케일이라고 부른다. monocular SLAM은 이미지만을 통해서는 실제 스케일을 결정할 수 없으므로 이런 특성을 스케일 모호성(scale ambiguity) 이라고도 부른다.
• 정리하자면, monocular SLAM에서 depth값은 병진 운동(translational movement)을 통해서만 계산될 수 있고, 실제 스케일은 결정할 수 없다. 이 두가지 특성으로 인해 monocular SLAM의 실제 적용은 많은 문제가 생기기도 한다. 근본적인 원인은 하나의 이미지로는 depth를 결정할 수 없다는 것인데, 그래서 실제 스케일에 맞는 depth를 얻기 위해 우리는 stereo나 RGB-D 카메라를 쓰기 시작했다.

Stereo (양안) 카메라와 RGB-D 카메라

• 거리값을 알고 나면 단일 프레임으로부터 장면의 3D 구조를 복원하는 게 가능해지고, 스케일 모호성 또한 없어진다. Stereo와 RGB-D는 각각 다른 원리를 사용하여 실제 거리값을 계산한다.
• 스테레오 카메라는 두 개의 동기화 된 단안 카메라로 이루어져있다. 두 카메라 간의 물리적인 거리- 베이스라인(baseline) 이라고 부름-가 실제 값으로 주어지고, 이를 이용해 우리는 실제 사람의 눈과 매우 비슷한 원리로 각 픽셀의 3D 위치를 계산할 수 있다. 우리 인간은 왼쪽 눈과 오른쪽 눈으로 들어오는 이미지 간의 차이를 이용해 물체의 거리를 추정하는데, 같은 방식으로 아래 그림과 같이 각 픽셀의 거리값을 계산한다. 다만, 이 연산을 컴퓨터로 구현하면 꽤나 큰 연산량을 필요로 한다. 이러한 방식은 스테레오 뿐 아니라 multi-camera 시스템으로도 확장될 수 있지만 여러 대의 카메라로부터 구하는 결과라고 훨씬 정확하다거나 하지는 않다.
• 스테레오 카메라가 측정 할 수 있는 depth 범위는 베이스라인 길이와 관련이 있다. 베이스라인이 길면 더 멀리까지 측정할 수 있다. 따라서 보통 자율주행 기기에 부착되는 스테레오 카메라는 꽤 긴 베이스라인을 갖고있다.
• 스테레오 카메라의 단점은 configuration과 calibration 과정이 복잡하다는 것이다. 이들의 depth 범위와 정확도는 베이스라인 길이와 카메라 해상도(resolution)에 따라 제한된다. 또한, 앞서 말했듯이 스테레오 매칭과 시차 계산 과정은 많은 계산량을 필요로 하고 보통 GPU나 FPGA같은 가속기가 있어야 실시간 연산을 소화할 수 있다. 따라서 대부분의 스테레오 SOTA 알고리즘에서조차 계산비용이 여전히 주요 문제 점 중 하나로 남아있다.
• depth 카메라 (RGB-D 카메라)는 2010년 이후 부상한 새로운 카메라 타입이다. 레이저 스캐너와 비슷하게, RGB-D 카메라는 적외선이나 ToF(Time-of-Flight) 원리를 사용하여 물체에 빛을 쏘고 반사되는 빛을 수신하여 물체와 카메라 사이의 거리를 측정한다. 양안 카메라가 소프트웨어적인 해결법을 사용한다면, RGB-D 카메라는 물리적인 센서를 활용함으로써 계산 자원을 매우 절감하였다. 일반적으로 많이 쓰이는 RGB-D 카메라는 Kinect / Kinect V2, Xtion Pro Live, RealSense 등등이 있다. 하지만, RGB-D 카메라도 작은 FOV(field of view), 작은 depth 측정 범위, 데이터의 노이즈, 태양광 간섭에 민감하며 투명한 물체를 측정할 수 없다는 점 등 여러 이슈를 가지고 있다. SLAM에의 활용에 있어 RGB-D 카메라는 보통 실내환경에서 쓰이고 실외환경에는 적합하지 않다고 여겨진다.

이렇게 일반적인 카메라 타입들을 간단히 살펴보았다. 이제, 장면에서 움직이고 있는 카메라를 상상해보라. 우리는 카메라로부터 연속적으로 변화하는 일련의 이미지들을 얻을 것이다. Visual SLAM의 목표는 이 이미지들을 이용해 localization과 building map을 수행하는 것이다. 이건 꽤 복잡한 작업이기 때문에 하나의 알고리즘만으로 이미지를 입력받아 위치와 맵에 대한 정보를 연속적으로 출력하도록 만드는 것은 불가능하다. SLAM은 여러 알고리즘을 통합한 프레임워크를 통해 수행되는데, 수십년에 걸친 연구 끝에 아래와 같은 정형화된 형태가 확립되었다.

1.2 기본적인 Visual SLAM 프레임워크

1. 센서 데이터 취득 : Visual SLAM에서는 주로 카메라 이미지를 취득하고 전처리하는 과정을 일컫는다. 이동형 로봇의 경우 모터 인코더나 IMU 센서 등의 취득/동기화 또한 포함될 수 있다.
2. Visual Odometry (VO) : VO의 과제는 인접 프레임간 카메라의 움직임(ego-motion)을 추정하고 대략적인 부분 맵(local map)을 생성하는 것이다. VO는 프론트엔드라는 용어로 표현되기도 한다.
3. 백엔드 필터링/최적화 : 백엔드는 VO로부터 각각의 time stamp에서의 카메라 포즈를 전달받고 루프 클로징의 결과를 전달받아 최적화를 적용하여 전체적으로 최적화된 경로와 맵을 만들어준다. VO 다음에 연결되기 때문에 백엔드로 불린다.
4. 루프 클로징 : 루프 클로징은 로봇이 이전의 위치로 되돌아왔는지 여부를 알아내는 부분이다. 이를 탐지하면 축적되는 드리프트 에러 (drift error)를 감소시킬 수 있다. 만약 루프가 감지되면, 이 정보를 백엔드로 전달해 최적화에 활용할 수 있도록 한다.
5. 환경 재건(reconstruction) : 추정된 카메라의 trajectory를 바탕으로 task-specific한 맵을 구성한다.

위 프레임워크는 몇십 년간의 시행착오 끝에 정착된 것으로, 사실 안정적인 조명 조건과 제한되고 고정된 환경과 사람의 특별한 방해가 없는 경우를 상정한다면 visual SLAM 문제는 이미 해결되었다고 봐도 된다. (문제는 현실에서는 이런 가정이 통하지 않는다는 것😂)

이제 각 모듈의 기능에 대해 자세히 알아볼 건데, 작동 원리를 더 깊이 이해하려면 여러 수학적 지식이 추가로 필요하며 이는 책의 part 2에서 자세히 다룰 것이다. 지금은 각 모듈에 대한 직관적이고 정성적인 이해에 집중해보자.

Visual Odometry (VO)

VO는 인접 이미지 프레임 사이의 카메라 움직임을 파악하는 작업으로 가장 간단한 경우로는 앞뒤 연속 이미지 사이의 움직임을 추정하는 일이 될 것이다. 아래 두 이미지를 보면 우리는 자연히 오른쪽 이미지를 왼쪽으로 살짝 회전하면 왼쪽의 이미지가 될 것이라는 것을 알아챌 수 있다 (비디오로 보았으면 좀 더 쉬웠을 것이다). 이때 여러분은 '왼쪽으로 살짝 돌린다'는 움직임을 어떻게 두 이미지를 통해 알아챘는가? 아마 자연스럽게 이런 생각을 했을 것이다. “보자, 바테이블은 지금 가깝고 벽이랑 블랙보드는 멀리 떨어져있지. 카메라가 왼쪽으로 돌면 바테이블의 가까운 부분이 나타나기 시작할거고, 오른쪽의 캐비닛은 이미지 밖으로 나가기 시작하겠지?” 이 정보들로 우리는 카메라가 왼쪽으로 돈다고 결론짓게 된다. 여기서 더 나아가서, 여러분은 카메라가 얼만큼 돌았는지 또는 이동했는지 각도나 센티미터 단위로 대답 할 수 있는가? 이는 인간에게도 어려운 문제이다. 왜냐하면 우리 직관은 숫자에는 약하기 때문이다. 하지만 컴퓨터에게는, 이런 움직임은 숫자로 설명되어야한다. 그렇다면 어떻게 컴퓨터가 이미지들만으로 카메라의 움직임을 계산하게 할 수 있을까? Visual SLAM에서 주어지는 것은 이미지 내 픽셀들의 값과 그들이 공간의 한 점을 카메라의 이미지 평면으로 투영한 결과라는 사실 뿐이다. 카메라의 움직임을 정량화하기 위해서는 첫째로 카메라와 공간상의 점들 간의 기하학적 관계를 이해해야 한다. 이 관계를 밝히기 위해서는 약간의 배경지식이 필요하지만, 여기서는 우선 직관적인 개념만을 이해해도 충분하다. 현재로서는 VO가 인접 프레임의 이미지들로부터 카메라 움직임을 추정하고 장면의 3D 구조를 복원할 수 있다는 내용만 가져가면 된다. 이름에 odometry가 들어가는 이유는, VO가 실제 휠 오도메트리 (휠 인코더를 이용한)와 유사하게 순간순간의 ego-motion만 계산하고 글로벌 맵이나 절대 포즈 (absolute pose)를 추정하지 않기 때문이다. VO는 금붕어같이 금방금방 까먹고 전체적인 구조정보를 사용하지 않는다.

이제 우리가 VO를 구현을 해놔서, 지금 모든 앞뒤 프레임 간 카메라 움직임을 추정할 수 있다고 가정해보자. 만약 우리가 인접한 움직임끼리 모두 연결한다면 이는 자연히 로봇의 전체 경로를 나타내게 될 것이고 그럼 localization 문제는 끝이다. 또, 우리는 각 시간의 카메라 위치에서의 모든 픽셀들의 3D 위치를 계산할 수 있게 되고, 이것을 통합하면 맵을 형성할 수 있다. 여기까지 보면 VO만으로 SLAM 문제가 거의 해결된 것 처럼 보인다. 정말 그럴까? VO는 visual SLAM 문제를 풀기 위한 핵심 기술이 맞다. 우리는 이 책에서 VO에 대한 자세한 설명을 제공하기 위해 꽤 많은 부분을 투자할 것이다. 하지만, VO 만을 사용해 경로를 추정하는 것은 필연적으로 드리프트의 축적을 수반한다. 각 추정에는 항상 일정 오차가 따라온다. odometry가 작동하는 방식 때문에 이전 순간의 오차는 다음 순간으로 그대로 전파되고, 일정 시간이 지나면 추정이 부정확해지게 된다. 예를 들어, 로봇이 처음에 왼쪽으로 90도 돌고 그 다음 오른쪽으로 90도 돌았다고 해보자. 오차로 인해 우리는 처음 90도의 회전을 89도로 추정했다. 그러면 로봇이 다시 오른쪽으로 회전했을 때 로봇의 추정된 위치가 원점이 아니라는 사실이 우리를 당황스럽게 만들 것이다. 더 기분 나쁜 사실은, 이후의 모든 추정이 완벽하게 정확하게 이루어져도 이 처음의 1도 실수는 지워지지 않고 모든 경로상에 반영될 것이라는 점이다!

축적되는 드리프트는 일관성있는 맵을 구축할 수 없도록 만든다. 일자 복도는 비스듬해지고, 직각 코너는 비뚤어지게 된다🤮 드리프트 문제를 해결하기 위해 우리는 두가지 요소가 추가로 필요한데, 바로 뒤에 나오는 백엔드 최적화와 루프 클로징이다. 루프 클로징은 로봇이 이전 위치로 돌아왔는지를 판별해주고, 백엔드 최적화는 이런 정보들을 바탕으로 전체 경로의 모양을 교정해주는 역할을 한다.

백엔드 최적화

제너럴하게 말하자면, 백엔드 최적화는 SLAM 시스템에서 노이즈를 처리하는 부분을 맡고 있다고 할 수 있다. 모든 센서 데이터가 정확하면 정말 좋겠지만, 실제로는 가장 비싼 센서조차도 일정량의 노이즈를 항상 포함한다. 거기에 더해 많은 센서의 성능은 주변의 자기장이나 온도 등에 많은 영향을 받는다. 따라서 이미지로부터 카메라의 움직임을 추정하는 문제를 푸는것에 더해 우리는 이 추정이 어느정도의 노이즈를 포함하고 있는지, 이전 시간으로부터 다음 시간까지 이 노이즈가 어떻게 전파되어오는지, 현재 추정치를 어느정도 신뢰할 수 있는지를 추가적으로 고려해야한다. 백엔드 최적화는 노이즈를 포함한 인풋 데이터로부터 전체 시스템의 상태(state)를 추정하고 그들의 불확실성(uncertainty)을 계산한다. 여기서 말하는 상태는 로봇 자체의 경로와 환경에 대한 맵 둘 모두를 포함하는 개념이다.

SLAM 프레임워크에서 프론트엔드는 백엔드에 최적화할 데이터와 그 초기값을 전달한다. 백엔드는 넘겨받은 데이터의 최적화만을 진행하기 때문에, 이미지랑은 이제 관계가 없고 전달받은 숫자와 행렬들과만 관련이 있다. 따라서 visual SLAM에서 프론트엔드는 컴퓨터 비전의 영역과 맞닿아 있고- feature 추출과 매칭 등- 백엔드는 상태 추정 이론(state estimation theory)과 밀접한 관련이 있다고 할 수 있다. 역사적으로 오랫동안 백엔드의 최적화 파트 자체가 SLAM 연구의 동의어였다. 초기 SLAM 문제는 지금의 백엔드가 해결하려고 하는 것과 정확히 일치하는, 상태 추정에 대한 문제 그 자체로 설명되었다. 초기 논문에서 이는 “estimation of spatial uncertainty”로 명명되었으며, 이는 SLAM의 근본적인 특성- 자기 자신의 움직임과 주변 환경에 대한 uncertainty를 추정하는-을 직접적으로 설명하는 용어였다. 우리는 상태 추정 이론을 이용해 상태에 대한 mean과 covariance(=uncertainty)를 추정하기 위해 다양한 필터나 비선형 최적화 등을 사용한다. 자세한 이론적 내용은 5장, 8장 및 9장에서 설명한다.

루프 클로징

loop closure detection으로도 알려진 루프 클로징 모듈은 주로 SLAM에서의 위치 추정에서 발생하는 드리프트 에러를 해결하는 역할을 한다. 로봇이 일정 시간 이동 후 원점으로 돌아왔다고 가정했을 때, 실제로 우리가 추정하는 마지막 위치는 드리프트로 인해 원점이 아닐 것이다. 이 오차를 어떻게 교정해줄 수 있을까? 만약 로봇에게 그것이 사실은 현재 원점으로 돌아왔다는 것을 알려줄 방법이 있다면, 우리는 현재 위치를 원점으로 끌어다 놓음으로써 드리프트를 제거할 수 있을 것이다. 이게 정확히 루프 클로징이 하는 일이다.

루프 클로징은 localization과 map building 둘 모두와 깊은 관계가 있다. 사실, 맵을 구축하는 주된 목적 중 하나가 로봇이 자신이 다녀온 장소를 알 수 있도록 하는 것이기도 하다. 루프 클로징을 가능케 하기 위해선 로봇이 이전에 방문한 장면을 식별할 수 있도록 해주어야 하는데, 예를 들자면 로봇이 출발하는 곳에 QR 코드와 같은 마커를 설치하는 식의 방법을 생각해 볼 수 있다. 만약 동일한 마커를 다시 목격한다면 우리는 로봇이 원점으로 돌아왔다는 것을 알 수 있다. 하지만 마커는 앞서 말했듯이 침입형 센서이다. 비침입형 센서만으로 로봇이 루프 클로징을 수행하려면 어떻게 해야 할까? 가능한 방법 중 하나는 이미지 간의 유사도를 측정하는 것이다. 사람은 비슷한 두 이미지를 보고 두 이미지가 같은 장소에서 찍힌 것인지 아닌지 쉽게 판별할 수 있다. 따라서 우리가 살펴볼 시각적 루프 탐지는 이미지 간 유사도를 측정하는 알고리즘이라고 봐도 된다.

루프가 탐지되고 나면, 우리는 백엔드 최적화 알고리즘에게 “자, A랑 B는 같은 점이야!”와 같은 정보를 전달하게 된다. 이 정보를 바탕으로 전체 경로와 맵은 A와 B를 한 점으로 잇도록 조정될 것이다. 이런 방식을 통해, 충분하고 신뢰할 수 있는 루프 탐지를 가지고 있다면 축적된 에러를 제거하고 전체적으로 일관성있는 경로와 맵을 얻을 수 있게 된다.

매핑

매핑은 맵을 구축하는 과정을 뜻하는데, 맵이라는건 아래 그림과 같이 환경에 대한 묘사를 의미하고 이 묘사는 정해져 있는 것이 아니라 실제 용도에 따라 어떤 형태든 될 수 있다. 로봇 청소기를 예로 들어보자. 로봇 청소기는 바닥에 붙어서 움직이기 때문에 네비게이션을 위해서는 single-line 레이저 스캐너를 이용해 만든 2D 맵이면 충분할 것이다. 카메라의 경우에는 카메라의 6-DoF(degree-of-freedom) 움직임을 커버할 수 있는 최소 3차원의 맵이 필요할 것이다. 가끔 우리는 단순한 점들의 집합이 아닌 부드럽고 아름다운 삼각면의 텍스쳐들로 이루어진 모델링 결과가 필요할 수도 있다. 또는, 생김새는 전혀 신경쓰지 않고 “점 A와 점 B는 연결되어있고, 점 B와 점 C는 연결되어있지 않군” 과 같은 위상적인(topological) 정보만을 필요로 할 수도 있다. 아니면 가끔은 맵 자체가 아예 필요하지 않을 수도 있다. 예를 들어 level 2 자율주행차는 차선에 상대적인 움직임만을 추정함으로써 차선을 따르며 주행할 수 있다. 이렇게 맵에 대한 요구조건이나 아이디어는 용도에 따라 다양한 모습을 띤다. 앞서 말한 VO나 루프 탐지, 백엔드 최적화와는 다르게 매핑은 특정 알고리즘을 가지고 있지 않다. 여러 형태가 될 수 있는 이 맵이라는 개념은 크게는 두가지 카테고리로 나뉜다.

• Metric Map
  • Metrical map은 물체의 정확한 미터법상 위치를 강조한다. 보통 sparse하냐 dense하냐로 분류되기도 한다. sparse metric map은 장면을 모든 물체를 나타내지 않고 간결한 형태로 나타내어 저장한다. 예를 들자면 도로환경에서 차선이나 교통신호 등의 대표적인 랜드마크들만을 선택하여 sparse map을 구성할 수 있다. 반대로 dense metrical map은 보이는 모든 것을 모델링하는데 초점을 맞춘다. sparse map은 localization에는 충분할 수 있지만 navigation을 위해서는 보통 dense map이 필요하다 (그렇지 않다면 우리는 아마 두 랜드마크 사이의 벽에 콰당 부딪힐 수도 있을 것이다). dense map은 보통 특정 해상도의 작은 격자들로 구성된다. 예를 들자면 2D의 경우 occupancy grid의 형태를 사용 할 수 있고, 3D의 경우 voxel grid를 사용 할 수 있다.
  • 이 맵은 A*, D*와 같은 다양한 navigation 알고리즘에 사용될 수 있다. 하지만 공간 크기를 모두 커버하는 격자 구조에 모든 상태를 저장하는 이런 방식은 저장공간을 많이 잡아먹는다. 이외에도 다른 여러 이슈들이 존재한다. 예를 들어 큰 규모의 metrical map에서 매핑 시의 작은 에러가 두 방 사이의 벽이 겹쳐져버리도록 만들 수도 있다.
• Topological Maps
  • metric map과 비교했을 때, topological map은 맵 요소 간의 관계에 조금 더 집중한다. topological map은 노드와 엣지로 이루어진 그래프 구조이며, 각 노드 간의 연결성만을 고려한다. 예를 들자면 실제로 점 A와 점 B가 얼마나 떨어져있는지는 궁금하지 않고 두 점이 연결되어있는지 여부만이 중요한 상황이 있을 수 있다. 이는 정확한 위치를 구축하는 것에 대한 부담을 덜어주고, 맵에서 디테일을 제거함으로써 좀더 콤팩트한 표현을 가능하게 한다.
  • 하지만 이런 topological map은 복잡한 구조를 나타내기엔 좋지 않다. 또한 노드와 엣지를 형성하기 위해 맵을 쪼개는 방법과 navigation/path planning을 위해 topological map을 어떻게 활용할지에 대한 질문은 여전히 답이 정해지지 않은 문제이다.

1.3 SLAM 문제를 수학적 표현을 사용해 나타내보자

Little Carrot이 몇가지 센서를 장착하고 미지의 환경에서 돌아다니는 상황을 수학적인 언어로는 어떻게 나타낼 수 있을까? 우선 센서들은 개별 time point에서 데이터들을 모으기 때문에, 우리는 그 순간순간의 맵과 위치만을 고려한다. 즉, 움직임이라는 연속적인 과정을 우리는 데이터 샘플링이 일어나는 불연속적인 time step ($1,...,k$) 상에서 해석한다는 말이다. $\textbf{x}$를 사용해 Little Carrot의 위치를 나타낸다면, 각 time step에서의 위치는 $\textbf{x}_1$,..., $\textbf{x}_k$와 같이 표현될 수 있을 것이고, 이들은 Little Carrot의 경로를 이루게 된다. 맵의 경우엔, 우리는 맵이 몇 개의 랜드마크들로 구성되어있다고 가정한다. 각 time step에서 센서들은 랜드마크들의 일부를 볼 수 있고, 그 관찰(observation)을 기록한다. $\textbf{y}_1$,..., $\textbf{y}_N$를 사용해 맵 상 존재하는 총 N개의 랜드마크를 표현하도록 하자.

이 설정 하에, 'Little Carrot이 몇가지 센서를 장착하고 미지의 환경에서 돌아다니는 상황'은 기본적으로 다음 두 부분으로 이루어져있다.

1. motion: time step $k-1$ -> $k$ 사이에 Little Carrot의 위치 $\textbf{x}$가 어떻게 변하는지
2. observation: Little Carrot이 위치 $\textbf{x}_k$에서 $\textbf{y}_j$라는 랜드마크를 탐지하는 상황

motion equation

로봇에게 "왼쪽으로 15도 돌아"와 같은 모션 명령을 전달하면 이 명령은 컨트롤러를 통해 여러 방식으로 수행된다. 우리는 로봇의 위치에 대한 명령을 내릴 수도 있고, 가속도나 각속도 등을 조종할 수도 있다. 어쨌든 이런 다양한 컨트롤의 종류에 관계 없이 우리는 다음의 보편적이고 추상적인 수학 모델을 이용해 로봇의 모션을 나타낼 수 있다. $$ \textbf{x}k = f(\textbf{x}{k-1}, \textbf{u}_k, \textbf{w}_k) $$ 여기서 $\textbf{u}_k$는 인풋 명령을 의미하고, $\textbf{w}_k$는 노이즈를 의미한다. 여기서 우리는 특정 수식이 아닌 제너럴한 표현인 $f(\cdot)$를 사용해 프로세스를 표현함으로써 어떤 모션 인풋이든 나타낼 수 있도록 하였다. 이 수식을 _motion equation_이라고 한다. 노이즈의 존재는 이 모델을 확률 모델(stochastic model)로 바꾼다. 즉, 우리가 “앞으로 1미터 직진”이라는 명령을 한다고 해도 그게 로봇이 실제로 정확히 1미터 갈 거라는걸 의미하진 않는다. 만약 모든 지시가 정확하게 수행된다면 사실 추정 작업 자체가 필요가 없지만, 로봇이 언제는 0.9미터를 가고 언제는 또 1.1미터를 가는게 현실이다. 따라서 각 움직임에서 노이즈는 랜덤하게 적용된다. 만일 우리가 노이즈를 무시한다면, 인풋 명령들로만 추정한 포지션은 불과 몇 분 뒤 실제 위치와 몇백 마일 떨어지게 될 수도 있다.

observation equation

motion equation과 비슷하게 observation equation이라는 것이 있는데, 이 수식은 Little carrot이 위치 $\textbf{x}k$에서 랜드마크 $\textbf{y}_j$를 보고 관측 데이터 $\textbf{z}{k,j}$를 생성하는 상황을 묘사한다. 이 관계 또한 일반성을 잃지 않도록 추상적인 함수 $h(\cdot)$를 이용해 표현해보자. $$ \textbf{z}{k,j} = h(\textbf{y}_j, \textbf{x}_k, \textbf{v}{k,j}) $$ 여기서 $\textbf{v}_{k,j}$는 observation의 노이즈이다. 여러 센서 종류에 따라 관측 데이터 $\textbf{z}$와 observation eq. $h$는 다양한 형태가 될 수 있다.

여러분은 아마 "아니, 그래서 $f$, $h$는 정확히 뭐고 $\textbf{x}$, $\textbf{y}$, $\textbf{z}$는 뭘 나타내는거야.."라며 모호한 표현에 혼란을 느끼고 있을 지 모른다. 실제 적용에서는 모션이나 센서의 종류에 따라 몇가지 종류의 특정한 매개변수화(parameterization) 방법이 존재한다. 매개변수화란 다음과 같은 것이다. 로봇이 평면 상에서 움직인다고 할 때 로봇의 포즈는 2차원의 $x-y$ 좌표와 각도로 표현될 수 있다. 즉, 이 상황에서 로봇의 포즈 $\textbf{x}k$는 두 축 상에서의 위치 $x_1$, $x_2$와 각도 $\theta$를 통해 $\textbf{x}_k = [x_1, x_2, \theta]^T_k$와 같이 나타내어질 수 있다. 이 때, 인풋 명령은 단위시간 사이에서 이 위치와 각도의 변화량($\triangle$)으로 해석될 수 있으므로, $\textbf{u}_k=[\triangle x_1, \triangle x_2, \triangle \theta ]^T_k$와 같은 형태로 표현해보자. 그러면 motion equation은 다음과 같이 매개변수화된다. $$ \left[ \begin{matrix} x_1\x_2\ \theta \end{matrix} \right]_k = \left[\begin{matrix} x_1\x_2\ \theta \end{matrix} \right]{k-1} + \left[ \begin{matrix} \triangle x_1\\triangle x_2\ \triangle \theta \end{matrix} \right]_k + \textbf{w}_k $$ 여기서 $\textbf{w}_k$는 앞에서 말했듯이 노이즈이다. 위 식은 간단한 선형관계를 나타낸다. 하지만 모든 인풋 명령이 이런식으로 위치와 각도의 변화량으로만 구성되어있진 않다. 예를 들어, 스로틀이나 조이스틱의 인풋은 속도나 가속도이므로 위와는 다른 형태의 조금 더 복잡한 motion equation을 갖게 된다. 그 때는 역학에 대한 추가적인 지식이 필요하다.

observation equation의 경우에는 예를 들어 로봇이 2차원의 레이저 센서를 사용한다고 해보자. 레이저는 로봇과 랜드마크 사이의 거리 $r$과 각도 $\phi$를 측정함으로써 2D 랜드마크를 관측한다. 랜드마크가 $\textbf{y}j =[y_1,y_2]^T_j$에 있고, 로봇의 포즈가 $\textbf{x}_k=[x_1,x_2]^T_k$, 관측데이터를 $\textbf{z}{k,j}=[r_{k,j}, \phi_{k,j}]^T$로 놓으면 observation eq.는 다음과 같이 표현된다: $$ \left[ \begin{matrix} r_{k,j}\ \phi_{k,j} \end{matrix} \right] = \left[ \begin{matrix} \sqrt{((y_{1,j}-x_{1,k})^2 + (y_{2,j}-x_{2,k})^2} \ \arctan \left({y_{2,j}-x_{2,k}}\over {y_{1,j}-x_{1,k}}\right) \end{matrix} \right] + \textbf{v}_{k,j} $$ visual SLAM으로 한정해 생각해보면, 센서는 카메라이고 observation eq.는 '랜드마크에 대한 이미지의 픽셀을 가져오는' 과정으로 생각해 볼 수 있다. 이는 카메라 모델에 대한 수식과 관련되어있고, chapter4에서 자세히 다뤄볼 것이다.

확실히 motion/observation 이 두 식은 센서 별로 매우 다른 형태로 매개변수화된다는 것을 알 수 있는데, 만약 우리가 범용성을 유지하면서 이들을 일반적인 abstract form으로 표현해본다면 SLAM 프로세스는 다음의 두 기본식으로 요약 가능하다. $$\begin{cases} \textbf{x}k = f(\textbf{x}{k-1}, \textbf{u}k ,\textbf{w}_k), \quad k = 1,...,K \ \textbf{z}{k,j} = h(\textbf{y}j, \textbf{x}_k, \textbf{v}{k,j}), \quad (k,j) \in \mathcal{O} \end{cases} $$ 여기서 $\mathcal{O}$는 랜드마크를 관측했던 위치에 대한 정보를 담고있는 집합(모든 순간에 랜드마크 관측이 가능하진 않다- 보통 한 순간에 랜드마크의 작은 일부만을 관측할 수 있음)이다. 이 두 식은 동시에 SLAM의 기본적인 문제를 묘사한다: “어떻게 노이즈가 포함된 인풋 컨트롤 $\textbf{u}$와 센서 관측값 $\textbf{z}$를 가지고 $\textbf{x}$(localization)와 $\textbf{y}$(mapping)를 정확하게 추정할 수 있을까?” 지금 보이듯이 우리는 SLAM 문제를 상태 추정 문제로 모델링하였는데, 그럼 어떻게 노이즈가 포함된 측정값들을 통해 내부적인(숨겨진) 상태 변수들을 추정할 수 있을까? 해법은 두 식의 특정한 형태와 noise의 확률분포에 있다. motion / observation eq. 가 선형이거나/비선형이거나, 노이즈가 gaussian/non-gaussian 으로 가정된다면, 모델은 선형/비선형, gaussian/non-gaussian 시스템으로 분류된다. 선형-가우시안(LG system) 은 가장 간단하고, 이것의 편향되지않은 최적의 추정치는 칼만 필터 (KF)를 통해 구할 수 있다. 복잡한 nonlinear non-gaussian(NLNG system)의 경우엔 기본적으로 extended Kaman filter (EKF), nonlinear optimization 두 가지 방법에 의존한다. 21세기 초반까지는 SLAM에서 EKF기반 방식이 지배적이었다. 이에 따라, EKF의 단점(부분 선형화에 따른 오차라든가 noise를 가우시안 분포로 추정하는 것 등)을 극복하기 위해 사람들은 입자 필터와 같은 다른 필터들을 사용하기 시작했고, 비선형 최적화 방법도 쓰기 시작했다. 오늘날 visual SLAM은 SOTA 최적화 방식인 그래프 최적화를 사용하는 것이 주류가 되었다. 현재로서는 최적화 방법이 명백하게 필터 기반 방식보다 우월하다고 생각되며, 컴퓨팅 파워가 받쳐주는 한 최적화 방법이 보통 선호된다(챕터8, 9).

여기까지 왔으면 SLAM의 수학적 모델에 대한 전반적인 이해는 되었으리라 생각하지만, 여전히 분명하게 해야 할 이슈 몇가지가 있다. 첫째로, 우리는 포즈 $\textbf{x}$가 무엇인지 엄밀하게 정의해야 한다. 포즈라는 단어는 조금 모호하다. 앞에서는 계속 2D의 예시를 들었지만 많은 로봇은 3차원의 공간 상에서 움직인다. 3차원 공간에서의 모션은 3개의 좌표축 상의 평행이동과 회전으로 표현되고, 이는 총 6개의 degree of freedom (DoF)을 가진다. 그럼 6차원의 벡터로 표현된다는건가 하면 그것보다는 살짝 더 복잡하다. 그럼 6DoF 포즈를 어떻게 표현해야 하고, 그걸 어떻게 최적화할 것인가? 이를 어떻게 수학적 성질을 이용해 표현할 것인가? 이 내용이 chapter 2,3에서 주요하게 다룰 내용이다. 다음으로, 우리는 visual SLAM에서 observation equation이 어떻게 매개변수화되는지를 다룰 것이다. 다른 말로, 어떤 과정을 통해 공간 상의 랜드마크 점이 사진에 투영되는지를 살펴볼 것이다. 이를 위해서는 카메라의 투영 모델과 왜곡에 대한 설명이 필요하며, 챕터 4에서 다룰 것이다. 마지막으로, 우리가 이 모든 정보를 알고있을 때 상태 추정 문제를 풀기 위해 필요한 비선형 최적화에 대한 지식을 챕터 5에서 다룰 것이다. 위의 내용이 이 책에서 수학적인 배경지식을 설명하는 부분인 파트1을 이루고, 나머지 파트에서는 visual odometry나 백엔드 최적화 등등에 대한 자세한 내용을 살펴본다.

1.4 실습: Basics

1.4.1 Ubuntu 설치하기

이런건 생략..

1.4.2 Hello SLAM

1.4.3 CMake 사용법 익히기

1.4.4 라이브러리 사용하기

리눅스에서 라이브러리 파일은 정적 라이브러리와 공유 라이브러리 두 개로 나뉜다.

정적 라이브러리는 “.a” 확장자를 가지고 있고, 공유 라이브러리는 “.so” 확장자를 가진다. 모든 라이브러리는 포장된 함수들의 콜렉션이다. 차이점은 정적 라이브러리는 그들이 불러질 때마다 카피를 생성하고, 공유 라이브러리는 하나의 카피만을 만든다는 것이다. 만약 정적 라이브러리 대신 공유 라이브러리를 생성하고싶으면 CMakeLists.txt에서 add_library를 할 때 SHARED 플래그를 추가해주면 된다.

1.4.5 IDE 사용하기

IDE란 Integrated Development Environments의 약어로, 파이참이나 VS Code 등의 개발을 도와주는 툴을 말한다. 기본적인 text editor만 있어도 코딩은 가능하지만 각종 자동완성부터 시작해 디버깅까지 다양한 지원기능을 활용하며 효율적인 개발을 하기 위해서는 개발에 없으면 안되는 존재인데, 이 세상에는 굉장히 다양한 IDE가 존재한다. 이 책에서는 KDevelop, CLion을 추천하는데 사실 나는 처음 들어보는 IDE이고, 개인적으로는 그냥 VSCode 쓰는 것을 추천한다. Extension이 넘치기 때문에 저자들이 말하는 KDevelop, CLion의 특장점을 커버하고도 남을거라 생각한다.

나 또한 회사에서 Visual SLAM 기술 개발을 계속 맡고 있고 논문도 썼지만서도 광활한 배경 이론의 필드를 헤매며 쌓아올린 지식이 여전히 빈공간이 많은 모래성 같다는 찝찝함을 지우기 힘들었다. 그러던 와중 오로지 Visual SLAM이라는 주제로 한정해 그 안에서 활용되는 모든 개념을 딱 그만큼의 범위로 집약해놓은 좋은 요약서를 발견하게 되어 한 번 이론적인 토대를 재점검하고 재구조화하는 시간을 가지기로 했다. 동시에 같은 분야를 공부하는 분들께도 참고 자료로 활용이 되었으면 하는 마음에 혼자 읽고 이해하는데 그치지 않고 열심히 포스팅을 해보려고 한다. 얼마나 걸릴지 정확히는 모르겠지만, 꾸준히 업로드 하다보면 언젠가 마지막 장을 넘길거라 생각하면서 첫 포스트 시작!

아래는 Xiang Gao와 Tao Zhang의 <Introduction to Visual SLAM>을 읽고 번역정리+추가적인 코멘트를 단 내용이다. 말은 가능하면 간소화하고, 이론적으로 더 참고할만한 부분은 추가적인 코멘트를 넣었음. 저자에 의해 모든 자료가 무료 배포되었음. 아래 링크 참고 📘 영어판 도서 파일: https://github.com/gaoxiang12/slambook-en 📂 practice 코드: https://github.com/gaoxiang12/slambook2

0. 서문

0.1 이 책은 무슨 내용을 다루고 있을까❓

• SLAM (Simultaneous Localization and Mapping) : 특정 센서들을 갖춘 로봇(또는 움직이는 강체)이 사전 정보 (priori information) 없이 자신을 둘러싼 주변 환경을 모델링하고, 동시에 그 환경 속에서 자신의 모션을 추정하는 작업을 의미한다. 그 중에서도 카메라 센서를 메인으로 사용하여 이를 수행하는 경우를 Visual SLAM 이라고 한다.

일단, 위 설명에서 나타나듯이 SLAM은 기본적으로 localization과 map building 이라는 두 가지 문제를 동시에 푸는 것을 목표로 한다. 즉, 센서 자신의 위치를 추정하면서 동시에 자신을 둘러싼 환경에 대한 모델을 추정하는 것 까지 해결해야 하는 것이다. 이를 위해서는 센서 데이터에 대한 상당한 지식이 필요하다. 여러 다른 센서는 그들만의 특정한 형태로 외부세계를 관측하는데, 이를 활용하기 위해서는 각기 다른 종류의 관측치들을 해석하는 각각의 접근법을 알아야만 한다. 여기에 더해 아무런 사전정보도 주어지지 않는다는 기본 가정과 실시간성을 만족해야한다는 점이 SLAM의 까다로운 부분이다. visual SLAM의 경우 우리는 연속적인 이미지 세트(=비디오)를 가지고 경로와 맵을 추정해나가야한다.

말만 들으면 어느정도 직관적인 문제로 보이기도 한다. 우리가 낯선 환경에 입장하게 되었을 때 하는 게 똑같은 일 아닌가? 문제는, 우리가 프로그램을 작성해서 컴퓨터가 그 똑같은 일을 하게 할 수 있냐는 것이다. 컴퓨터 비전이 막 탄생했을 때, 사람들은 언젠가는 컴퓨터가 인간처럼 행동할거라- 세상을 보고, 관찰하고, 주변 환경을 이해하는- 생각했고, 처음에는 이것이 그리 불가능하거나 어려운 목표처럼 여겨지지 않았다. 하지만 연구가 진행되면 될수록 드러나는 사실은 컴퓨터가 인간의 사고를 흉내내는 것은 절대 쉬운 일이 아니라는 것이었다. 꽃, 나무, 벌레, 새, 동물들은 컴퓨터에서는 너무나 다르게 기록되었다: 그것들은 컴퓨터 안에서는 그냥 숫자로 이루어진 행렬일 뿐이었다. 컴퓨터가 이미지의 내용을 이해하도록 만드는 것은 우리 사람이 그 숫자 덩어리를 이해하는 것 만큼이나 어려운 일이었다. 하지만 몇십 년에 걸친 고군분투 끝에 연구자들은 드디어 성공의 실마리를 잡기 시작했다. 인공지능(AI)과 머신러닝(ML) 기술에 힘입어 컴퓨터가 물체, 얼굴, 목소리, 텍스트를 조금씩 인식할 수 있게 된 것이다 (물론 우리 사람의 사고과정과는 여전히 다르지만).

한편으론, 거의 30년에 달하는 발전 과정을 거치며 SLAM에서도 카메라가 자신의 움직임을 파악하고 현재 위치를 알 수 있게 되기 시작했다. 연구자들은 성공적으로 여러 다양한 실시간 SLAM 시스템을 설계하였고, 그들 중 일부는 심지어 실시간 내에 3차원 공간 재구성까지도 수행한다. 더 좋은 소식은 근 몇년 새 SLAM에 관련된 적용 분야가 엄청나게 넓어졌다는 점이다. 로봇 청소기, 자율주행차, 무인항공기(UAVs), 가상현실(VR), 증강현실(AR) 등등.. SLAM은 현재에 와서 너무나 중요한 연구분야가 되었다.

21세기 이후 visual SLAM은 이론과 실제적인 부분 모두에서 상당한 변화와 혁신을 겪었으며 점차 실험실에서 실제 세계로 이동하고 있다. 하지만 동시에 SLAM 관련 기초 자료는 여전히 매우 부족해 SLAM의 매력에 빠져든 입문자들이 마땅한 가이드가 없어 고생하는 안타까운 광경을 많이 보아왔다. SLAM의 이론적 프레임워크는 많은 시간을 지나며 이제 완전히 자리를 잡았지만, 전체적인 SLAM 시스템을 구현하는 것은 여전히 매우 어렵고, 높은 수준의 전문 지식을 필요로 한다. 이 분야를 처음 접하는 연구자들은 상당한 양의 산재된 지식을 이 책, 저 책 뒤적이며 조각맞추기를 하듯이 공부하고, 정말 필요한 핵심 내용에 접근하기 위해 멀리서부터 돌아오는 수고를 하는 경우가 많다. 이 책은 visual SLAM에서 쓰이는 모든 기술을 체계적으로 설명한다.

SLAM의 이론적 배경과 시스템의 구조, 주류로 쓰이는 다양한 모듈들에 대한 설명을 제공하며, 동시에 이 모든 것에 대한 실습 파트를 구성해놓았다. 이 책에서 소개하는 모든 필수 알고리즘은 독자들이 직접 실험해보면서 보다 깊이 있는 이해를 할 수 있도록 바로 돌아가는 코드와 함께 제공된다. Visual SLAM은 결국 실제 응용을 위한 기술이므로 수학 이론을 구경하고 이해하는 것은 아름다울 순 있지만 결국 이것을 코드로 변환할 수 없다면 모든 이론은 뜬구름에 불과하다. 우리는 실습이 진정한 지식(그리고 진정한 사랑)을 가져온다고 믿는다! 알고리즘으로 양 손을 더럽힌 후에야 당신은 진정으로 SLAM을 이해했고 SLAM과 사랑에 빠졌다고 주장할 수 있을 것이다. SLAM의 긴 역사에서 등장한 모든 알고리즘과 그 변형에 대한 완전한 소개는 매우 어려울 뿐 아니라 사실 불필요하다. 우리는 SLAM 트리의 줄기를 보여주되 복잡하고 이상한 모양의 쓸데없는 잎은 생략하는 것이 효율적이라고 생각한다. 이 책의 목표는 SLAM 입문자가 자격을 갖춘 연구원 또는 개발자로 빠르게 성장할 수 있도록 만들어주는 것이다. 그렇지만, 이미 경험이 풍부한 SLAM 연구자라도 이 책은 익숙하지 않은 영역을 조명하고 새로운 통찰력을 제공하는 역할을 할 수 있을 것이다.

0.2 이 책을 어떻게 활용하는 것이 좋을까❓

이 책은 마치 강의와 같이 구성되어있다. 각 lecture는 논리적 순서에 따라 하나의 특정 주제를 설명하며, 각 장은 이론 파트와 실습 파트로 이루어져있다. 이론파트에서는 알고리즘을 이해하기 위한 필수 수학 개념을 대부분의 수학교재와 같은 definition / theorem / inference 형식이 아닌 서술형식으로 따라가기 쉽게 소개하고, 실습 파트에서는 코드 제공과 함께 각 부분의 의미를 설명하고 여러 실험 결과를 논의해 볼 것이다. 실습 이라는 단어가 제목에 보인다면, 바로 컴퓨터를 켜고 함께 프로그래밍을 해 볼 것을 추천한다. 책은 크게 두 파트로 나뉜다.

• 기초적인 수학 개념에 집중하는 part 1

1. 서문 (지금 읽고 있는 부분): 책의 내용과 구조를 설명
2. Lecture 1: SLAM 시스템에 대한 전반적인 설명. 전형적인 SLAM 시스템을 이루는 각 모듈을 살펴본다. 실습 파트에서는 리눅스 환경에서의 기본적인 C++ 프로그래밍과 IDE 활용을 소개한다.
3. Lecture 2: 3D 공간 상에서 rigid body의 모션을 다룬다. 회전 행렬, 쿼터니언, 오일러각을 배우고, 실습 파트에서는 Eigen 라이브러리를 소개한다.
4. Lecture 3: Lie group과 Lie 대수를 살펴본다. 처음 들어봤더라도 걱정하지 말 것. Lie 그룹의 기본을 배우고, Sophus를 통해 그들을 다루는 법을 배울 것이다.
5. Lecture 4: 핀홀 카메라 모델과 컴퓨터 내에서 이미지가 어떻게 표현되는지를 배운다. OpenCV를 사용해 카메라의 intrinsic / extrinsic parameter를 찾고, PCL을 통해 depth 정보를 이용해 point cloud를 생성해본다.
6. Lecture 5: 비선형 최적화 (상태 추정을 포함한), 최소자승, 그리고 가우스-뉴턴법이나 Levenburg-Marquardt method와 같은 gradient descent 방법에 대해 살펴본다. Ceres와 g2o 라이브러리를 이용해 curve-fitting 실습을 진행한다.

• VO에서 시작하여 SLAM에 직접적으로 쓰이는 알고리즘들을 다루는 part 2

7. Lecture 6: VO에서의 대세인 feature 기반 visual odometry에 대해 배운다. feature extraction과 matching, 에피폴라 기하학, Perspective-n-Point(PnP) 알고리즘, Iterative Closest Point(ICP) 알고리즘, Bundle Adjustment(BA) 등등을 살펴본다. 이 알고리즘들을 OpenCV에서 호출하거나, Ceres, g2o를 이용해 커스텀 최적화 문제를 설계함으로써 구현해본다.
8. Lecture 7: direct(또는 intensity 기반) VO에 대해 살펴본다. direct method와 optical flow의 원리에 대해 배우게 된다. 실습 파트에서는 single-layer / multi-layer optical flow를 작성하고 two-view VO에 대해 direct method를 구현한다.
9. Lecture 8: 백엔드 최적화. Bundle Adjustment에 대해 자세히 논의하고 sparse한 구조와 그것에 상응하는 그래프 모델 간의 관계를 보인다. 실습 파트에서는 같은 BA 문제를 Ceres와 g2o를 이용해 각각 구현해 볼 것이다.
10. Lecture 9: 백엔드 최적화의 포즈 그래프에 대한 내용을 다룬다. 포즈 그래프는 BA를 위한 좀 더 간단한 표현법으로, 모든 맵 포인트를 키프레임 간의 constraint로 변환해 해석한다. g2o를 활용해 포즈 그래프를 최적화해본다.
11. Lecture 10: Bag-of-Word(BoW) 방식을 중점으로 하여 loop closure detection에 대해 설명한다. DBoW3를 이용해 이미지로부터 dictionary를 학습하고 비디오 내에서 loop를 탐지해 볼 것이다.
12. Lectuer 11: map building. 단안 SLAM에서 어떻게 픽셀의 깂이값을 추정하는지, 그리고 그 추정치가 왜 믿을만하지 않은지 설명한다. 단안 깊이 추정과 비교했을 때, RGB-D 카메라를 이용해 dense map을 구축하는건 훨씬 쉽다. 실습에서는 단안 이미지로부터 에피폴라 라인 서치와 패치 매칭을 통해 깊이를 추정하고, 이를 이용해 point cloud map을 구축해보고, 여기에 더해 RGB-D 데이터를 이용해 octagonal treemap을 구성해 볼 것이다.
13. Lecture 12: 스테레오 VO를 위한 실습 챕터. 지금까지 배운 내용을 활용하여 프레임과 맵포인트 관리를 어떻게 할 지, 키프레임 선정은 어떻게 할 지, 최적화 컨트롤은 어떻게 할 지를 생각해보며 VO 프레임워크를 짜본다.
14. Lecture 13: 현재 존재하는 오픈소스 SLAM 프로젝트들을 소개하고 앞으로의 개발 방향에 대하여 논의한다.

마지막으로, 지금 위의 얘기가 무슨 말인지 전혀 이해하지 못하겠다면.. 축하한다! 이 책은 당신을 위한 것이다.

0.3 대상 독자

이 책은 SLAM을 배우고자 하는 학생들과 연구자들을 위한 책으로, 독자들은 어느 정도의 특정 전제조건은 만족하여야 한다. 우리는 다음의 지식은 독자들이 이미 갖추고 있을 것으로 예상하고 내용을 전개한다:

• 미적분학, 선형대수, 확률 이론. 최소한 벡터랑 행렬이 뭔지, 미적분의 의미가 무엇인지는 알아야 한다. 이 이상의 수학적 지식은 책에서 어느정도 설명을 제공한다.
• 기본적인 C++ 프로그래밍. SLAM 개발의 주 언어는 C++이다. 기본 내용과 문법에 익숙할 필요가 있다. 클래스가 무엇인지, STL이 무엇인지, 템플릿 클래스는 어떻게 사용하는지 등..
• 리눅스 기초. 제공되는 코드 또한 리눅스 기반이며, 우리는 SLAM 연구를 위해서는 리눅스를 사용하는 것이 백번 낫다고 강력히 주장한다. 이 책을 따라가다보면 여러분도 이 의견에 동의하게 될 것이라 생각한다. 윈도우 관련 이슈는 질문하지 말길 바란다. 독자들은 엄청난 리눅스 스킬을 갖출 필요는 없지만, 최소한 터미널을 켜고 코드가 있는 디렉토리로 이동할 줄은 알아야 한다.

사실 작업 시작한지 좀 돼서 기본적인 공부는 자료정리 없이 읽어보고 만져보고 시행착오를 반복한 끝에 어느정도 된 것 같은데 지금부터라도 조금 정리를 해야 할 것 같아서 개발 과정에서 새롭게 찾아보거나 문제가 생긴 부분을 틈틈이 메모처럼 남겨놓으려고 함

(1) 배열 선언

float *arr;

cudaMalloc((void**)&arr, size);

또는

float *arr[10];

아래가 더 간단하지만 다 알다시피 아래는 컴파일 타임에 배열의 크기가 지정되기 때문에 10이 들어가는 자리에 변수가 들어갈 수 없음. 대부분의 경우 배열 크기가 고정인 경우는 흔치 않으므로.. (내 경험 상 다른 변수.size() 이런 식으로 다른 변수에 종속적인 사이즈를 가지도록 선언해야 하는 경우가 대부분이다) 첫번째를 사용하는 것을 추천!

(2) 텐서 선언

torch::Tensor tensor_ = torch::empty({a,b}, {torch::kFloat64});

주의할 점은 torch::empty를 통한 텐서 선언은 _device_ 함수에서는 불가하다. torch::empty 는 _host_ 형으로 정의되어 있는데 _host_ 함수는 같은 _host_ 형 함수에서만 사용 가능하고 _device_ 에서는 호출할 수 없기 때문. 이렇게 생성한 tensor_를 꼭 _device_ 함수 내에서 사용해야 한다면 _host_ 함수로부터 입력인자로 넘겨받아 사용하면 될 듯?

텐서 내 성분에 접근할 때는 auto 형을 써주는게 속편하다.

float i = tensor_[1]; 하니까 오류나더라. 자세히는 안봤고 그냥 auto i = tensor_[1]; 해주는게 간단하고 좋음.

rosrun image_view extract_images image:=[이미지 토픽 이름]

이거를 켜고, rosbag play [bag file] 해주면 rosrun 명령어를 실행한 터미널 경로에 이미지가 저장됨.

재귀함수나 상호참조 등으로 스택메모리가 터지는건 봤는데 지역변수를 너무 크게 잡아서 터질거라고는 생각도 못하고 있다가 이번에 에러를 마주하게 돼서 기록함 (생각보다 기본 스택사이즈가 매우 작더라..) 리눅스 쉘에서 ulimit -a 명령어 입력하면 stack size 확인 가능. 1024000 byte이니까 int array[506][506] 이정도 배열을 함수 내부에서 잡아버리면 seg fault 뜨면서 터져버린단소리.. gcc에 플래그로 스택사이즈 변경해서 컴파일 할 수 있다는거같은데 자세히 안찾아봄.

🔧 메모리 크게 필요한 변수 있으면 힙 메모리를 쓰는게 좋다! stdlib의 calloc을 쓰거나, 스마트 포인터 이용