들어가며

이번 글에서는 PIN-SLAM의 online SLAM loop에서 가장 중요한 두 block을 읽는다.

  • utils/tracker.py
  • utils/mapper.py

이 둘의 관계는 아주 고전적인 SLAM 구조와 같다.

Tracking: map을 고정하고 pose를 찾는다.
Mapping : pose를 고정하고 map을 업데이트한다.

다만 PIN-SLAM에서 map은 point cloud가 아니라 neural point 기반 SDF field이다. 그래서 tracking과 mapping도 조금 다르게 생겼다.

  • tracking은 point-to-point ICP가 아니라 point-to-implicit registration이다.
  • mapping은 occupancy grid update가 아니라 SDF supervision으로 neural map을 online training한다.

이 두 문장을 이해하면 PIN-SLAM 코드의 절반은 이해한 셈이다.

입력/출력 파이프라인

먼저 입력/출력부터 살펴보자. 대충 살펴봤을 때 헷갈리는 부분은 tracking과 mapping이 둘 다 SDF map을 query하는데, 무엇을 고정하고 무엇을 업데이트하는지가 다르다는 점이다.

요약하면 다음과 같다.

Block 입력 출력 업데이트되는 것
Tracker.tracking() current scan point, pose guess, 현재 neural SDF map refined pose, covariance, point weight, valid flag pose
DataSampler current scan point, sensor origin, refined pose SDF training sample, SDF label 학습 데이터
NeuralPoints.update() world frame의 observation/sample point 새 neural point, local map map structure
Mapper.mapping() SDF sample/label, local neural point map, decoder loss, updated neural point feature, updated decoder map parameter

N개의 current scan point가 들어왔을 때는 이렇게 흘러간다.

current scan point N개
  ├─ Tracking
  │    -> pose guess로 point를 world/map frame에 올림
  │    -> neural SDF map을 query해서 SDF residual과 gradient 계산
  │    -> residual이 줄어들도록 pose update
  │    -> refined pose 출력
  │
  └─ Mapping
       -> refined pose를 사용해서 LiDAR ray 주변 SDF sample 생성
       -> 필요한 위치에 neural point 추가
       -> sample을 neural SDF map에 query
       -> SDF label과 prediction 차이로 neural map update

즉 tracking은 map을 고정하고 pose를 바꾸는 block이고, mapping은 pose를 고정하고 map을 바꾸는 block이다. 둘 다 decoder를 거치지만, 목적이 다르다.

Tracking이 하는 일

pin_slam.py의 odometry block을 다시 보자.

tracking_result = tracker.tracking(
    dataset.cur_source_points,
    dataset.cur_pose_guess_torch,
    dataset.cur_source_colors,
    dataset.cur_source_normals,
    vis_result=config.o3d_vis_on,
)
cur_pose_torch, cur_odom_cov, weight_pc_o3d, valid_flag = tracking_result
dataset.update_odom_pose(cur_pose_torch)

입력은 현재 scan point와 초기 pose guess이다. 출력은 refined pose이다.

초보자식으로 말하면 Tracker는 다음 문제를 푼다.

현재 scan을 world frame에 올렸을 때, scan point들이 현재 SDF map의 zero-level set 위에 오도록 pose를 고쳐라.

여기서 zero-level set은 SDF가 0인 곳, 즉 surface이다.

point-to-implicit registration

일반적인 ICP는 대개 이런 식이다.

현재 scan point p_i
  -> map에서 nearest neighbor q_i 찾기
  -> p_i와 q_i 사이 거리 줄이기

PIN-SLAM은 다르다.

현재 scan point p_i
  -> 현재 pose T로 world에 올림
  -> neural SDF map S(T p_i)를 query
  -> S(T p_i)가 0이 되도록 T를 업데이트

즉 residual은 correspondence distance가 아니라 SDF value이다.

\[r_i = S(Tp_i)\]

이 방식의 장점은 explicit correspondence를 잡지 않아도 된다는 점이다. map이 smooth한 SDF field라면, SDF gradient가 surface normal 역할을 해준다.

Tracker.tracking() 흐름

Tracker.tracking()은 반복 optimization loop이다.

  1. 초기 pose T를 잡는다.
  2. source point를 T로 transform한다.
  3. registration_step()으로 incremental update delta_T를 구한다.
  4. T = delta_T @ T로 pose를 갱신한다.
  5. residual, valid point 수, eigenvalue 등을 보고 실패 여부를 판단한다.

코드상으로는 대략 이런 모양이다.

for i in range(iter_n):
    cur_points = transform_torch(source_points, T)
    delta_T, cov_mat, eigenvalues, ... = self.registration_step(cur_points, ...)
    T = delta_T @ T

즉 매 iteration마다 “현재 pose에서 scan point들이 map에 얼마나 안 맞는지”를 보고, 그 residual을 줄이는 작은 SE(3) update를 구한다.

registration_step(): SDF와 gradient를 query한다

registration_step()의 첫 핵심은 query_source_points()이다.

sdf_pred, sdf_grad, ..., mask, certainty, sdf_std = self.query_source_points(...)

여기서 query_source_points()는 내부적으로 neural_points.query_feature()를 부르고, sdf_mlp.sdf()를 통해 SDF를 예측한다.

필요한 값은 두 개이다.

  • sdf_pred: 현재 point의 SDF residual
  • sdf_grad: SDF gradient, 즉 surface normal 비슷한 방향

그 다음 유효한 point만 남긴다.

valid_idx = (
    mask
    & (grad_norm < max_grad_norm)
    & (grad_norm > min_grad_norm)
    & (sdf_std < max_sdf_std)
)

이 filter가 중요하다. implicit map이 아직 충분히 학습되지 않은 곳, neighbor가 부족한 곳, gradient가 이상한 곳은 registration에 쓰면 pose를 망칠 수 있다.

implicit_reg(): Jacobian은 어떻게 생겼나

가장 핵심은 implicit_reg()이다. 여기서 Levenberg-Marquardt update를 직접 만든다.

cross = torch.linalg.cross(points, sdf_grad, dim=-1)
J_mat = torch.cat([cross, sdf_grad], -1)
N_mat = J_mat.T @ (weight * J_mat)
g_vec = -(J_mat * weight).T @ sdf_residual
t_vec = inv(N_mat) @ g_vec

초보자 입장에서 이것만 기억하면 된다.

pose를 조금 움직였을 때 SDF residual이 어떻게 변하는지는, point 위치와 SDF gradient로 계산할 수 있다.

translation에 대한 Jacobian은 SDF gradient 자체이다.

\[\frac{\partial S(p)}{\partial t} = \nabla S(p)\]

rotation에 대한 Jacobian은 point와 gradient의 cross product로 나온다.

\[\frac{\partial S(p)}{\partial \theta} \approx p \times \nabla S(p)\]

그래서 코드에서 [cross, sdf_grad]를 이어 붙여 6D pose update의 Jacobian을 만든다. 앞 3개는 rotation, 뒤 3개는 translation이다.

robust weight와 실패 판단

실제 LiDAR scan에는 dynamic object, occlusion, 아직 학습 안 된 map 영역이 섞인다. 그래서 모든 point를 똑같이 믿으면 안 된다.

registration_step()에서는 여러 weight를 곱한다.

  • w_res: SDF residual이 큰 point를 줄이는 Geman-McClure weight
  • w_grad: SDF gradient norm이 이상한 point를 줄이는 weight
  • w_normal: source normal이 있을 때 gradient 방향과의 일관성
  • w_color: color/intensity consistency가 켜져 있을 때 사용

그리고 tracking loop 끝에서는 다음을 확인한다.

  • valid point가 너무 적은가?
  • final residual이 너무 큰가?
  • Hessian eigenvalue가 너무 작아서 degeneracy가 있는가?
  • optimization 중 residual이 갑자기 커졌는가?

이 checks가 없으면 neural map이 덜 학습된 초반이나 featureless 구간에서 pose가 쉽게 망가질 수 있다.

Mapping이 하는 일

이제 Mapper를 보자. pin_slam.py에서는 tracking 이후 mapping block이 실행된다.

mapper.process_frame(
    dataset.cur_point_cloud_torch,
    dataset.cur_sem_labels_torch,
    dataset.cur_pose_torch,
    frame_id,
    filter_dynamic,
)

mapper.mapping(cur_iter_num)

process_frame()은 학습 데이터를 준비하고 map structure를 업데이트한다. mapping()은 실제로 neural point feature와 decoder를 optimize한다.

DataSampler: LiDAR ray에서 SDF label 만들기

LiDAR point 하나는 사실 단순한 3D point가 아니다. 센서 origin에서 해당 point까지의 ray가 관측된 것이다.

PIN-SLAM은 이 ray를 이용해서 여러 SDF training sample을 만든다. utils/data_sampler.py의 DataSampler.sample()이 이 역할이다.

하나의 LiDAR measurement에 대해 sample은 크게 네 종류이다.

  1. measured point 실제 surface point이다. SDF label은 0이다.

  2. near-surface sample surface 근처에 Gaussian-like하게 뿌린 sample이다. 작은 signed distance label을 가진다.

  3. free-space front sample 센서와 surface 사이의 빈 공간이다.

  4. behind-surface sample surface 뒤쪽에 조금 뿌리는 sample이다.

이렇게 만든 (coord, sdf_label, weight)가 mapper의 training data가 된다.

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

LiDAR ray 하나를 surface point 하나로만 쓰지 않고, 그 ray 주변의 SDF supervision으로 확장한다.

process_frame(): map update와 data pool 관리

Mapper.process_frame()의 흐름은 다음이다.

1. 현재 scan을 준비한다

현재 point cloud를 sensor local frame에서 가져오고, 필요하면 dynamic filtering을 한다.

frame_point_torch = point_cloud_torch[:, :3]

2. SDF training sample을 만든다

coord, sdf_label, normal_label, sem_label, color_label, weight = self.sampler.sample(...)

여기서 coord는 아직 sensor local frame 기준이다.

3. neural point map을 업데이트한다

neural point를 추가할 위치를 정한다. default로는 sample 중 surface 근처 point를 world frame으로 transform해서 쓴다.

update_points = transform_torch(update_points, cur_pose_torch)
self.neural_points.update(update_points, frame_origin_torch, frame_orientation_torch, frame_id)

여기서 2편에서 본 NeuralPoints.update()가 호출된다. 즉 새 observation 근처에 neural point가 없으면 새 neural point가 생긴다.

4. data pool에 training sample을 쌓는다

PIN-SLAM은 현재 frame sample만 학습하지 않는다. 과거 sample도 pool에 넣어두고 replay한다.

self.coord_pool = torch.cat((self.coord_pool, coord), 0)
self.sdf_label_pool = torch.cat((self.sdf_label_pool, sdf_label), 0)
self.time_pool = torch.cat((self.time_pool, time_repeat), 0)

이게 중요한 이유는 catastrophic forgetting을 줄이기 위해서이다. 매 frame 현재 scan만 학습하면 map이 최근 observation에만 맞게 흔들릴 수 있다.

5. pool을 local window 기준으로 필터링한다

pool이 무한히 커지면 안 되므로, 현재 위치 주변 window 안의 sample만 유지한다.

pool_relative_dist < self.config.window_radius**2

그리고 너무 많으면 random하게 일부를 버린다.

mapping(): neural map을 실제로 학습한다

Mapper.mapping()은 PIN map online training 함수이다. 최적화 대상은 neural point feature와 decoder parameter이다.

neural_point_feat = list(self.neural_points.parameters())
sdf_mlp_param = list(self.sdf_mlp.parameters())
opt = setup_optimizer(...)

여기서 self.neural_points.parameters()가 동작하는 이유는, 현재 local map feature가 nn.Parameter로 잡혀 있기 때문이다.

각 iteration의 흐름은 다음이다.

  1. data pool에서 batch를 뽑는다.
  2. 각 sample의 frame pose를 이용해 world coordinate로 변환한다.
  3. neural map에서 feature를 query한다.
  4. decoder로 SDF를 예측한다.
  5. SDF loss, eikonal loss, optional semantic/color loss를 계산한다.
  6. backpropagation으로 neural feature와 decoder를 업데이트한다.
  7. local feature를 global feature에 써 넣는다.

코드 흐름은 다음과 같다.

coord, sdf_label, ts, ..., weight = self.get_batch(...)
poses = self.used_poses[ts]
coord = transform_batch_torch(coord, poses)

geo_feature, ..., weight_knn, _, certainty = self.neural_points.query_feature(coord, ts)
sdf_pred = self.sdf_mlp.sdf(geo_feature)

sdf_loss = sdf_bce_loss(...)
cur_loss = sdf_loss + eikonal_loss + sem_loss + color_loss

cur_loss.backward()
opt.step()

SDF loss와 eikonal loss

PIN-SLAM의 default main loss는 BCE 계열이다. 코드에서는 utils/loss.py의 sdf_bce_loss()를 쓴다. 개념적으로는 SDF prediction이 ray sample의 signed distance label과 맞도록 학습하는 것이다.

여기에 eikonal loss가 붙는다.

eikonal_loss = ((g_used.norm(2, dim=-1) - 1.0) ** 2).mean()

SDF의 중요한 성질 중 하나는 gradient norm이 1에 가까워야 한다는 점이다.

\[\|\nabla S(x)\| \approx 1\]

이 regularization이 있어야 SDF field가 너무 이상하게 찌그러지지 않는다. Tracking에서도 SDF gradient를 pose update에 쓰기 때문에, gradient quality는 odometry에도 중요하다.

decoder freeze가 왜 필요한가

pin_slam.py에서는 일정 frame이 지나면 decoder를 freeze한다.

if (frame_id-neural_points.reboot_ts) == config.freeze_after_frame:
    freeze_decoders(mlp_dict, config)
    config.decoder_freezed = True

초반에는 shared decoder가 “SDF를 어떻게 읽을지”를 배워야 한다. 하지만 SLAM이 진행되는 동안 decoder가 계속 바뀌면, 과거 neural point feature의 의미도 같이 바뀌어 map이 불안정해질 수 있다.

그래서 초반 warmup 이후에는 decoder를 고정하고, 새로운 공간은 주로 neural point feature를 통해 표현한다.

이건 PIN-SLAM을 이해할 때 매우 중요한 design choice라고 생각한다.

Tracking과 Mapping의 상호작용

PIN-SLAM의 online loop는 다음 반복이다.

1. 이전까지 만든 SDF map으로 현재 scan pose를 추정한다.
2. 추정된 pose를 믿고 현재 scan에서 SDF training sample을 만든다.
3. 그 sample로 SDF map을 더 잘 학습한다.
4. 다음 frame은 더 좋아진 map으로 tracking한다.

이 구조는 항상 chicken-and-egg 문제가 있다. map이 좋아야 pose가 잘 나오고, pose가 좋아야 map이 잘 학습된다. PIN-SLAM은 이를 위해 여러 안전장치를 둔다.

  • 첫 frame에서는 더 많은 mapping iteration을 수행한다.
  • tracking 실패 시 map update를 막는다.
  • valid point 수와 residual로 registration failure를 감지한다.
  • data pool replay로 forgetting을 줄인다.
  • local map만 학습해서 optimization을 안정화한다.

optional bundle adjustment

Mapper.bundle_adjustment()도 있다. 이 함수는 sliding window 안의 pose와 neural point feature를 함께 refine한다.

다만 default config에서는 ba_freq_frame = 0이라 꺼져 있다. 처음 코드를 읽을 때는 핵심 path에서 제외해도 된다.

중요한 점은, BA가 켜지면 data pool의 local coordinate와 pose가 다시 연결되므로, global_coord_pool을 다시 transform해야 하는 등 bookkeeping이 복잡해진다는 것이다.

이 글의 핵심 정리

PIN-SLAM의 tracking/mapping은 다음 한 줄로 정리할 수 있다.

Tracking은 neural SDF map에 scan을 붙여 pose를 찾고, Mapping은 LiDAR ray에서 만든 SDF sample로 neural SDF map을 학습한다.

코드 block으로 보면 다음이다.

Tracker.tracking()
  -> transform source scan by current pose
  -> query SDF and SDF gradient
  -> solve LM pose update with implicit_reg()

Mapper.process_frame()
  -> sample SDF labels along LiDAR rays
  -> add new neural points
  -> maintain replay data pool

Mapper.mapping()
  -> query neural features
  -> decode SDF
  -> optimize SDF/eikonal/semantic/color losses
  -> write local features back to global map

초보자라면 tracker.py와 mapper.py를 한 번에 다 이해하려고 하지 말고, 먼저 “SDF residual을 pose에도 쓰고, map 학습에도 쓴다”는 큰 그림을 잡는 것이 좋다.

다음 글

다음 글에서는 loop closure, pose graph optimization, 그리고 PGO 이후 neural point map이 어떻게 변형되는지 살펴본다. PIN-SLAM의 제목에 들어간 global map consistency가 코드에서 어디에 구현되어 있는지 볼 예정이다.

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