Sliding Window Filter and Optimization

• Controlling the Structure of BA

  • SLAM 프로세스에서는 실시간 계산을 유지하기 위해 problem의 scale를 제어해야 함

    • Example
      1. landmark를 10,000개로 제한
      2. iteration 횟수를 20회로 제한
      3. 작동 시간은 0.5초를 초과하면 안됨

  • scale control 방법

    • 연속적인 이미지에서 keyframe을 추출하고 keyframe과 랜드마크 사이에 BA만 구성 하여 계산 스케일을 제어
    • keyframe이 아닌 frame은 localization에만 사용되며 mapping 부분에는 기여하지 않음
    • 하지만 시간이 지날수록 keyframe의 수는 증가하고 지도의 규모는 계속 커져, BA와 같은 배치 최적화 방법의 경우 계산 효율성이 계속 저하되므로 backend의 scale을 제어해야 함

  • Solution: sliding window method

    → BA scale을 제어하기 위해 N개의 키 프레임만 현재 순간에 가장 가깝게 유지하고 이전 키 프레임을 제거 (BA를 time window 동안 수정하고, 이 기간을 벗어나는 것들은 폐기)

    • co-visibility graph: 현재의 키 프레임과 함께 관찰되는 특징

      → BA에서는 아래 그림처럼 co-visibility graph에서 일부 키 프레임과 랜드마크를 포함함

      → 올바르게 co-visibility graph를 구성한다면 더 오랜 시간동안 최적화 유지 가능

      

    • 그럼 무엇을 유지하고 무엇을 버릴까?

      → ”fixing”: 최적화하는 동안 keyframe estimate을 변경하지 않고 유지함

      → ”discarding”: window 밖의 변수가 window 안의 변수에 영향을 미치지 않는 것을 버림

    • 가정

      1. $N$개의 keyframe pose들이 있는 sliding window 가정

         → pose: Lie 대수 표현을 통해, vector space 상에서 정의된다고 가정

         

         → keyframe: 가우시안 분포를 따르는 평균과 공분산에 대응됨

         → landmark: $M$개 랜드마크를 아래와 같이 정의

         

      2. BA의 marginalization 과정 후의, pose에 대한 조건부 확률을 아래와 같이 정의

         → $\bold{\mu}$: $k$번째 키프레임의 평균

         → $\bold{\Sigma}$: 모든 키프레임들의 공분산 (= marginalization의 결과 = 행렬 $S$)

         

    • 새로운 Keyframe이 들어왔을 때 어떻게 처리할 것 인가?

      1. Adding New Keyframes (방법1) ****(window에서 landmark에 상응하는 새로운 keyframe 추가)

         1. sliding window가 마지막 순간에 N개의 keyframe을 설정했다고 가정 (이 때 keyframe들은 위에서 언급한 것 처럼 가우시안 분포를 따름)
         2. 다음으로, 새로운 keyframe $x_{N+1}$이 도착하면, 전체 problem의 변수는 $N+1$ keyframe의 집합됨
         3. 당연히 모든 포인트가 marginalize되면, $N+1$ keyframe의 pose도 가우시안 분포를 따름

      2. Removing Old Keyframes (방법2) ****(window에서 오래된 keyframe와, 그 keyfrarme을 관측하는 landmark 삭제)

         

         1. 가장 이전 keyframe $x_1$을 삭제하고 싶지만, marginalizing 후 위 그림처럼 $x_1$은 더 이상 sparse하지 않게 됨 (그러나 일부 프로세스를 진행하면 sparsity 유지 가능)

            • 왼쪽 그림 (marginalizing 전)

              → $x_1$이 $y_1$~$y_4$까지의 랜드마크를 본다고 가정

              → $x_1$ 줄에 0이 아닌 값($y_1$~$y_4$)이 있으므로, $x_1$이 랜드마크를 본다는 의미

            • 오른쪽 그림 (Schur trick을 사용하여 marginalizing)

              →Landmark-Landmark part($H_{22}$)의 Hessian이 information으로 채워져서 더 이상 대각행렬이 아니게 됨 (not sparse)

         2. 여기서 랜드마크 $y_1$~$y_4$는 아래 3가지 경우에 해당하게 됨

            • 경우1: 랜드마크들은 $x_1$에서만 관측됨
              • 즉, 삭제해도 다른 것에 영향X
            • 경우2: 랜드마크들이 $x_2$~$x_4$에서도 관측되지만, 앞으로는 관측 안 됨
              • missing feature points는 더 이상 tracking 하지 않음
              • 랜드마크들이 marginalized되면 pose-pose part($H_{11}$)의 prior가 생성되며, 이는 future pose estimation의 priori information이 됨
            • 경우3: 랜드마크들이 $x_2$~$x_4$에서도 관측되고, 앞으로도 관측됨
              • 나중에 estimate에 사용되어야 하므로 아무것도 하지 않음
              • marginalized하지 않고, 적은 연산을 위해 diagonal structure를 유지함

    • SWF(Sliding Window Filter) 정리

      → 확률에서 marginalization의 의미

      • joint distribution을 conditional distribution과 marginal distribution으로 나눔을 의미

      → Keyframe을 marginalization한다는 것의 의미

      • 현재 keyframe의 현재 추정값을 유지하고, 이 keyframe에 대해 조건화된 다른 상태 변수의 조건부 확률을 찾음

        • 즉, 관측되는 landmark point들은 "이 landmark가 있어야 할 위치"에 대한 priori information을 생성되고, 나머지 추정 값들에도 영향을 미침
        • 만약, landmark point들이 marginalize될 경우, "landmark를 관찰하는 keyframe의 위치"에 대한 priori information을 얻게 됨

      • 수학적으로, 전체 window의 상태 변수에 대한 description이 joint distribution에서 conditional probability distribution로 바뀜을 의미하게 됨

        • 그 다음, information의 marginalized된 부분을 버림
        • 변수가 marginalized된 다음에는 절대 사용하면 안 됨

        

      → VO에는 적합하지만, 대규모 Mapping System에는 적합하지 않음

Pose Graph Optimization

• Pose Graph Optimization Idea

  • 랜드마크 pose 최적화에 많은 시간 소요
  • observation이 몇 번 수행되고나면 공간적 위치는 한 값으로 수렴하게 됨

    • 수렴후 랜드마크 최적화는 시간낭비임

    • idea: 반복 후, 랜드마크 위치가 수렴하면 feature point를 고정하고 constraints로 사용 (= 랜드마크 포인트의 위치를 최적화하지 않고, 모든 카메라 포즈 사이의 연결에만 신경을 씀)

      

• Residuals & Jacobians

  • vertices와 edges 구하기

    1. node(= camera pose)를 $\bm{T}_1,\cdots,\bm{T}_n$으로 정의

       → edge: 두 node들 간, 상대적인 움직임의 추정치 (feature point method, direct method, GPS+IMU 등으로 부터 계산됨)

    2. $\bm{T}_i$와 $\bm{T}j$ 사이의 움직임 $\Delta\bm{T}{ij}$은 아래와 같이 정의될 수 있음

       

       또는, $\text{SE(3)}$ 방식으로 정의하면,

       

    3. least-square 문제를 설정하기 위해, 위 식의 $\bm{T}_{ij}$를 우변으로 이동시켜, error를 정의하면

    4. 두 최적화 변수 $\bm{\xi}_i,\bm{\xi}j$에 대한 $\bm{e}{ij}$의 미분을 찾기 위해, Lie 대수의 미분 방법에 따라 $\bm{\xi}_i,\bm{\xi}_j$의 왼쪽 섭동 $\bm{\delta\xi}_i,\bm{\delta\xi}_j$을 구하면 error는 아래와 같음

    5. 위 식에서 두 섭동 $\bm{\delta\xi}_i,\bm{\delta\xi}_j$은 식의 중간에 배치되고, ‘BCH 근사법’ 사용을 위해 섭동을 다른 변으로 이동시키면

    6. 재 정리하면

    7. 위 식을 erorr 식에 대입하고 정리하면,

    8. 여기서 2개의 Jacobians은,

    9. Jacobian $\mathcal{J}_r$은 $\mathfrak{se(3)}$ 형태이고 매우 복잡하므로 근사시키면,

    10. 최종적으로 목적함수를 정의하면,

    11. Gauss Newton’s method 또는 Levenberg-Marquardt method를 통해 해를 찾음

  • 모든 pose vertices와 pose edges에 대해 graph optimization 수행을 의미