标签：wedge phi ORBSLAM3 tilde 协方差 IMU Delta dt

以双目IMU为例，即以stereo_inertial_tum_vi为入口，在该函数中通过LoadIMU()加载IMU数据至vImuMeas容器中。

然后开始进入SLAM系统的双目跟踪。

SLAM.TrackStereo(imLeft,imRight,tframe,vImuMeas);

如果是单目VIO模式，把IMU数据存储到mlQueueImuData中，

if (mSensor == System::IMU_STEREO)
        for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
            mpTracker->GrabImuData(vImuMeas[i_imu]);

获得IMU数据之后，获得双目图像GrabImageStereo，开始核心函数Track()。

在ORBSLAM3中认为bias在两帧间不变，然后对IMU数据进行预积分 PreintegrateIMU()。

mCurrentFrame.SetNewBias(mpLastKeyFrame->GetImuBias());

因为IMU的频率是大于图像的采集频率，因此在两关键帧之间存在多个IMU数据，将两帧间的imu数据放入mvImuFromLastFrame中，m个imu组数据会有m-1个预积分量，针对预积分位置的不同做不同中值积分的处理IntegrateNewMeasurement()。

---

Step 1.构造协方差矩阵，可参考博客VIO残差函数的构建以及IMU预积分和协方差传递
误差的传递由两部分组成：1.当前时刻的误差传递给下一时刻，2.当前时刻测量噪声传递给下一时刻。
协方差矩阵可以通过递推 Σ y = A Σ x A ⊤ Σ_y = AΣ_xA^⊤ Σy​=AΣx​A⊤计算得到： Σ i k = A k − 1 Σ i k − 1 A k − 1 ⊤ + B k − 1 Σ n B k − 1 ⊤ Σ_{ik} = {\color{red} A_{k−1}Σ_{ik−1}A^⊤_{k−1}} +{\color{blue} B_{k−1}Σ_nB^⊤_{k−1}} Σik​=Ak−1​Σik−1​Ak−1⊤​+Bk−1​Σn​Bk−1⊤​其中， Σ n Σ_n Σn​ 是测量噪声的协方差矩阵，方差从 i i i 时刻开始进行递推， Σ i i = 0 Σ_{ii} = 0 Σii​=0。

cv::Mat A = cv::Mat::eye(9,9,CV_32F);
cv::Mat B = cv::Mat::zeros(9,6,CV_32F);

根据没有更新的旋转矩阵 d R dR dR来更新 d P dP dP与 d V dV dV ，

dP = dP + dV*dt + 0.5f*dR*acc*dt*dt;
dV = dV + dR*acc*dt;

因为本次测量值 a c c acc acc为当前坐标系下的加速度，需要将其转换到上一次IMU坐标系下，因此需要乘上未更新的旋转矩阵，计算得到相对于上一帧的位置和速度。

见邱笑晨预积分推导P12，已知矩阵A和B的形式为：
A j − 1 = [ Δ R ~ j j − 1 0 0 − Δ R ~ i j − 1 ( f ~ j − 1 − b i a ) ∧ Δ t I 0 − 1 2 Δ R ~ i j − 1 ( f ~ j − 1 − b i a ) ∧ Δ t 2 Δ t I I ] A_{j-1}=\begin{bmatrix} \Delta \tilde R_{jj-1} & 0 & 0 \\ -\Delta \tilde R_{ij-1}(\tilde f_{j-1}-b^a_i)^\wedge \Delta t & I & 0 \\-{1\over 2}\Delta \tilde R_{ij-1}(\tilde f_{j-1}-b^a_i)^\wedge \Delta t^2 & \Delta tI &I \\ \end{bmatrix} Aj−1​=⎣⎡​ΔR~jj−1​−ΔR~ij−1​(f~​j−1​−bia​)∧Δt−21​ΔR~ij−1​(f~​j−1​−bia​)∧Δt2​0IΔtI​00I​⎦⎤​ B = [ Δ R ~ j j − 1 Δ t 0 0 − Δ R ~ i j − 1 Δ t 0 − 1 2 Δ R ~ i j − 1 Δ t 2 ] B=\begin{bmatrix} \Delta \tilde R_{jj-1}\Delta t & 0 \\ 0& -\Delta \tilde R_{ij-1}\Delta t \\ 0& -{1\over 2}\Delta \tilde R_{ij-1}\Delta t^2 \\ \end{bmatrix} B=⎣⎡​ΔR~jj−1​Δt00​0−ΔR~ij−1​Δt−21​ΔR~ij−1​Δt2​⎦⎤​
其中 ( f ~ j − 1 − b i a ) ∧ (\tilde f_{j-1}-b^a_i)^\wedge (f~​j−1​−bia​)∧即为Wacc：
w ∧ = [ w 1 w 2 w 3 ] ∧ = [ 0 − w 3 w 2 w 3 0 − w 1 − w 2 w 1 0 ] = W w^\wedge=\begin{bmatrix} w_1 \\ w_2 \\w_3 \\ \end{bmatrix}^\wedge = \begin{bmatrix} 0 & -w_3 & w_2 \\ w_3 & 0 & -w_1 \\ -w_2 & w_1 & 0 \\ \end{bmatrix}=W w∧=⎣⎡​w1​w2​w3​​⎦⎤​∧=⎣⎡​0w3​−w2​​−w3​0w1​​w2​−w1​0​⎦⎤​=W
Δ R ~ j j − 1 \Delta \tilde R_{jj-1} ΔR~jj−1​即旋转量预积分测量值，它由陀螺仪测量值和对陀螺 bias 的估计或猜测计算得到，这里就是dR。

cv::Mat Wacc = (cv::Mat_<float>(3,3) << 0, -acc.at<float>(2), acc.at<float>(1),
                                           acc.at<float>(2), 0, -acc.at<float>(0),
                                           -acc.at<float>(1), acc.at<float>(0), 0);
A.rowRange(3,6).colRange(0,3) = -dR*dt*Wacc;
A.rowRange(6,9).colRange(0,3) = -0.5f*dR*dt*dt*Wacc;
A.rowRange(6,9).colRange(3,6) = cv::Mat::eye(3,3,CV_32F)*dt;
B.rowRange(3,6).colRange(3,6) = dR*dt;
B.rowRange(6,9).colRange(3,6) = 0.5f*dR*dt*dt;

然后求解 Δ R ~ j j − 1 \Delta \tilde R_{jj-1} ΔR~jj−1​和 Δ R ~ j j − 1 \Delta \tilde R_{jj-1} ΔR~jj−1​

Step 2. 构造函数，会根据更新后的bias进行角度积分
IntegratedRotation dRi(angVel,b,dt)根据更新后的bias进行角度积分。
可参考frost的论文对这部分的求解：
e x p ( ϕ ∧ ) = I + s i n ( ∥ ϕ ∥ ) ∥ ϕ ∥ ϕ ∧ + 1 − c o s ( ∥ ϕ ∥ ) ∥ ϕ ∥ 2 ( ϕ ∧ ) 2 exp(\phi^\wedge)=I+{sin(\lVert \phi \rVert) \over \lVert \phi \rVert} \phi^\wedge+{1-cos(\lVert \phi \rVert) \over \lVert \phi \rVert ^2} (\phi^\wedge)^2 exp(ϕ∧)=I+∥ϕ∥sin(∥ϕ∥)​ϕ∧+∥ϕ∥21−cos(∥ϕ∥)​(ϕ∧)2
对应的代码为：

const float d2 = x*x+y*y+z*z;
const float d = sqrt(d2);
cv::Mat W = (cv::Mat_<float>(3,3) << 0, -z, y,
               z, 0, -x,
               -y,  x, 0);
deltaR = I + W*sin(d)/d + W*W*(1.0f-cos(d))/d2;

根据罗德里格斯公式求极限，当 d < e p s = 1 e − 4 d < eps = 1e-4 d<eps=1e−4时，后面的高阶小量忽略掉得到此式。此时指数映射的一阶近似是：
e x p ( ϕ ∧ ) = I + ϕ ∧ exp(\phi^\wedge)=I+\phi^\wedge exp(ϕ∧)=I+ϕ∧
对应的代码为：

if(d<eps)
  {
      deltaR = I + W;
      rightJ = cv::Mat::eye(3,3,CV_32F);
  }

根据公式8：
J r ( ϕ ) = I − 1 − c o s ( ∥ ϕ ∥ ) ∥ ϕ ∥ 2 ( ϕ ∧ ) + ∥ ϕ ∥ − s i n ( ∥ ϕ ∥ ) ∥ ϕ ∥ 3 ( ϕ ∧ ) 2 J_r(\phi)=I-{1-cos(\lVert \phi \rVert) \over \lVert \phi \rVert ^2} (\phi^\wedge)+{\lVert \phi \rVert - sin(\lVert \phi \rVert) \over \lVert \phi \rVert ^3} (\phi^\wedge)^2 Jr​(ϕ)=I−∥ϕ∥21−cos(∥ϕ∥)​(ϕ∧)+∥ϕ∥3∥ϕ∥−sin(∥ϕ∥)​(ϕ∧)2

rightJ = I - W*(1.0f-cos(d))/d2 + W*W*(d-sin(d))/(d2*d);

然后强行归一化使dRi符合旋转矩阵的格式，然后继续填充到AB矩阵中。

dR = NormalizeRotation(dR*dRi.deltaR);
A.rowRange(0,3).colRange(0,3) = dRi.deltaR.t();
B.rowRange(0,3).colRange(0,3) = dRi.rightJ*dt;

Step 3.更新协方差
根据公式更新协方差矩阵：

 C.rowRange(0,9).colRange(0,9) = A*C.rowRange(0,9).colRange(0,9)*A.t() + B*Nga*B.t();

这一部分最开始是0矩阵，随着积分次数增加，每次都加上随机游走，偏置的信息矩阵

 C.rowRange(9,15).colRange(9,15) = C.rowRange(9,15).colRange(9,15) + NgaWalk;

---

关于IMU预测位姿两个地方会用到：

1. 匀速模型计算速度，但并没有给当前帧位姿赋值；
2. 跟踪丢失时不直接判定丢失，通过这个函数预测当前帧位姿看看能不能拽回来，代替纯视觉中的重定位。

计算当前帧在世界坐标系的位姿,原理都是用预积分的位姿（预积分的值不会变化）与上一帧的位姿（会迭代变化）进行更新 。
参考 ORB-SLAM3详细注释的代码持续更新

标签：wedge,phi,ORBSLAM3,tilde,协方差,IMU,Delta,dt
来源： https://blog.csdn.net/qq_39266065/article/details/115703313

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。