Pose-graph optimization 이란?

• 계산 효율성을 높이기 위해 Landmark 를 state vector 에서 제외하는 시도들이 2000년대 후반 SLAM 학계에서 이어졌다.
• 그리고 3D 정보를 직접 정교하게 측정하는 LiDAR sensor 가 대중화되며, ambiguous landmark를 굳이 최적화할 필요 없이, 로봇의 pose만을 최적화 하는 것이 더욱 대중화된 듯하다.
• SymForce 를 이용해서 Pose-graph Optimization을 구현해보자.

Why SymForce for PGO?

• 사실 Pose-graph optimization 에 대해서는 GTSAM에서 이미 그 기능을 (API도 쓰기 쉽게) 너무 잘 제공하고 있기 때문에 새로운 library가 굳이 필요해? 라는 생각이 들기도 한다.
  • LIO-SAM이 그중 가장 유명한 GTSAM을 PGO에 사용한 예시이다 (거의 star도 이제 2천개가 다 되어 간다).
    • https://github.com/TixiaoShan/LIO-SAM 에서 mapOptmization.cpp 을 보면된다.
  • 하지만 보통 이렇게 되면 엔지니어가 할일은 매우 추상화되게 된다.
    1. isam = new ISAM2(parameters);를 선언하고,
    2. gtSAMgraph.add(some factor) 하고,
    3. isam->update(gtSAMgraph, initialEstimate); 하는 식이다 ..
  • 그래서 내부적으로 어떻게 실제로 최적화가 이루어지는지 어지간한 열정이 없으면 모르고 쓰게 된다.
    • 내 얘기다 ..
  • GTSAM은 C++에서 custom loss 를 작성할 수 있지만 Ceres에 비해 공개된 구현체나 docs 가 적은 느낌이다. 그래서 기존 built-in cost 들의 구현을 톺아보며 따라 작성해야 했고 .. 그리고 왠지 뭔가 내부구조를 좀 알기가 어렵다는 인상이 있었다. 내부적으로는 traits 과 Template 으로 복잡하게 짜여져있는데 .. 파악하기가 좀 복잡하다.
  • 반면 SymForce는 Jacobian 을 Symbolic 하게 계산해서 보여주므로 명확한 인상이 있다..
    • 앞의 블로그 튜토리얼 에서 Symforce 가 구해준 Jacobain을 가져다 사용해서 직접 Gauss-Newton iteration 을 구성해볼 수 있었다.
    • 무엇보다 python 에서도 custom loss 를 쉽게 작성할 수 있어서 이것저것 해보기가 좋은듯 하다.
  • ps. 한편, GTSAM 을 사용하는 그룹이 몇 있었고, 그 외에는 대체로 Ceres 를 많이들 사용하는 듯하다. Ceres 로 PGO를 구현한 가장 유명한 사례는 아마 VINS-mono일 것이다.
    • https://github.com/HKUST-Aerial-Robotics/VINS-Mono/tree/master/pose_graph 여기를 보면된다.
• 그래서 SymForce tutorial 차원에서 이것저것 작성해보고 있다.
  • Iterative optimizatino을 From scratch 로 구현하는 것은 지난 포스팅에서 해보았고, 앞의 블로그 튜토리얼의 todo 에서 얘기했듯 SymForce 가 제공하는 optimizer class 를 사용해보자.
  • 관련 예시는 landmark 를 bearing measurement 로 측정해서 global localization 을 수행하는 것이 공식readme 에 있다.
  • PGO가 딱 있지는 않은데 어짜피 구조는 비슷하니까 직접 작성해보자.

SymForce기반의 PGO 구현

• 여기에 전체 실습 코드가 있다.

Cost functions

• PGO에서 사용하는 cost function 은 (최소한으로) prior 와 odometry, loop closure 가 있다.
• SymForce 로는 다음과 같이 구현된다.

    eps = 0.000001
  
    def prior_residual(
            pose    : sf.Pose3, 
            pose_prior: sf.Pose3,
            diagonal_sigmas: sf.V6 
        ) -> sf.V6:
  
        tangent_error = pose.local_coordinates(pose_prior, epsilon=eps)
        return sfT.cast(sf.V6, sf.M.diag(diagonal_sigmas.to_flat_list()).inv() * sf.V6(tangent_error))
  
    def odometry_residual(
            pose_prev: sf.Pose3, 
            pose_next: sf.Pose3, 
            movement : sf.Pose3,
            diagonal_sigmas: sf.V6,
        ) -> sf.V6:
          
        # The original reference of the below lines 
        #  - see https://github.com/symforce-org/symforce/blob/main/symforce/examples/robot_3d_localization/robot_3d_localization.py#L63
        movement_predicted = pose_prev.inverse() * pose_next
        tangent_error = movement_predicted.local_coordinates(movement, epsilon=eps)
        return sfT.cast(sf.V6, sf.M.diag(diagonal_sigmas.to_flat_list()).inv() * sf.V6(tangent_error))
  
    def loop_residual(
            pose_prev: sf.Pose3, 
            pose_next: sf.Pose3, 
            movement : sf.Pose3,
            diagonal_sigmas: sf.V6,
        ) -> sf.V6:
  
        return odometry_residual(pose_prev, pose_next, movement, diagonal_sigmas)
  

• 설명
  1. prior 는 pose 의 SE(3)을 direct 로 추정하는 unary factor (인수가 하나만 쓰인다는 뜻) 이다.
  2. odometry 는 consecutive 한 node 사이의 motion 을 제약한다.
     • 따라서 pose $i$ 와 pose $i+1$ 사이의 예측된 값 (motion model) 이 실제로 측정된 값 (measurement)과 일치되도록 pose $i$와 $i+1$ 의 위치가 조정된다.
       • 이런 특성 때문에 GTSAM에서는 이를 between factor 라고 칭하기도 한다.
     • 즉, 둘 사이의 relative 한 제약만을 만족하면 되기 때문에 두 노드의 전역적인 위치 자체는 어디에 놓여있든 상관이 없게 된다.
       • 예를 들어, 두 노드가 1과 2에 있어도 그 사이 간격은 1이고, 2와 3에 있어도 그 사이 간격은 1이므로 여전히 제약을 만족한다.
       • 따라서 최소한의 prior 가 필요하며 보통 SLAM에서는 첫 node 에 아주 강한 (==매우 작은 covariance를 가지는) prior 제약을 걸어주게 된다.
       • 혹은 GPS 같은 값을 사용하여 전역적인 위치를 고정할 수도 있다.
         • rotation 정보가 없이 translation (즉, lat, lng, altitude) 만 사용가능한 경우, 하나의 node 만 prior 를 걸어주는 것으로는 부족하고 둘 이상의 서로 다른 노드에 prior 를 걸어주어야 global heading 을 알 수 있게 된다.
       • ps. 만약 prior 를 안걸어주면 이 노드들이 모양새는 유지하면서 전역적으로 미쳐날뛰게 (…) 되는데, 이를 게이지 프라블럼 이라고도 하는 듯하다. Ceres 의 공식 example 에서도 딱 이 이야기가 있다.

         // The pose graph optimization problem has six DOFs that are not fully
         // constrained. This is typically referred to as gauge freedom. You can apply
         // a rigid body transformation to all the nodes and the optimization problem
         // will still have the exact same cost. The Levenberg-Marquardt algorithm has
         // internal damping which mitigates this issue, but it is better to properly
         // constrain the gauge freedom. This can be done by setting one of the poses
         // as constant so the optimizer cannot change it.
         

  3. loop closure 는 not consecutive 한 두 node 사이의 motion을 제약한다.
     • 따라서 수학적으로는 odometry 와 완전히 동일하다.
       • 그래서 위의 구현에서도 loop_residual 에서는 내부적으로 odometry_residual 을 call하도록 구현하였다.
     • 다만 분리해서 이야기하는 것이 좋은 이유는,
       • odometry와 loop closure 에는 noise scale 을 다르게 적용하는 것이 현실 문제를 풀 때에 도움이 되기 때문이다.
         • 이는 본인이 사용하는 센서의 품질에 따라 달리 적용해야 한다. 예를 들어 odometry로 wheel odometer 를 사용하는데 그것의 품질이 좋지 않다면 odometry residual에 대해서는 높은 noise 를 부여할 수 있겠다. 반면, 고정밀의 lidar 센서 scan 을 통해 loop closing 을 수행한다면 loop closure residual에 대해서는 낮은 noise (==높은 신뢰도) 를 사용할 수 있겠다.
       • 또한 loop closure factor 를 추가하기 이전에, loop closure detection (or aka place recognition) 이라는 과정을 수행해야 하는데, 이것이 100% 항상 올바르다는 보장이 없다. 따라서 odometry factor 와 달리 loop closure factor 에 대해서는 robust kernel 을 씌워주는 작업 등이 요구되기도 한다.
         • 여기에서 GTSAM의 Cauchy kernel 을 사용한 예제 를 참고바람.
     • 따라서 프랙티컬하게는 loop closure factor 와 odometry factor 를 분리해서 생각하는 편이 좋다.
• 설명 2
  • 위의 코드를 보면 $\text{SE}(3)$를 인풋으로 받지만 실제로 diff는 local_coordinates라는 함수를 통해서 계산되는 것을 알 수 있다.
    • SymForce 의 코드를 뜯어보면 Pose3에 대해 local_coordinates 은 아래(원본링크)와 같이 구현되어 있다.

        @staticmethod
        def local_coordinates(a, b, epsilon):
            # type: (sym.Pose3, sym.Pose3, float) -> T.List[float]
      
            # Total ops: 50
      
            # Input arrays
            _a = a.data
            _b = b.data
      
            # Intermediate terms (4)
            _tmp0 = -_a[0] * _b[0] - _a[1] * _b[1] - _a[2] * _b[2]
            _tmp1 = _a[3] * _b[3]
            _tmp2 = min(1 - epsilon, abs(_tmp0 - _tmp1))
            _tmp3 = (
                2
                * (2 * min(0, (0.0 if -_tmp0 + _tmp1 == 0 else math.copysign(1, -_tmp0 + _tmp1))) + 1)
                * math.acos(_tmp2)
                / math.sqrt(1 - _tmp2 ** 2)
            )
      
            # Output terms
            _res = [0.0] * 6
            _res[0] = _tmp3 * (-_a[0] * _b[3] - _a[1] * _b[2] + _a[2] * _b[1] + _a[3] * _b[0])
            _res[1] = _tmp3 * (_a[0] * _b[2] - _a[1] * _b[3] - _a[2] * _b[0] + _a[3] * _b[1])
            _res[2] = _tmp3 * (-_a[0] * _b[1] + _a[1] * _b[0] - _a[2] * _b[3] + _a[3] * _b[2])
            _res[3] = -_a[4] + _b[4]
            _res[4] = -_a[5] + _b[5]
            _res[5] = -_a[6] + _b[6]
            return _res
      

    • 여기서 return 값은 두 Pose3인 a, b의 difference 이며 3-4-5 인덱스가 translation 차이에 해당하고, 0-1-2 인덱스가 rotation 차이에 해당한다.
      • 매번 게시글에서 이야기하지만 rotation 의 minimal representation은 3-dim 이기 때문에 …
        • ps. Rotation 공부자료 추천은 여기 를 참고바람.
    • Total ops 를 명기한 것도 흥미로운 부분이다. 즉, 저 연산이 Pose3에 대해서 deterministic 하게 최적화되어 있다는 뜻.
  • 아무튼 우리가 알아야 할 것 한줄요약은, rotation matrix은 vector space 가 아니기 때문에, Pose3 (== $\text{SE}(3)$) type 사이의 error 를 계산하고 싶을 때는 local_coordinates 라는 함수를 사용해야 한다는 것이다.
    • ps. 지난 from scratch 게시글에서는 minimal 한 vector3 인 rotvec을 먼저 정의하고, 이를 이용해서 SO(3)인 rotmat을 만들어서 썼었다.

          rotvec   = sf.V3.symbolic("Theta") # i.e., angle-axis parametrization
          rotmat   = LieGroupOps.from_tangent(sf.Rot3, rotvec) # for debug, display(rotmat.to_rotation_matrix())
      

Experiments Setting

• 암튼 사설이 길었는데 실험은 다음과 같다.

Dataset generation

• 먼저 토이 데이터셋을 직접 만들어보자. 자세한 건 코드를 참고 (링크)바람.
• move() 함수를 통해 정다각형 모양의 trajectory를 구성하는 n개의 pose 를 생성한다. 예시는 아래와 같다.

Hyper parameter setting

• PGO에서 hyper parameter 라고 하면 위에서 이야기한 3종류의 factors 에 대한 noise 를 결정하는 일이 되겠다.
• 이 예제에서는 다음과 같이 구성해보았다.

    ## noises 
    prior_diagonal_sigmas = np.array([0.001, 0.001, 0.001, 0.01, 0.01, 0.01]) * 0.001
    odometry_diagonal_sigmas = np.array([0.05, 0.05, 0.05, 0.1, 0.1, 0.1]) 
    loop_diagonal_sigmas = odometry_diagonal_sigmas * 0.1
  

  1. prior 의 경우 어짜피 제일 첫 노드에만 부여할 것이며, 전체 trajectory 가 흔들리지 않아야 하므로 (==gauge problem에 빠지지 않아야 하므로) 아주 작은 값으로 설정하였다.
  2. odometry 는 (rad와 meter 라는 단위를 고려하여) 적당한 값으로 설정하였다.
     • ps. 이 때 이 값은 std일수도 있고 covariance (std의 제곱) 일수도 있는데, library마다 다를 수 있으므로 문맥상 파악하여야 한다. 예를 들어 GTSAM에서는 covariance 이다.
       • 하지만 그것보다 더 중요한 건 어짜피 적정 스케일은 실험적으로도 파악하는 것이 요구된다.
       • 이 예제에서는 앞서 sfT.cast(sf.V6, sf.M.diag(diagonal_sigmas.to_flat_list()).inv() * sf.V6(tangent_error)) 와 같이 error 에 noise 의 inverse 를 바로 곱해주는 형태로 구현되었다.
         • 즉, std가 된다. 아래 수식을 참고하면 이해가 된다. (출처: Factor Graphs for Robot Perception, 2017). error $e$에 바로 (역수가)곱해지는 것은 covariance 의 sqrt임을 알 수 있다. 그래서 앞서 구현에서도 변수 이름을 diagonal_sigma 라고 칭해두었다.

     

  3. loop closure 의 noise 는 odometry보다 더 작게 (더 강력하게 작용하도록) 설정하였다.

Key-based Value Management

• SymForce 가 또 좋은 점이 바로 Key 기반으로 value 를 관리하는 것이다.
• factors들을 실제로 선언할 때 다음과 같이 하면 된다.

# 1/ Prior factors 
if use_factors['prior']:
    for i in range(num_poses):
        if i not in [0]:
            continue        
        # only add at the first node 
        factors.append(Factor(
            residual=prior_residual,
            keys=[f"poses[{i}]", f"poses_prior[{i}]", f"prior_diagonal_sigmas"],
        ))

# 2/ Odometry factors
if use_factors['odometry']:        
    for i in range(num_poses-1):        
        factors.append(Factor(
            residual=odometry_residual,
            keys=[f"poses[{i}]", f"poses[{i+1}]", f"odoms[{i}]", "odometry_diagonal_sigmas"],
        ))

# 3/ Loop factors
if use_factors['loop']:
    for ii, loops_index_pair in enumerate(loops_indexes):        
        idx_from = loops_index_pair[0]
        idx_to = loops_index_pair[1]
        factors.append(Factor(
            residual=odometry_residual,
            keys=[f"poses[{idx_from}]", f"poses[{idx_to}]", f"loops[{ii}]", "loop_diagonal_sigmas"],
        ))

• 위의 코드에서 보다시피, residual=에 함수를 입력해주고, keys= 에 이 함수에 인자로 들어갈 변수들을 차례로 나열해주면 된다.
  • 그런데 이 stringfied된 변수이름들은 어디에 있는가? 다음과 같이 Values 라는 곳에다가 다 담아주면 된다.

      initial_values = Values(
          # poses=[sf.Pose3.identity()] * num_poses, # may not converge.
          poses=initial_poses,
                
          poses_prior=poses_prior,
          prior_diagonal_sigmas=prior_diagonal_sigmas,
    
          odoms=odoms_obs,
          odometry_diagonal_sigmas=odometry_diagonal_sigmas,
                
          loops=loops_obs,
          loop_diagonal_sigmas=loop_diagonal_sigmas,
      )
    

    • 우변에 있는 것이 우리가 사전에 정의해둔 변수들이며
    • 좌변에 사용한 이름이 key로 쓰이게 된다.
• 그러면 최종적으로 Optimizer class를 사용하는 것은 쉽다. 아래와 같이 해주면 끝!

  optimizer = Optimizer(
    factors=factors,
    optimized_keys=[f"poses[{i}]" for i in range(num_poses)],
    debug_stats=True,
  )
  

  • 여기서는 최적화할 변수에 대한 key만 또 넣어주게 되는데, 이 부분은 좀 Pytorch와 닮은 것 같기도 하다.

Results

• 사설이 또 역시 길었는데, 결과는 다음과 같다.

Without a Loop constraint

• odometry factors 에 대해 ground-truth 대비 perturbation noise 를 섞어주었기 때문에 (see perturb_alpha in the codes), loop closure factor 없이는 아래와 같이 trajectory에 drift 가 누적되는 것을 볼 수 있다.

With only a single Loop constraint

• 한편, 처음 노드와 마지막 노드 사이에 loop closure factor 를 단 하나만 넣어주더라도, 아래와 같이 전체 trajectory의 shape 이 많이 개선되는 것을 볼 수 있다.

• 사실 수학적으로 당연한 결과이다ㅎㅎ 그래도 새로운 툴을 이용해서 간단하게 이렇게 실습해보는 것은 참 재미있다.
  • 이런 류의 (꽤 작은 코드베이스 규모의) PGO 는 SLAM계의 hello-world 라고 할 수 있지 않을까?

결론

• SymForce 로 Pose-graph Optimization 을 수행해보았다.
  • Pose3 들에 대해 unary (prior factor) or binary (between factor) constraints 를 걸어주기 위해서, measurement 와 estimated (direct value or error value) 사이에 error 를 계산해야 한다. 이를 계산하기 위해서 local_coordinates 이라는 함수를 사용한다. 이런 프로세스를 tangent space optimization 라고도 부른다.
• ps. 그 외에.. simularity 를 방지하기 위해 epsilon을 잘 사용해주어야 한다.

TODO

• 직접 제작한 toy example 에 대해서 해보았으니, SLAM 논문들에서 많이 쓰이는 데이터셋 에 대해서도 실험을 해보자.
  • 이 때 잘못된 (사실은 loop가 아닌데) loop closure factors 들이 많이 삽입될 때, 어떻게 방어할 수 있는지 알아보자.