[ENG] ORB-SLAM3 IMU preintegration code review (1)

날짜
2025/01/05
4 more properties

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
1.
The relative state (rotation, translation, velocity) values between the two keyframes.
2.
The covariance matrix used for noise propagation to perform optimization later on
3.
A Jacobian matrix for updating the bias.
The overall calculation is performed in the IntegrateNewMeasurement function of the Preintegrated class. The complete function code is shown below.
void Preintegrated::IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt) { mvMeasurements.push_back(integrable(acceleration,angVel,dt)); // Position is updated firstly, as it depends on previously computed velocity and rotation. // Velocity is updated secondly, as it depends on previously computed rotation. // Rotation is the last to be updated. //Matrices to compute covariance Eigen::Matrix<float,9,9> A; A.setIdentity(); Eigen::Matrix<float,9,6> B; B.setZero(); Eigen::Vector3f acc, accW; acc << acceleration(0)-b.bax, acceleration(1)-b.bay, acceleration(2)-b.baz; accW << angVel(0)-b.bwx, angVel(1)-b.bwy, angVel(2)-b.bwz; avgA = (dT*avgA + dR*acc*dt)/(dT+dt); avgW = (dT*avgW + accW*dt)/(dT+dt); // Update delta position dP and velocity dV (rely on no-updated delta rotation) dP = dP + dV*dt + 0.5f*dR*acc*dt*dt; dV = dV + dR*acc*dt; // Compute velocity and position parts of matrices A and B (rely on non-updated delta rotation) Eigen::Matrix<float,3,3> Wacc = Sophus::SO3f::hat(acc); A.block<3,3>(3,0) = -dR*dt*Wacc; A.block<3,3>(6,0) = -0.5f*dR*dt*dt*Wacc; A.block<3,3>(6,3) = Eigen::DiagonalMatrix<float,3>(dt, dt, dt); B.block<3,3>(3,3) = dR*dt; B.block<3,3>(6,3) = 0.5f*dR*dt*dt; // Update position and velocity jacobians wrt bias correction JPa = JPa + JVa*dt -0.5f*dR*dt*dt; JPg = JPg + JVg*dt -0.5f*dR*dt*dt*Wacc*JRg; JVa = JVa - dR*dt; JVg = JVg - dR*dt*Wacc*JRg; // Update delta rotation IntegratedRotation dRi(angVel,b,dt); dR = NormalizeRotation(dR*dRi.deltaR); // Compute rotation parts of matrices A and B A.block<3,3>(0,0) = dRi.deltaR.transpose(); B.block<3,3>(0,0) = dRi.rightJ*dt; // Update covariance C.block<9,9>(0,0) = A * C.block<9,9>(0,0) * A.transpose() + B*Nga*B.transpose(); C.block<6,6>(9,9) += NgaWalk; // Update rotation jacobian wrt bias correction JRg = dRi.deltaR.transpose()*JRg - dRi.rightJ*dt; // Total integrated time dT += dt; }
C++
복사
It's hard to understand at a glance because it's a jumbled mess of calculations, so let's take a look at the three parts mentioned above in order.

Relative motion increments

In the Imu preintegration paper, formula (33) defines the formula for relative motion increments.
First, the term for position during relative motion increments Δpij≐∑k=ij−1[ΔvikΔt+12ΔRik(a~k−bka−ηkad)Δt2]\Delta \mathbf{p}_{ij} \doteq \sum_{k=i}^{j-1} \left[ \Delta \mathbf{v}_{ik} \Delta t + \frac{1}{2} \Delta \mathbf{R}_{ik} \left( \tilde{\mathbf{a}}_k - \mathbf{b}^a_k - \boldsymbol{\eta}^{ad}_k \right) \Delta t^2 \right]in the code.
dP = dP + dV*dt + 0.5f*dR*acc*dt*dt;
C++
복사
Where acc represents the acceleration value in the form with the bias bka\mathbf{b}^a_kremoved. The noise ηkad\boldsymbol{\eta}^{ad}_kis modeled separately and used to create the covariance matrix, so we don't need to consider it here. And you'll notice that the Σ\Sigma symbol, which represents the sum, is expressed in the code as dP = dP + something.
Next, we need to add the terms for velocity, Δvij≐∑k=ij−1ΔRik(a~k−bka−ηkad)Δt\Delta \mathbf{v}_{ij} \doteq \sum_{k=i}^{j-1} \Delta \mathbf{R}{ik} \left( \tilde{\mathbf{a}}_k - \mathbf{b}^a_k - \boldsymbol{\eta}^{ad}_k \right) \Delta tin the code looks like this, where acc can also be thought of as the expression mentioned above.
dV = dV + dR*acc*dt;
C++
복사
Next, we need to find the term for rotation, ΔRij≐Ri⊤Rj=∏k=ij−1Exp((ω~k−bkg−ηkgd)Δt)\Delta \mathbf{R}_{ij} \doteq \mathbf{R}_i^\top \mathbf{R}_j = \prod_{k=i}^{j-1} \mathrm{Exp} \left( \left( \tilde{\boldsymbol{\omega}}_k - \mathbf{b}^g_k - \boldsymbol{\eta}^{gd}_k \right) \Delta t \right)in the code.
IntegratedRotation dRi(angVel,b,dt); dR = NormalizeRotation(dR*dRi.deltaR);
C++
복사
This part looks a little different from the code in the paper, but if you look at the constructor part of the IntegratedRotation class, you can see that the formula is written in the paper.
IntegratedRotation::IntegratedRotation(const Eigen::Vector3f &angVel, const Bias &imuBias, const float &time) { const float x = (angVel(0)-imuBias.bwx)*time; const float y = (angVel(1)-imuBias.bwy)*time; const float z = (angVel(2)-imuBias.bwz)*time; const float d2 = x*x+y*y+z*z; const float d = sqrt(d2); Eigen::Vector3f v; v << x, y, z; Eigen::Matrix3f W = Sophus::SO3f::hat(v); if(d<eps) { deltaR = Eigen::Matrix3f::Identity() + W; rightJ = Eigen::Matrix3f::Identity(); } else { deltaR = Eigen::Matrix3f::Identity() + W*sin(d)/d + W*W*(1.0f-cos(d))/d2; rightJ = Eigen::Matrix3f::Identity() - W*(1.0f-cos(d))/d2 + W*W*(d-sin(d))/(d2*d); } }
C++
복사
Here, the (ω~k−bkg−ηkgd)\left( \tilde{\boldsymbol{\omega}}_k - \mathbf{b}^g_k - \boldsymbol{\eta}^{gd}_k \right)) part of the formula is the code below, and the
const float x = (angVel(0)-imuBias.bwx)*time; const float y = (angVel(1)-imuBias.bwy)*time; const float z = (angVel(2)-imuBias.bwz)*time;
C++
복사
The code below is where the Exp()\mathrm{Exp}()function is written to represent the exponential map.
Eigen::Matrix3f W = Sophus::SO3f::hat(v); if(d<eps) { deltaR = Eigen::Matrix3f::Identity() + W; rightJ = Eigen::Matrix3f::Identity(); } else { deltaR = Eigen::Matrix3f::Identity() + W*sin(d)/d + W*W*(1.0f-cos(d))/d2; rightJ = Eigen::Matrix3f::Identity() - W*(1.0f-cos(d))/d2 + W*W*(d-sin(d))/(d2*d); }
C++
복사
Exponential maps are well described in the paper in equation (3).
Using these terms, we can accumulate the continuously incoming IMU sensor acceleration and angular velocity values and estimate the relative motion between keyframes i and j. This relative motion is later used to perform bundle adjustment using keyframes, or to perform pose estimation for a single camera frame.

Covariance from noise propagation

Next, we need matrices A and B for noise propagation, which we talk about in the paper in equation (62).
ηijΔ=Aj−1ηij−1Δ+Bj−1ηj−1d,(62)\boldsymbol{\eta}_{ij}^\Delta = \mathbf{A}_{j-1} \boldsymbol{\eta}_{ij-1}^\Delta + \mathbf{B}_{j-1} \boldsymbol{\eta}_{j-1}^d, \tag{62}
However, the paper doesn't explicitly say how the matrices A and B are constructed; instead, the paper says that they are
Therefore, by the definitions of ηijΔ\boldsymbol{\eta}_{ij}^\Delta ηj−1d\boldsymbol{\eta}_{j-1}^dit follows from equations (59)-(61) that [δϕij−1,δvij−1,δpij−1][\delta \boldsymbol{\phi}_{ij-1}, \delta \mathbf{v}_{ij-1}, \delta \mathbf{p}_{ij-1}]and [ηj−1gd,ηj−1ad][\boldsymbol{\eta}_{j-1}^{gd}, \boldsymbol{\eta}_{j-1}^{ad}]], and the coefficient terms multiplied by [ηj−1gd,ηj−1ad][\boldsymbol{\eta}_{j-1}^{gd}, \boldsymbol{\eta}_{j-1}^{ad}]].
[δϕijδvijδpij]=[ΔR~j−1j⊤00−ΔR~ij−1(a~j−1−bia)ΔtI0−12ΔR~ij−1(a~j−1−bia)Δt2ΔtII][δϕij−1δvij−1δpij−1]+[Jrj−100ΔR~ij−1Δt012ΔR~ij−1Δt2][ηj−1gdηj−1ad]\begin{bmatrix}\delta \boldsymbol{\phi}_{ij} \\\delta \mathbf{v}_{ij} \\\delta \mathbf{p}_{ij}\end{bmatrix}=\begin{bmatrix}\Delta \tilde{\mathbf{R}}_{j-1j}^\top & \mathbf{0} & \mathbf{0} \\-\Delta \tilde{\mathbf{R}}_{ij-1} \left( \tilde{\mathbf{a}}_{j-1} - \mathbf{b}^a_i \right) \Delta t & \mathbf{I} & \mathbf{0} \\-\frac{1}{2} \Delta \tilde{\mathbf{R}}_{ij-1} \left( \tilde{\mathbf{a}}_{j-1} - \mathbf{b}^a_i \right) \Delta t^2 & \Delta t \mathbf{I} & \mathbf{I}\end{bmatrix}\begin{bmatrix}\delta \boldsymbol{\phi}_{ij-1} \\\delta \mathbf{v}_{ij-1} \\\delta \mathbf{p}_{ij-1}\end{bmatrix}+\begin{bmatrix}\mathbf{J}_{r}^{j-1} & \mathbf{0} \\\mathbf{0} & \Delta \tilde{\mathbf{R}}_{ij-1} \Delta t \\\mathbf{0} & \frac{1}{2}\Delta \tilde{\mathbf{R}}_{ij-1} \Delta t^2\end{bmatrix}\begin{bmatrix}\boldsymbol{\eta}_{j-1}^{gd} \\\boldsymbol{\eta}_{j-1}^{ad}\end{bmatrix}
If we look in the code for the parts of the formula that correspond to matrices A and B, we can see that
// Compute velocity and position parts of matrices A and B (rely on non-updated delta rotation) Eigen::Matrix<float,3,3> Wacc = Sophus::SO3f::hat(acc); A.block<3,3>(3,0) = -dR*dt*Wacc; A.block<3,3>(6,0) = -0.5f*dR*dt*dt*Wacc; A.block<3,3>(6,3) = Eigen::DiagonalMatrix<float,3>(dt, dt, dt); B.block<3,3>(3,3) = dR*dt; B.block<3,3>(6,3) = 0.5f*dR*dt*dt; (...) // Compute rotation parts of matrices A and B A.block<3,3>(0,0) = dRi.deltaR.transpose(); B.block<3,3>(0,0) = dRi.rightJ*dt;
C++
복사
If we compare the matrices A and B, we can see that the code is written the same as in the paper.
With matrices A and B organized, we can now find the covariance matrix.
Σij=Aj−1Σij−1Aj−1⊤+Bj−1ΣηBj−1⊤(63)\Sigma_{ij} = \mathbf{A}_{j-1} \Sigma_{ij-1} \mathbf{A}_{j-1}^\top + \mathbf{B}_{j-1} \Sigma_{\eta} \mathbf{B}_{j-1}^\top \tag{63}
// Update covariance C.block<9,9>(0,0) = A * C.block<9,9>(0,0) * A.transpose() + B*Nga*B.transpose(); C.block<6,6>(9,9) += NgaWalk;
C++
복사
Note that the Σ\Sigmafor the covariance matrix is represented by C in the code.

Jacobians used for the a bias update

Next, we need to calculate the Jacobians for the bias update. This part is unnumbered in the paper and is located above equation (70) in the Appendix. This is the result of differentiating the residual function by the acceleration and angular velocity bias terms.
∂ΔRˉij∂bg=−∑k=ij−1[ΔR~k+1:j(bˉi)⊤JrkΔt],∂Δvˉij∂ba=−∑k=ij−1ΔRˉikΔt,∂Δvˉij∂bg=−∑k=ij−1ΔRˉik(a~k−bˉia)∧∂ΔRˉik∂bgΔt,∂Δpˉij∂bg=∑k=ij−1∂Δvˉik∂bgΔt−12ΔRˉikΔt2,∂Δpˉij∂ba=∑k=ij−1∂Δvˉik∂baΔt−12ΔRˉik(a~k−bˉia)∧∂ΔRˉik∂bgΔt2.\begin{aligned}\frac{\partial \Delta \bar{\mathbf{R}}_{ij}}{\partial \mathbf{b}^g} &=- \sum_{k=i}^{j-1} \left[ \Delta \tilde{\mathbf{R}}_{k+1:j} (\bar{\mathbf{b}}_i)^\top \mathbf{J}_r^k \Delta t \right], \\\frac{\partial \Delta \bar{\mathbf{v}}_{ij}}{\partial \mathbf{b}^a} &=- \sum_{k=i}^{j-1} \Delta \bar{\mathbf{R}}_{ik} \Delta t, \\\frac{\partial \Delta \bar{\mathbf{v}}_{ij}}{\partial \mathbf{b}^g} &=- \sum_{k=i}^{j-1} \Delta \bar{\mathbf{R}}_{ik} \left( \tilde{\mathbf{a}}_k - \bar{\mathbf{b}}_i^a \right)^\wedge \frac{\partial \Delta \bar{\mathbf{R}}_{ik}}{\partial \mathbf{b}^g} \Delta t, \\\frac{\partial \Delta \bar{\mathbf{p}}_{ij}}{\partial \mathbf{b}^g} &=\sum_{k=i}^{j-1} \frac{\partial \Delta \bar{\mathbf{v}}_{ik}}{\partial \mathbf{b}^g} \Delta t - \frac{1}{2} \Delta \bar{\mathbf{R}}_{ik} \Delta t^2, \\\frac{\partial \Delta \bar{\mathbf{p}}_{ij}}{\partial \mathbf{b}^a} &=\sum_{k=i}^{j-1} \frac{\partial \Delta \bar{\mathbf{v}}_{ik}}{\partial \mathbf{b}^a} \Delta t - \frac{1}{2} \Delta \bar{\mathbf{R}}_{ik} \left( \tilde{\mathbf{a}}_k - \bar{\mathbf{b}}_i^a \right)^\wedge \frac{\partial \Delta \bar{\mathbf{R}}_{ik}}{\partial \mathbf{b}^g} \Delta t^2.\end{aligned}
// Update position and velocity jacobians wrt bias correction JPa = JPa + JVa*dt -0.5f*dR*dt*dt; JPg = JPg + JVg*dt -0.5f*dR*dt*dt*Wacc*JRg; JVa = JVa - dR*dt; JVg = JVg - dR*dt*Wacc*JRg; (...) // Update rotation jacobian wrt bias correction JRg = dRi.deltaR.transpose()*JRg - dRi.rightJ*dt;
C++
복사
Where JPa is ∂Δpˉij∂ba\frac{\partial \Delta \bar{\mathbf{p}}_{ij}}{\partial \mathbf{b}^a}, JPg is ∂Δpˉij∂bg\frac{\partial \Delta \bar{\mathbf{p}}_{ij}}{\partial \mathbf{b}^g}, JVa is ∂Δvˉij∂ba\frac{\partial \Delta \bar{\mathbf{v}}_{ij}}{\partial \mathbf{b}^a}, JVg represents ∂Δvˉij∂bg\frac{\partial \Delta \bar{\mathbf{v}}_{ij}}{\partial \mathbf{b}^g}, and JRg represents ∂ΔRˉij∂bg\frac{\partial \Delta \bar{\mathbf{R}}_{ij}}{\partial \mathbf{b}^g}. As you can see, the rotation values are only affected by the angular velocity bias, so we only perform a partial differentiation for the angular velocity bias. However, the velocity and position values are affected by both acceleration and angular velocity biases, so we perform a partial differentiation on both terms.
The Jacobian matrices here will be used to find the residual error, which will be discussed later.

Wrapping up

We have seen how the first part of the imu preintegration in the ORB-SLAM3 code, the IMU sensor measurement integration, is implemented. In the measurement integration part, we (1) compute the relative state (rotation, translation, velocity) values between two keyframes, (2) obtain the covariance matrix that is used to perform the optimization through noise propagation. We also (3) compute the Jacobian matrix for updating the bias and use these values later when performing bundle adjustment using keyframes, or when performing pose estimation for a single camera frame.
Next time, we will see how the Jacobian matrix is implemented in the paper to define and optimize the residual function between two keyframes using these defined relative motion increments.
Thanks for reading this long post :-)