SLAM & Odometry 평가 방법 : EVO

EVO는 SLAM과 Odometry의 성능을 평가하기 위한 파이썬 패키지이다. 추정된 pose와 GT pose 간의 비교 및 분석을 수행하고, APE(Absolute Pose Error)와 RPE(Relative Pose Error)를 계산할 수 있다. APE는 전체 궤적에 대한 절대 위치 오차를 RMSE, mean 등으로 계산한 결과이고, RPE는 연속된 포즈 간의 상대적 오차를 계산함으로써 드리프트를 분석할 수 있다. 

https://github.com/MichaelGrupp/evo

GitHub - MichaelGrupp/evo: Python package for the evaluation of odometry and SLAM

 

EVO Command Line Interface

• evo_ape : absolute pose error
• evo_rpe : relative pose error
• evo_traj : 
• evo_res
• evo_fig
• evo_config

주요 옵션들

-v, --verbose           # 상세한 출력
-a, --align            # 궤적 정렬 수행
--plot                 # 결과 플롯 표시
--plot_mode xy        # 플롯 모드 설정 (xy, xz, yz, xyz)
--save_plot file.pdf  # 플롯 저장
--save_results file   # 결과 저장
--t_max_diff 0.01     # 최대 시간 차이 설정
--t_offset auto       # 자동 시간 오프셋 계산
--pose_relation trans_part  # 포즈 관계 설정 (translation_part, rotation_part, full)
--delta               # 상대적 오차 계산
--delta_unit m       # 거리 단위 설정

예시 커맨드 : 

evo_ape tum poses_normalized.txt traj_normalized.txt -va --plot --plot_mode xy --t_max_diff 0.1

출력 :

(base) ubuntuurl@ubuntuurl-System-Product-Name:~/ziwon/URL_LIDAR_SLAM/NTU$ evo_ape tum poses_aligned.txt traj_aligned.txt -va --plot --plot_mode xy
Updated outdated /home/ubuntuurl/.evo/settings.json
--------------------------------------------------------------------------------
Loaded 6616 stamps and poses from: poses_aligned.txt
Loaded 6616 stamps and poses from: traj_aligned.txt
--------------------------------------------------------------------------------
Synchronizing trajectories...
Found 6616 of max. 6616 possible matching timestamps between...
	poses_aligned.txt
and:	traj_aligned.txt
..with max. time diff.: 0.01 (s) and time offset: 0.0 (s).
--------------------------------------------------------------------------------
Aligning using Umeyama's method...
Rotation of alignment:
[[ 0.7812338   0.62412556 -0.01187555]
 [-0.62338902  0.7810216   0.03730131]
 [ 0.03255576 -0.02173795  0.9992335 ]]
Translation of alignment:
[-0.1707576   0.00545008  0.1816923 ]
Scale correction: 1.0
--------------------------------------------------------------------------------
Compared 6616 absolute pose pairs.
Calculating APE for translation part pose relation...
--------------------------------------------------------------------------------
APE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)

       max	1.258967
      mean	0.267258
    median	0.250006
       min	0.012328
      rmse	0.300567
       sse	597.694214
       std	0.137528

--------------------------------------------------------------------------------

 

다만 EVO를 수행할 때는 timestamp가 잘 안 맞는 경우가 많다. 

import numpy as np
from scipy.interpolate import interp1d

def align_trajectories(poses_file, traj_file):
    poses_data = np.loadtxt(poses_file)
    traj_data = np.loadtxt(traj_file)
    
    # poses_data의 타임스탬프를 기준으로 traj_data를 interpolate
    interpolators = []
    for i in range(1, 8):  # x, y, z, qx, qy, qz, qw에 대해
        interp = interp1d(traj_data[:, 0], traj_data[:, i],
                         kind='linear', bounds_error=False,
                         fill_value="extrapolate")
        interpolators.append(interp)
    
    # 새로운 traj 데이터 생성
    aligned_traj = np.zeros_like(poses_data)
    aligned_traj[:, 0] = poses_data[:, 0]  # 타임스탬프 복사
    
    # 각 컬럼에 대해 interpolate
    for i in range(7):
        aligned_traj[:, i+1] = interpolators[i](poses_data[:, 0])
    
    np.savetxt('poses_aligned.txt', poses_data, 
               fmt='%.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f')
    np.savetxt('traj_aligned.txt', aligned_traj, 
               fmt='%.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f')
    
    return poses_data, aligned_traj

poses, aligned_traj = align_trajectories('poses.txt', 'traj_lidar_converted.txt')

이런 식으로 trajectory를 interpolate해서 timestamp를 맞춰 준다.