Blog

Introduction

Recently, while studying IMU preintegration, I suddenly wondered how IMU preintegration is implemented in ORB-SLAM3, so I analyzed it. The theoretical part of IMU preintegration is already well explained Dr. Lim's blog, so I think it would be good to refer to it, and I compared the IMU preintegration paper with the ORB-SLAM3 code to see how the formula in the paper is implemented in the code.
It would take a lot of time to write the IMU preintegration part in one article, so I would like to write it in the following order.
1.
IMU sensor measurement integration
2.
Defining the residual function
3.
Definition of Jacobian matrix
This article will cover the code analysis of the first of them, IMU sensor measurement integration.

Code analysis

Integrating IMU Sensor Measurements

First, let's look at the part of the code that integrates the measurements between keyframe i and keyframe j by accumulating the acceleration and angular velocity measurements from the IMU sensor as they come in. During this process, the following values are computed
The overall calculation is performed in the IntegrateNewMeasurement function of the Preintegrated class. The complete function code is shown below.
[ENG] ORB-SLAM3 IMU preintegration code review (1)

Introduction

Last time, we analyzed the first of several components of IMU preintegration in ORB-SLAM3, the IMU sensor measurement integration part, by comparing the paper and the code. The measurement integration part (1) computes the relative state (rotation, translation, velocity) values between two keyframes, (2) obtains the covariance matrix through noise propagation, and (3) computes the Jacobian matrix for bias update. The values computed here are later used to perform bundle adjustment using keyframes, or to perform nonlinear optimization such as pose estimation for a single camera frame.
In this tutorial, we will analyze how the residual function of IMU factors and the definition of Jacobian matrix are implemented in the ORB-SLAM3 code to perform nonlinear optimization.

Prerequisites

Factor Graphs and MAP Esimation

A factor graph is a structure used to represent the relationships between variables in a stochastic model and to find solutions that satisfy all constraints (factors). It is used here to address the Visual-Inertial Odometry (VIO) problem and consists of the following main components
1.
Graph node
2.
Graph edges
The VIO problem defined in the IMU preintegration paper is represented as follows in Equation (25).
p(Xk∣Zk)∝p(X0)p(Zk∣Xk)=(a)p(X0)∏(i,j)∈Kkp(Ci,Iij∣Xk)=(b)p(X0)∏(i,j)∈Kkp(Iij∣xi,xj)∏i∈Kk∏l∈Cip(zil∣xi). \begin{equation} \begin{aligned} p(\mathcal{X}_k \mid \mathcal{Z}_k) &\propto p(\mathcal{X}_0) p(\mathcal{Z}_k \mid \mathcal{X}_k) \\ &\overset{(a)}{=} p(\mathcal{X}_0) \prod_{(i,j) \in \mathcal{K}_k} p(\mathcal{C}_i, \mathcal{I}_{ij} \mid \mathcal{X}_k) \\ &\overset{(b)}{=} p(\mathcal{X}_0) \prod_{(i,j) \in \mathcal{K}_k} p(\mathcal{I}_{ij} \mid \mathbf{x}_i, \mathbf{x}_j) \prod_{i \in \mathcal{K}_k} \prod_{l \in \mathcal{C}_i} p(\mathbf{z}_{il} \mid \mathbf{x}_i). \end{aligned} \tag{25} \end{equation}
[ENG] ORB-SLAM3 IMU preintegration code review (2)
안녕하세요 SLAM KR! SLAM에 입문하시는 슬린이 여러분들을 위해 SLAM KR 커뮤니티에서 공부한 내용들을 교재로 정리해보았습니다 :-) SLAM이 무엇인지 , 어디에 쓰일수 있는지 흥미를 돋우는 내용으로 시작하여, SLAM에 사용하는 센서들과 SLAM을 공부하실 때 필요한 수학적 기초 지식들(선형대수, 확률통계, 비선형 최적화, 회전변환)을 정리했습니다. 크게 Frontend와 Backend로 분리하여 SLAM을 공부하는데 필요한 내용들을 수식과 코드 설명, 실습을 포함하여 상세히 설명하고자 했습니다. 마지막으로 SLAM 기술의 미래 발전 방향과 이러한 SLAM 관련 기술들을 공부할 수 있는 커뮤니티 및 관련 자료들을 정리했습니다. 아직 부족한 부분이 많아서 공개를 해야할지 고민을 많이 하였으나 SLAM을 처음 접하시거나 SLAM에 대해 좀 더 깊이 공부하고 싶으신 분들에게 자그마한 도움이 되셨으면 좋겠다는 마음에 자료를 공개합니다. 슬린이가 슬잘알이 되는 그 날까지! 감사합니다 :-)
Loading PDF…
PASSWARD: slamkr
NVIDIA Jetson Nano와 함께하는 SLAM의 이해와 구현

소개

지난 시간에 우리는 ORB-SLAM3에서 IMU preintegration의 여러 구성요소 중에 첫번째로 IMU 센서 측정 값 integration 부분을 논문과 코드를 대조하여 분석해보았습니다. 측정값 integration 부분에서는 (1) 두 키프레임 사이의 상대적인 상태 (회전, 이동, 속도) 값을 계산하고, (2) 노이즈 전파를 통해 공분산 행렬을 구하고, (3) 바이어스 업데이트를 위한 자코비안 행렬을 계산합니다. 여기서 계산된 값들은 나중에 키프레임들을 이용한 Bundle adjustment를 수행할 때나, 카메라 프레임 하나에 대한 Pose estimation등 비선형 최적화를 수행할때 사용됩니다.
이번 시간에는 비선형 최적화를 수행할때 구성하는 IMU factor의 Residual 함수와 Jacobian 행렬의 정의에 대해서 ORB-SLAM3 코드에서는 어떻게 구현되어 있는지 분석해보도록 하겠습니다.

사전 지식

Factor Graphs and MAP Esimation

Factor graph는 확률 모델에서 변수 간의 관계를 표현하고 모든 제약조건(factor)을 만족시키는 해를 찾는데 사용되는 구조입니다. 여기서는 Visual-Inertial Odometry(VIO) 문제를 다루기 위해 사용되며 아래의 주요 구성 요소로 이루어져 있습니다.
1.
그래프 노드(Graph node)
2.
그래프 엣지(Graph edge)
IMU preintegration 논문에서 정의한 VIO 문제는 수식 (25)에서 아래와 같이 표현됩니다.
p(Xk∣Zk)∝p(X0)p(Zk∣Xk)=(a)p(X0)∏(i,j)∈Kkp(Ci,Iij∣Xk)=(b)p(X0)∏(i,j)∈Kkp(Iij∣xi,xj)∏i∈Kk∏l∈Cip(zil∣xi). \begin{equation} \begin{aligned} p(\mathcal{X}_k \mid \mathcal{Z}_k) &\propto p(\mathcal{X}_0) p(\mathcal{Z}_k \mid \mathcal{X}_k) \\ &\overset{(a)}{=} p(\mathcal{X}_0) \prod_{(i,j) \in \mathcal{K}_k} p(\mathcal{C}_i, \mathcal{I}_{ij} \mid \mathcal{X}_k) \\ &\overset{(b)}{=} p(\mathcal{X}_0) \prod_{(i,j) \in \mathcal{K}_k} p(\mathcal{I}_{ij} \mid \mathbf{x}_i, \mathbf{x}_j) \prod_{i \in \mathcal{K}_k} \prod_{l \in \mathcal{C}_i} p(\mathbf{z}_{il} \mid \mathbf{x}_i). \end{aligned} \tag{25} \end{equation}
[KOR] ORB-SLAM3 IMU preintegration code review (2)

소개

최근 IMU preintegration에 대해서 공부하다가 문득 ORB-SLAM3는 어떻게 IMU preintegration이 구현되어 있을지 궁금해서 분석해보았습니다. IMU preintegration에 대한 이론적인 부분은 형태님의 블로그에 이미 잘 설명이 되어있기 때문에 그 쪽을 참고하시면 좋을것 같고 저는 IMU preintegration 논문과 ORB-SLAM3 코드를 대조해보면서 논문의 수식이 코드에서 어떻게 구현되어 있는지를 살펴보았습니다.
Imu preintegration 부분을 하나의 글로 다 써내려가기는 시간이 많이 걸릴거 같아서 아래와 같은 순서의 글로 작성하고자합니다.
1.
IMU 센서 측정 값 integration
2.
Residual 함수 정의
3.
Jacobian 행렬 정의
본 글에서는 그 중 첫번째인 IMU 센서 측정값 integration에 대한 코드 분석 내용을 다룹니다.

코드 분석

IMU 센서 측정 값 integration

먼저 IMU 센서로부터 가속도 및 각속도 측정값이 들어오면 누적하여 키프레임 i와 키프레임 j사이의 measurements를 integration하는 부분에 대해서 살펴보도록 하겠습니다. 이 과정에서 아래와 같은 값들이 계산됩니다.
전체적인 계산은 Preintegrated 클래스의 IntegrateNewMeasurement 함수에서 수행됩니다. 전체 함수 코드를 살펴보면 아래와 같습니다.
[KOR] ORB-SLAM3 IMU preintegration code review (1)