IMU 와 LiDAR의 상부상조

• IMU에게는 (중력 성분을 보상하기 위해) 좋은 global attitude가 필요하다. 그러면 (짧은 시간동안에는) 좋은 relative motion을 생성해준다.
• LiDAR에게는 좋은 initial transformation 이 필요하다. 그러면 registration을 잘 해서 좋은 global pose 를 생성해준다.
• 그래서 서로가 필요하다!

Pypose

• Pypose 라고 pytorch 기반의 pose solver 가 지난 11월에 나왔다.
  • https://pypose.org/
  • 논문: PyPose: A Library for Robot Learning with Physics-based Optimization
• 나오기를 기다리고 있던 툴이기도 하고 좋아보여서 실습을 해보고 있다.
  • 예전 블로그 글들에서 항상 하는 얘기 를 위한 library이기 때문이다.
    • 그 얘기란: robotics 특히 slam 은 pose estimation 이다. pose 에는 rotation이 포함되어 있다. rotation은 nonlinear 하다. 따라서 rotation 성분에 대해서 최적화를 하기 위해서는 그것의 tangent space 인 angle-axis 에서 optimal error state 를 구해서 원래의 manifold space 에 boxplus 로 더하는 작업을 (수렴할 때까지 iterative 하게) 해주어야 한다…
  • 즉, 이렇게 manifold 와 ambient space 를 오가는 메커니즘이 구현되어 있어야 pose 를 푸는 문제에서 쓰기 좋은 library라고 할 수 있겠다.
    • 하지만 pytorch 자체로는 특별히 이에 대한 작업을 기본적으로 제공해주지 않기 때문에 (그래서 quaternion을 4-dim tensor 에 담아서 그냥 backprop 해서 최적화하자~ 이러면 안된다), 기존에 python으로 pose 를 풀고 싶거나 (ceres C++를 pybind해서 써야하는 귀찮음이 있음) deep learning module 들과 pose solving 을 함께 (python에서) 물리고 싶을 때, 답답함이 있었다.
  • ps. 최근에 facebook 에서는 역시 동일하게 이렇게 nonlinear 한 pose 를 푸는 용도로 쓰일 수 있는 pytorch 기반의 library인 theseus 를 공개하기도 하였다.
• library의 특징
  • Pypose 는 기본적으로 rotation에 관한 연산과 이에 대한 (pytorch기반) 미분을 지원하는 것도 특징이고
  • 게다가 robotics 에서 자주 등장하는 문제에 대해 특화된 예제 코드도 제공하고 있는게 특징이다.
    • 아직 (2022.12) 많지는 않지만 imu preintegration 과 pose-graph optimization 이 보인다.
• 암튼 사설이 길었는데 pypose 의 imu preintegration 실습을 해보다가 재미있는 이야기포인트가 있어서 글을 쓰게 되었다.
  • 예제 코드 원본: https://pypose.org/tutorials/imu/imu_integrator_tutorial.html#what-is-imu-integration
  • 조금 손 본 코드: https://github.com/gisbi-kim/pypose-practice/tree/main/imu/integration
    • docker 로 구성하여 쉽게 실습할 수 있도록 하였고,
    • argument 받는 것을 yaml로 좀 더 깔끔하게 구성하였고,
    • Covariance visualization 을 좀 더 예쁘게 해보았다.

Pypose imu integration 실습

IMU integration 이란?

• 짧은 시간 $\delta t$ 사이의 gyro로부터 오는 angular velocity 와 accelerometer 로 부터 오는 linear acceleration 정보를 이용해서, 아래 수식 (출처: Pypose docs)에 의해, IMU센서의 relative pose (여기서는 position, attitude, and velocity) 를 계산하는 것을 의미한다.
  • note: attitude 라는 용어는 global coordinate 에서의 rotation 을 의미하는 목적으로 사용하고자 한다 (rotation이라는 단어는 relative 를 의미할 때도 자주 쓰이므로).

    

• 이렇게 얻은 relative transformation 을 직전 pose 에 계속 더해나간다면, 그것이 바로 inertial odometry라고 할 수 있겠다.
  • state 뿐 아니라 uncertainty 도 함께 evolve 하는 것이 특징이다.
• 이러한 IMU kinematics에 관한 내용에 대한 설명은 여기서는 생략하며 아래 자료를 (늘) 추천한다.
  • Quaternion kinematics for the error-state Kalman filter
  • Using Inertial Sensors for Positionand Orientation Estimation
  • Supplementary Material to: IMU Preintegration on Manifold for Ecient Visual-Inertial Maximum-a-Posteriori Estimation

데이터 준비

• 먼저, KITTI dataset 의 raw data를 다운받자.
  • 여기에 로그인 하면 이런 화면이 보일 것이다.

    

  • 여기서 synced+rectified data 와 calibration 을 다운받고 unzip해주면 된다.
    • data들과 calib 파일은 다음 예시와 같이 날짜이름으로 정리가 되어있으면 된다.

      

      

      • 이렇게 디렉토리 구조를 구성하면 위의 코드 의 yaml 및 dataloader가 알맞게 작동할 것이다.

실습

• sh docker_build.sh 하고 sh docker_run.sh 하면 log 밑에 결과 그림이 저장될 것이다.
  • docker_run.sh 에 경로가 하드코딩되어 있으니 본인경로로 바꾸어 주어야 한다.
  • yaml에 있는 noise 등등을 바꿔보면서 실습해보면 재밌다.

결과 (희망편)

• 꽤나 그럴듯 하게 나온다.
  • 예시 1: KITTI06 으로 잘 알려진 2011_09_30_0020
    • 파랑색이 IMU로만 예측한 경로, 빨간색이 ground-truth (고가의 GPS+INS 센서가 제공한 값) 이다.
    • Gaussian ellipsoids 는 xy space에 대해서 3-sigma 의 uncertainty bounds 를 시각화한 것이다.

    

  • 예시 2: KITTI05 로 잘 알려진 2011_09_30_0018 의 2D/3D view.

    

    • 역시 꽤나 그럴듯하다. drift 가 있기는 하지만 주변 구조물 정보를 참고하거나 GPS를 사용하지 않고 IMU 만 이용했다는 것을 생각해보면, 원래의 궤적을 거의 유지하고 있는 것이 신기하다.
• 그런데 이 결과에는 비밀이 하나 있다.
  • 위의 pypose 예제 코드에서, integrator는 rot 이라는 arguement 를 인자로 optional 하게 받도록 되어 있다.
  • 위의 결과들은 이 rot 에 ground-truth 의 global attitude 값을 줘서 얻은 결과이다.
    • 어떻게 보면 치팅이라고 할 수 있겠다.
• 이걸 왜 줘야 하는가?
  • 가속도값의 measurement 에는 사실 중력이 포함되어 있기 때문에, 이를 빼주어야 하고, 이를 빼주려면 현재 자세(attitude) 를 알아야 한다.
    • 이 관계는 아래 식 230과 같이 정의된다 (출처: Quaternion kinematics for the error-state Kalman filter). $a_m$ 이 실측값 (raw measurement from a sensor) 이다. 앞서 ‘IMU integration 이란?’ 에서 소개한 imu integration 모델에 사용될 값은 true (nominal) acceleration 값 $a_t$ 이므로 식 230 의 양변을 옮기고 정리하면 식 232와 같이 된다. 즉 실측값에 현재 자세를 곱하고 중력 (좁은 수백미터 영역의 mobile robot application에서는 보통 constant로 가정) 을 더해주어야 한다.

      

• 이 과정이 당연히 pypose 에도 구현되어 있다.
  • 따라서 이 때 정밀한 global attitude (rotation) 을 알 고 있다면 gravity 보상을 그만큼 더 잘할테고, 그만큼 더 실제 모션에 가까운 relative transformation 을 imu kinematics model 이 계산해낼 수 있을 것이다. 좋은 인풋이 들어갔으니 좋은 아웃풋이 나오는 것. 이 때 가장 좋을 수 있는 (ground-truth global attitude를 공급하였을 때) 결과가 위의 결과 캡처라고 생각할 수 있다.

결과 (실전편)

• 이 값이 별도로 외부에서 주어지지 않는다면, 직전 pose 의 attitude 를 사용해야 할 것이다.
• 그 결과는 다음과 같다.

  

  • 처음에 ㄱ자 턴 정도는 따라가는가 싶더니 어느새 발산해버렸다.

레슨

• 이 실험을 통해서 다음 (당연한) 사실을 체감할 수 있었다.
  1. IMU 기반의 motion propagation 을 위해서는 좋은 global rotation 이 필요하다.
     • IMU Only 기반의 navigation (== relative motion estimation, == odometry) 가 왜 발산하게 되는지 알게 되었다.
     • == 어떻게 하면 IMU Only에서도 발산을 막을 수 있는지 알게 되었다. - 따라서 imu 센서는 짧은 시간의 drift 를 보상 (특히 global rotation) 하기 위해서 다른 센서와 융합되어야 한다!
     • 그래서 항법쪽에는 gps-aided inertial navigation 이런 용어로 많이 연구되어 왔고, robotics 에서는 visual-inertial slam, lidar-inertial odometry, 이런 식의 용어로 많이 연구되어 왔다.
  2. 그런데 gps, camera, lidar 등의 보완센서가 imu좋은일만 시키는 것인가? 하면 그것도 아니다. 그래서 imu와 visual or lidar 의 궁합이 좋은 것이다.
     • visual sensor 나 lidar sensor 는 보통 10-30hz 정도이며 이는 fast motion (or highly dynamic environment such as urban sites) 에서 a big pose jump 나 data의 degradation (image 의 motion blur 또는 lidar point cloud scan의 motion distrotion) 을 유발한다. 이는 결국 front-end 에서 cost function을 만들 때, wrong correspondences 를 야기하게 되고, 결국 올바르게 relative motion cost 를 줄이지 못하고, relative motion estimation 에 결국 실패 (혹은 성능저하)하게 된다.
     • 그런데 visual or lidar 센서가 좋은 global rotation 값을 imu에 직전까지 공급해왔다면, 빠른 모션에서도 imu 는 100-500~ hz 의 빠른 데이터를 공급하므로 짧은 시간동안에는 여전히 성능을 유지하는 (initial) relative transformation 결과를 제공해줄 수 있다. 이는 앞의 실험에서 보았듯이, KITTI 05 와 같은 긴 경로의 주행에서도 전체 trajectory 의 궤적의 모양새를 유지할 정도로 신뢰가능하다 (물론 여전히 drift는 누적되며, 짧은 시간내에서 신뢰가능하다는 것이다). 따라서 빠른 모션에서도 imu는 visual or lidar sensor 에 좋은 initial 을 공급해주게 되고, 이는 harsh 한 input data 품질에서도 강건한 correspondences를 구축할 수 있게 해주며 결과적으로 좋은 relative motion estimation 품질을 보장할 수 있다.
     • 결국 lidar 가 보완한 imu가 다시 lidar 에게 도움을 주고 이게 다시 imu의 good integration 에 기여하고 … 의 상부상조.

결론

• 너무 당연한 소리 (IMU센서와 다른 exteroceptive sensor의 퓨전이 필요한 이유)를 실험까지 곁들여 가며 알아보았다.

Future works

• 위의 실험에서는 ground-truth global attitude 를 공급해서 저정도의 성능을 얻었지만, 실전에서 ground-truth global attitude 를 줄 수는 없다. 그래서 레슨2 에서 이야기했듯 gps, camera, or lidar 센서를 이용해서 global attitude 를 계속 잘 유지해나가는 식으로 fusion system 을 구성하게 되는데, lidar 쪽에서는 fast-lio 가 요즘 가장 유명한 예시인 듯하다.
  • faster-lio의 C++ 코드를 보면 앞의 pypose 의 integrate() 함수와 마찬가지로 rot을 곱하고 g를 더해주는 것을 볼 수 있다.

    

    • 다만 이 때의 rot 은 imu only 로 계속 쌓아가는 것이 아니라, lidar-aiding 을 통해서 계속 매 스캔마다 보정(filtering) 되는 값이기 때문에, 좋은 품질의 gravity 보정이 이루어질 것으로 기대할 수 있다 (실제로 fast lio의 데모 비디오들도 그러하고).
• 이 과정 (lidar 퓨전)을 python만 이용해서 동일하게 구현해보자.
  • 앞의 실험코드 pypose-practice/tree/main/imu/integration 를 기반으로 확장해보자.
    • ps. 그런데 사실 KITTI 는 쉬운 데이터셋이라 A-LOAM 같은 코드들을 보면 IMU를 쓰지 않고 lidar 만으로도 KITTI05 와 같은 곳은 충분히 좋아보이는 odometry 결과를 보여준다. 하지만 이런 성능들은 scan to scan이 아니라 scan to map registration 과 같은 registration 쪽에서의 기법들로부터 얻어진 것들도 많다.
• 혹은 별도 추가센서 없이 global rotation guide 를 잘 해줄 수 있다면? 예를 들어서 자동차는 roll 과 pitch motion 이 많이 없음을 가정할 수 있다. 이런 가정들을 정량화해서 virtual measurement 로 넣어주면 발산을 늦츨 수 있을 것이다.