들어가며

이번 글에서는 PIN-SLAM의 backend와 output block을 읽는다. 핵심 파일은 다음이다.

  • utils/loop_detector.py
  • utils/pgo.py
  • model/neural_points.py의 adjust_map(), recreate_hash()
  • utils/mesher.py

PIN-SLAM의 제목에는 Achieving Global Map Consistency가 들어간다. 이 말은 단순히 loop closure로 trajectory error를 줄인다는 뜻이 아니다. loop closure 이후 neural point map 자체를 pose graph correction에 맞게 변형해서, reconstructed mesh까지 consistent하게 만드는 것이 핵심이다.

이번 글의 중심 질문은 다음이다.

loop가 닫혔을 때, PIN-SLAM은 pose graph와 neural point map을 어떻게 함께 고치는가?

입력/출력 파이프라인

먼저 입력/출력부터 살펴보자. 대충 살펴봤을 때 헷갈리는 부분은 “loop closure가 pose만 고치는가, 아니면 neural point map까지 같이 고치는가?”라는 질문이다.

정답부터 말하면 PIN-SLAM은 둘 다 고친다. PGO로 pose들을 update한 뒤, 그 pose 변화량을 neural point map과 data pool에도 적용한다.

모듈별 입력/출력은 다음과 같다.

Block 입력 출력 역할
local/global loop detection current pose/context, 과거 keyframe/context loop candidate 다시 방문한 장소 후보 찾기
Tracker.tracking(..., loop_reg=True) current scan, loop candidate map, loop pose guess refined loop transform, covariance, valid flag loop factor 검증
PoseGraphManager.optimize_pose_graph() odometry factor, loop factor optimized pose graph trajectory 전역 보정
NeuralPoints.adjust_map() PGO 전후 pose 차이 pose_diff 이동된 neural point map map geometry 보정
NeuralPoints.recreate_hash() 이동된 neural point position rebuilt voxel hash query structure 재구성
Mapper.transform_data_pool() pose_diff, 기존 training sample pool 보정된 data pool 이후 mapping data 정합성 유지
Mesher globally corrected neural SDF map mesh 최종 reconstruction

전체 backend 흐름은 이렇게 보면 된다.

current frame
  -> pose graph node/factor 추가
  -> local 또는 global loop candidate 탐지
  -> tracker로 loop transform refine
  -> loop factor 추가
  -> PGO로 pose graph update
  -> pose_diff 계산
  -> neural point map, voxel hash, data pool 보정
  -> corrected implicit map에서 mesh reconstruction

따라서 이 글에서 봐야 하는 핵심은 loop candidate를 찾는 것 자체보다, PGO 이후의 pose 변화가 neural point map까지 어떻게 전달되는가이다.

왜 loop closure가 필요한가

앞 글에서 본 tracking은 local SDF map에 현재 scan을 맞추는 방식이다. 이 방식은 frame-to-frame 또는 local map 기준으로는 잘 동작할 수 있지만, 긴 sequence에서는 drift가 쌓인다.

drift가 쌓인 상태에서 같은 장소를 다시 방문하면 문제가 생긴다.

  • trajectory는 같은 장소인데 서로 다른 위치로 추정된다.
  • neural point map에는 같은 구조물이 중복으로 생긴다.
  • mesh를 뽑으면 벽이나 기둥이 두 겹으로 보일 수 있다.

PIN-SLAM README의 teaser도 바로 이 문제를 보여준다. odometry-only implicit map은 duplicate structure가 생길 수 있고, SLAM backend를 통해 globally consistent mesh를 만든다.

main loop의 backend block

pin_slam.py의 loop detection and PGO block은 대략 다음 순서로 돌아간다.

1. 현재 frame을 pose graph node로 추가
2. 이전 frame과 odometry factor 추가
3. loop candidate 탐지
4. candidate가 있으면 tracker로 loop transform refine
5. loop factor 추가
6. pose graph optimization
7. optimized pose 차이를 neural point map과 data pool에 적용

코드상 핵심 호출은 다음이다.

pgm.add_frame_node(frame_id, dataset.pgo_poses[frame_id])
pgm.add_odometry_factor(frame_id, frame_id-1, dataset.last_odom_tran)

loop_id, ... = detect_local_loop(...)
# or
loop_id, ... = lcd_npmc.detect_global_loop(...)

pose_refine_torch, loop_cov_mat, ..., reg_valid_flag = tracker.tracking(..., loop_reg=True)
pgm.add_loop_factor(frame_id, loop_id, loop_transform)
pgm.optimize_pose_graph()

pose_diff_torch = torch.tensor(pgm.get_pose_diff(), ...)
neural_points.adjust_map(pose_diff_torch)
neural_points.recreate_hash(...)
mapper.transform_data_pool(pose_diff_torch)
dataset.update_poses_after_pgo(pgm.pgo_poses)

여기서 lcd_npmc는 NeuralPointMapContextManager의 instance이다.

loop detection은 두 단계로 보면 된다

loop closure는 보통 두 단계이다.

  1. place recognition 과거에 비슷한 장소를 방문했는지 후보를 찾는다.

  2. geometric verification / registration 후보가 진짜 맞는지 scan-to-map registration으로 검증하고 transform을 refine한다.

PIN-SLAM도 이 구조를 따른다. utils/loop_detector.py는 주로 1번을 담당한다. 진짜 transform refine은 다시 Tracker.tracking(..., loop_reg=True)가 한다.

이 분리가 중요하다. context descriptor는 빠르게 후보를 찾는 용도이고, 실제 loop factor로 넣을 SE(3)는 registration으로 검증해야 한다.

local loop: 가까운 과거 pose 찾기

먼저 단순한 local loop가 있다. detect_local_loop()는 현재 pose와 과거 pose들의 거리를 보고, 가까운 후보를 찾는다.

조건은 대략 다음이다.

  • 충분히 오래 이동한 과거 frame이어야 한다.
  • 현재 estimated pose와 spatial distance가 가까워야 한다.
  • drift radius가 너무 크지 않아야 한다.
  • 최근 loop registration이 계속 실패한 상황이면 더 조심한다.

후보가 있으면 초기 loop transform은 pose graph pose로부터 계산한다.

loop_transform = inv(pgo_poses[loop_id]) @ pgo_poses[-1]

이건 T_loop<-current 초기값이다.

global loop: context descriptor로 장소 찾기

local loop만으로는 큰 drift를 다루기 어렵다. 그래서 PIN-SLAM은 scan context 계열의 descriptor를 쓴다. 코드에서는 NeuralPointMapContextManager가 담당한다.

새 node를 추가할 때는 point cloud를 ring-sector grid로 바꾼다.

sc, sc_feature = ptcloud2sc_torch(ptcloud, ptfeatures, des_shape, max_length)
rk = sc2rk(sc)

여기서 sc는 scan context이다. default shape은 config에서 [20, 60]이다. 즉 20개 ring, 60개 sector로 공간을 나누고, 각 bin에 height 같은 summary를 저장한다.

rk는 ring key이다. sector 방향을 평균내서 rotation에 덜 민감한 descriptor를 만든다. 후보 검색은 ring key로 빠르게 하고, 자세한 yaw alignment는 scan context를 shift하면서 확인한다.

neural point map context

PIN-SLAM의 재미있는 점은 raw scan뿐 아니라 local neural point map으로도 context를 만들 수 있다는 것이다.

pin_slam.py에서 config.local_map_context가 켜져 있으면, 현재 frame보다 약간 latency가 있는 local map을 꺼내 descriptor를 만든다.

neural_points.reset_local_map(...)
context_pc_local = transform_torch(neural_points.local_neural_points.detach(), inv(local_map_pose))
neural_points_feature = neural_points.local_geo_features[:-1].detach() if config.loop_with_feature else None
lcd_npmc.add_node(local_map_frame_id, context_pc_local, neural_points_feature, ...)

왜 latency를 둘까? 현재 frame의 observation은 아직 충분히 학습되지 않았을 수 있기 때문이다. 몇 frame 뒤에 local map이 조금 안정된 다음 context를 만들면 descriptor quality가 좋아질 수 있다.

또 loop_with_feature가 켜져 있으면 neural point의 geometric feature도 context에 넣을 수 있다. 즉 단순 geometry뿐 아니라 learned feature를 place recognition에 활용하려는 구조이다.

virtual node: translation에 대한 보정

scan context는 yaw rotation에는 비교적 강하지만, translation에는 민감할 수 있다. PIN-SLAM은 local map context를 쓸 때 virtual node를 만든다.

set_virtual_node()를 보면 현재 pose 주변 lateral direction으로 여러 가상의 sensor position을 만든다.

virtual_positions = frame_pose[:3, 3] + lat_tran

각 virtual position에서 local map을 다시 바라본 context를 만들어 query candidate와 비교한다. 이건 descriptor matching에서 작은 translation error를 흡수하기 위한 장치로 볼 수 있다.

초보자 입장에서는 이렇게 이해하면 된다.

현재 pose가 조금 옆으로 틀어져 있어도 같은 장소를 놓치지 않도록, 주변 가상 위치에서도 context를 만들어 본다.

loop transform refine

context descriptor가 후보를 찾았다고 해서 바로 loop factor를 넣지는 않는다. pin_slam.py에서는 loop candidate의 local map을 활성화한 뒤, 현재 scan을 그 map에 registration한다.

pose_init_torch = dataset.pgo_poses[loop_id] @ loop_transform
neural_points.recreate_hash(pose_init_torch[:3,3], ..., loop_id)
pose_refine_torch, loop_cov_mat, ..., reg_valid_flag = tracker.tracking(
    loop_reg_source_point,
    pose_init_torch,
    loop_reg=True,
)

여기서도 결국 3편에서 본 point-to-implicit registration을 재사용한다. 즉 loop closure의 geometric verification도 PIN-SLAM의 SDF map 위에서 수행된다.

registration이 성공하면 loop factor를 pose graph에 추가한다.

loop_transform = inv(dataset.pgo_poses[loop_id]) @ pose_refine_np
pgm.add_loop_factor(frame_id, loop_id, loop_transform)

PoseGraphManager: GTSAM backend

utils/pgo.py의 PoseGraphManager는 GTSAM wrapper이다. node는 pose이고, factor는 크게 세 종류이다.

  • prior factor
  • odometry between factor
  • loop closure between factor

초기 frame에는 fixed prior를 건다.

pgm.add_pose_prior(0, init_pose, fixed=True)

매 frame odometry factor를 추가한다.

pgm.add_odometry_factor(frame_id, frame_id-1, dataset.last_odom_tran)

loop가 검증되면 loop factor를 추가한다.

pgm.add_loop_factor(frame_id, loop_id, loop_transform)

optimization은 iSAM2 또는 batch LM을 쓸 수 있다. default는 pgo_with_isam=True이다.

PGO 이후 pose_diff를 계산한다

PGO가 끝나면 각 frame pose가 바뀐다. PIN-SLAM은 optimization 전 pose와 후 pose의 차이를 계산한다.

pose_diff_torch = torch.tensor(pgm.get_pose_diff(), ...)

이 pose_diff가 중요하다. 각 frame에 대해 “이 frame 기준으로 만들어진 map element는 얼마나 움직여야 하는가”를 알려주는 correction이기 때문이다.

adjust_map(): neural point map을 같이 움직인다

NeuralPoints.adjust_map()이 global map consistency의 핵심이다.

각 neural point는 생성 timestamp를 가지고 있다. PGO 이후 각 timestamp에 해당하는 pose correction을 찾아 neural point 위치에 적용한다.

used_ts = self.point_ts_create
self.neural_points = transform_batch_torch(
    self.neural_points,
    pose_diff_torch[used_ts],
)

orientation도 같이 바꾼다.

diff_quat_torch = rotmat_to_quat(pose_diff_torch[:, :3, :3])
self.point_orientations = quat_multiply(
    diff_quat_torch[used_ts],
    self.point_orientations,
)

이게 왜 가능한가? PIN-SLAM의 map은 sparse point-based representation이기 때문이다. dense MLP 하나가 scene 전체를 통째로 외우는 구조라면, PGO 이후 map을 부분별로 움직이기 어렵다. 반면 neural point map은 각 point가 어느 시점에 만들어졌는지 알고 있으므로, pose graph correction을 point별로 적용할 수 있다.

이 부분이 PIN-SLAM contribution의 핵심이라고 생각한다.

hash와 data pool도 같이 고쳐야 한다

map point를 움직였으면 voxel hash table은 더 이상 맞지 않는다. 그래서 바로 recreate_hash()를 부른다.

neural_points.recreate_hash(...)

또 mapper의 training data pool도 world coordinate를 가지고 있으므로 같이 변형해야 한다.

mapper.transform_data_pool(pose_diff_torch)

마지막으로 dataset 내부 pose들도 PGO 결과로 업데이트한다.

dataset.update_poses_after_pgo(pgm.pgo_poses)

즉 PGO 이후에는 다음 세 가지가 함께 바뀐다.

  • trajectory
  • neural point map
  • training data pool

이 세 가지를 같이 맞춰야 다음 frame부터 tracking/mapping이 새 global frame 기준으로 이어진다.

Mesher: implicit map을 mesh로 뽑기

마지막으로 utils/mesher.py를 보자. Mesher는 SLAM estimation 자체보다 output 쪽에 가깝다. 하지만 PIN-SLAM의 장점을 보여주는 중요한 block이다.

핵심은 다음이다.

  1. bounding box 안에 grid query point를 만든다.
  2. 각 query point에서 SDF를 예측한다.
  3. valid neighbor가 충분한 곳만 marching cubes mask로 남긴다.
  4. skimage.measure.marching_cubes로 mesh를 만든다.

query_points()는 내부적으로 다시 neural_points.query_feature()와 sdf_mlp.sdf()를 호출한다.

batch_geo_feature, ..., weight_knn, nn_count, _ = self.neural_points.query_feature(...)
batch_sdf = self.sdf_mlp.sdf(batch_geo_feature)

즉 meshing도 별도의 map을 쓰는 것이 아니다. tracking과 mapping이 쓰던 바로 그 neural SDF map을 dense grid에서 query하는 것이다.

이게 implicit representation의 장점이다. map이 “점들의 모음”을 넘어, 공간 어디서든 SDF를 물어볼 수 있는 field가 된다.

전체 backend 흐름 정리

PIN-SLAM의 backend는 다음처럼 정리할 수 있다.

Odometry drift 발생
  -> local/global loop candidate 탐지
  -> SDF map 기반 registration으로 loop transform refine
  -> GTSAM pose graph에 loop factor 추가
  -> PGO로 trajectory correction 계산
  -> neural point map을 timestamp 기준으로 deform
  -> hash table과 data pool 재정렬
  -> globally consistent SDF map에서 mesh reconstruction

이 흐름에서 중요한 것은 loop closure가 trajectory만 고치는 것이 아니라는 점이다. PIN-SLAM은 map representation 자체가 point-based이기 때문에, PGO correction을 map에도 적용할 수 있다.

PIN-SLAM을 처음 읽는 순서 추천

내 기준으로는 다음 순서가 가장 덜 헷갈린다.

  1. pin_slam.py: 전체 loop를 먼저 본다.
  2. model/neural_points.py: map이 어떻게 저장되고 query되는지 본다.
  3. model/decoder.py: SDF decoder가 얼마나 단순한지 확인한다.
  4. utils/tracker.py: SDF residual과 gradient로 pose update를 구하는 부분을 본다.
  5. utils/data_sampler.py와 utils/mapper.py: LiDAR ray sample이 어떻게 SDF supervision이 되는지 본다.
  6. utils/loop_detector.py와 utils/pgo.py: loop closure와 global consistency를 본다.
  7. utils/mesher.py: 최종 mesh reconstruction을 본다.

반대로 처음부터 NeuralPoints.query_feature() 내부의 모든 tensor shape을 따라가려고 하면 금방 지친다. 먼저 전체 pipeline을 잡고, 그 다음 함수 단위로 내려가는 게 낫다.

시리즈를 마치며

PIN-SLAM을 한 문장으로 다시 정리하면 다음과 같다.

Sparse neural point map으로 online SDF field를 만들고, 그 SDF field에 scan을 registration하며, loop closure 이후 pose graph correction을 neural point map에도 적용하는 LiDAR SLAM system.

내가 이 코드를 읽는 이유도 여기에 있다. 기존 SLAM에서는 map representation이 대개 geometric data structure였는데, PIN-SLAM은 learned implicit representation을 SLAM frontend/backend 안에 꽤 실용적으로 녹여냈다.

특히 다음 아이디어들이 인상적이다.

  • neural point를 voxel hash로 관리해서 online SLAM에 맞춘 점
  • SDF residual을 tracking과 mapping 모두에 쓰는 점
  • decoder를 초반에 학습하고 이후 freeze하는 점
  • loop closure 이후 neural point map을 timestamp 기준으로 같이 deform하는 점
  • 같은 implicit map에서 odometry, loop refinement, meshing이 모두 나온다는 점

이 시리즈는 완벽한 line-by-line 해설이라기보다는, PIN-SLAM 코드를 다시 읽기 위한 지도에 가깝다. 다음에 더 깊게 들어간다면 query_feature()의 tensor shape, implicit_reg()의 Jacobian 유도, sdf_bce_loss()의 sign convention을 따로 파고들면 좋을 것 같다.

PIN-SLAM 코드 읽기 시리즈입니다.

  1. PIN-SLAM 코드 읽기 - 1. Contribution과 전체 Pipeline
  2. PIN-SLAM 코드 읽기 - 2. Neural Point Map과 Decoder
  3. PIN-SLAM 코드 읽기 - 3. Tracking과 Mapping
  4. PIN-SLAM 코드 읽기 - 4. Loop Closure, PGO, Meshing

References