ORB-SLAM3中IMU预积分
文章目录
- ORB-SLAM3中IMU预积分
- 1读取IMU参数
- 2 预积分噪声递推与预测
- 2.1 更新预积分测量值dP和dV
- 2.2 更新预积分噪声项的递推模型的矩阵A和B部分元素
- 2.3 位置、速度预积分测量值对零偏的雅可比矩阵
- 2.4 更新旋转预积分测量
- 2.5 补充A和B矩阵剩余部分，更新协方差
- 2.6 旋转预积分量对零偏的雅可比矩阵
- 3 预积分零偏更新
- 4 预积分残差雅可比计算
IMU预积分之前也博客也提到几次了，这次把ORB-SLAM3中相应的流程记录下，与推导公式一一对应，顺带复习。
1读取IMU参数
Tracking::newParameterLoader和ORB2类似，都在这个追踪里面读取参数，这里会把读取的连续噪声离散化！
//IMU parameters
// 3. 读取imu参数
Sophus::SE3f Tbc = settings->Tbc();
mInsertKFsLost = settings->insertKFsWhenLost();
mImuFreq = settings->imuFrequency();
mImuPer = 0.001; //1.0 / (double) mImuFreq; //TODO: ESTO ESTA BIEN?
float Ng = settings->noiseGyro();
float Na = settings->noiseAcc();
float Ngw = settings->gyroWalk();
float Naw = settings->accWalk();
// 把配置文件中的连续噪声离散化！！！！！！！！
const float sf = sqrt(mImuFreq);
mpImuCalib = new IMU::Calib(Tbc,Ng*sf,Na*sf,Ngw/sf,Naw/sf);
mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib);
读取得参数被扔到这里构建协方差
/**
* @brief imu标定参数的构造函数
* @param calib imu标定参数
*/
Calib::Calib(const Calib &calib)
{
mbIsSet = calib.mbIsSet;
// Sophus/Eigen parameters
mTbc = calib.mTbc;
mTcb = calib.mTcb;
Cov = calib.Cov;
CovWalk = calib.CovWalk;
}
Calib::Set
/**
* @brief 设置参数
* @param Tbc_ 位姿变换
* @param ng 噪声
* @param na 噪声
* @param ngw 随机游走
* @param naw 随机游走
*/
void Calib::Set(const Sophus::SE3<float> &sophTbc, const float &ng, const float &na, const float &ngw, const float &naw)
{
mbIsSet = true;
const float ng2 = ng * ng;
const float na2 = na * na;
const float ngw2 = ngw * ngw;
const float naw2 = naw * naw;
// Sophus/Eigen
mTbc = sophTbc;
mTcb = mTbc.inverse();
// 噪声协方差
Cov.diagonal() << ng2, ng2, ng2, na2, na2, na2;
// 随机游走协方差
CovWalk.diagonal() << ngw2, ngw2, ngw2, naw2, naw2, naw2;
}
赋值校准后IMU噪声协方差矩阵
Preintegrated::Preintegrated(const Bias &b_, const Calib &calib)
{
Nga = calib.Cov;
NgaWalk = calib.CovWalk;
Initialize(b_);
}
2 预积分噪声递推与预测
写在前面，这里想简单的把ORB-SLAM3中imu部分公式与代码相对应，理清楚顺序关系。首先给出预积分中相关变量的定义
float dT;
Eigen::Matrix<float,15,15> C;
Eigen::Matrix<float,15,15> Info;
Eigen::DiagonalMatrix<float,6> Nga, NgaWalk; // δa δg δba δbg对应得协方差矩阵
// Values for the original bias (when integration was computed)
Bias b;
Eigen::Matrix3f dR; // 预积分、
Eigen::Vector3f dV, dP;
Eigen::Matrix3f JRg, JVg, JVa, JPg, JPa; // 预积分量对零偏得雅可比
Eigen::Vector3f avgA, avgW;
2.1 更新预积分测量值dP和dV
由于位置预积分测量值依赖“旧”的速度和旋转，速度预积分测量值依赖“旧”的旋转，因此这里的计算顺序不能乱，先用“旧”数据计算位置预积分测量值，再计算速度预积分测量值。然后计算A和B矩阵中“旧”数据对应的元素。
下面这个函数中传入的IMU数据是经过线性插值后，进行中值积分得到的，插值主要是在两个图像帧附近。
ImuTypes.cc
预积分 = 预积分测量 + 预积分噪声
位置预积分测量值（是利用“旧”的旋转和“旧”的速度）
Δ p ~ i j ≐ ∑ k = i j − 1 [ Δ v ~ i k Δ t + 1 2 Δ R ~ i k ( a ~ k − b i a ) Δ t 2 ] \Delta\tilde{\boldsymbol{p}}_{ij}\doteq\sum_{k=i}^{j-1}\left[\Delta\tilde{\boldsymbol{v}}_{ik}\Delta t+\frac{1}{2}\Delta\tilde{\boldsymbol{R}}_{ik}\left(\tilde{\boldsymbol{a}}_{k}-\boldsymbol{b}_{i}^{a}\right)\Delta t^{2}\right] Δp~ij≐k=i∑j−1[Δv~ikΔt+21ΔR~ik(a~k−bia)Δt2]
速度预积分测量值（利用“旧”的旋转）
Δ v ~ i j = ∑ k = i j − 1 Δ R ~ i k ( a ~ k − b i a ) Δ t \Delta\tilde{\boldsymbol{v}}_{ij}=\sum_{k=i}^{j-1}\Delta\tilde{\boldsymbol{R}}_{ik}\left(\tilde{\boldsymbol{a}}_k-\boldsymbol{b}_i^a\right)\Delta t Δv~ij=k=i∑j−1ΔR~ik(a~k−bia)Δt
/**
* @brief 预积分计算，更新noise
*
* @param[in] acceleration 加速度计数据
* @param[in] angVel 陀螺仪数据
* @param[in] dt 两图像 帧之间时间差
*/
void Preintegrated::IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt)
{ // 考虑偏置后的加速度、角速度
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)
// 根据没有更新的dR来更新dP与dV
dP = dP + dV * dt + 0.5f * dR * acc * dt * dt;
dV = dV + dR * acc * dt;
...
}
2.2 更新预积分噪声项的递推模型的矩阵A和B部分元素
注意，A和B矩阵的部分元素使用的是“旧”数据，先更新这部分元素。
矩阵A
矩阵B
void Preintegrated::IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt){
...
// Step 1.构造协方差矩阵
// 噪声矩阵的传递矩阵，这部分用于计算i到j-1历史噪声或者协方差
Eigen::Matrix<float, 9, 9> A;
A.setIdentity();
// 噪声矩阵的传递矩阵，这部分用于计算j-1新的噪声或协方差，这两个矩阵里面的数都是当前时刻的，计算主要是为了下一时刻使用
Eigen::Matrix<float, 9, 6> B;
B.setZero();
// 根据η_ij = A * η_i,j-1 + B_j-1 * η_j-1中的Ａ矩阵和Ｂ矩阵对速度和位移进行更新
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;
...
}
2.3 位置、速度预积分测量值对零偏的雅可比矩阵
用递推方式计算零偏更新后，预积分量对零偏的雅可比矩阵。先计算位置预积分量对零偏的雅可比矩阵，再计算速度预积分量对零偏的雅可比矩阵.当“旧”的旋转预积分测量值不再需要时再更新它。
位置
∂ Δ p ~ i j ∂ b g = ∑ k = i j − 1 ( ∂ Δ v ~ i k ∂ b g Δ t − 1 2 Δ R ~ i k ( a ~ k − b i a ) ∧ ∂ Δ R ~ i k ∂ b g Δ t 2 ) = ∂ Δ p ~ i , j − 1 ∂ b g + ( ∂ Δ v ~ i , j − 1 ∂ b g Δ t − 1 2 Δ R ~ i , j − 1 ( a ~ j − 1 − b i a ) ∧ ∂ Δ R ~ i , j − 1 ∂ b g Δ t 2 ) ∂ Δ p ~ i j ∂ b a = ∑ k = i j − 1 ( ∂ Δ v ~ i k ∂ b a Δ t − 1 2 Δ R ~ i k Δ t 2 ) = ∂ Δ p ~ i , j − 1 ∂ b a + ( ∂ Δ v ~ i , j − 1 ∂ b a Δ t − 1 2 Δ R ~ i , j − 1 Δ t 2 ) (15-67) \begin{aligned} &\frac{\partial\Delta\tilde{\boldsymbol{p}}_{ij}}{\partial b^g} =\sum_{k=i}^{j-1}\left(\frac{\partial\Delta\tilde{\boldsymbol{v}}_{ik}}{\partial\boldsymbol{b}^{g}}\Delta t-\frac{1}{2}\Delta\tilde{\boldsymbol{R}}_{ik}\left(\tilde{\boldsymbol{a}}_{k}-\boldsymbol{b}_{i}^{a}\right)^{\wedge}\frac{\partial\Delta\tilde{\boldsymbol{R}}_{ik}}{\partial\boldsymbol{b}^{g}}\Delta t^{2}\right) \\ &=\frac{\partial\Delta\tilde{\boldsymbol{p}}_{i,j-1}}{\partial\boldsymbol{b}^{g}}+\left(\frac{\partial\Delta\tilde{\boldsymbol{v}}_{i,j-1}}{\partial\boldsymbol{b}^{g}}\Delta t-\frac{1}{2}\Delta\tilde{\boldsymbol{R}}_{i,j-1}\left(\tilde{\boldsymbol{a}}_{j-1}-\boldsymbol{b}_{i}^{a}\right)^{\wedge}\frac{\partial\Delta\tilde{\boldsymbol{R}}_{i,j-1}}{\partial\boldsymbol{b}^{g}}\Delta t^{2}\right) \\ &\frac{\partial\Delta\tilde{p}_{ij}}{\partial b^a} =\sum_{k=i}^{j-1}\left(\frac{\partial\Delta\tilde{\boldsymbol{v}}_{ik}}{\partial\boldsymbol{b}^a}\Delta t-\frac12\Delta\tilde{\boldsymbol{R}}_{ik}\Delta t^2\right) \\ &=\frac{\partial\Delta\tilde{\boldsymbol{p}}_{i,j-1}}{\partial\boldsymbol{b}^{a}}+\left(\frac{\partial\Delta\tilde{\boldsymbol{v}}_{i,j-1}}{\partial\boldsymbol{b}^{a}}\Delta t-\frac{1}{2}\Delta\tilde{\boldsymbol{R}}_{i,j-1}\Delta t^{2}\right) \text{(15-67)} \end{aligned} ∂bg∂Δp~ij=k=i∑j−1(∂bg∂Δv~ikΔt−21ΔR~ik(a~k−bia)∧∂bg∂ΔR~ikΔt2)=∂bg∂Δp~i,j−1+(∂bg∂Δv~i,j−1Δt−21ΔR~i,j−1(a~j−1−bia)∧∂bg∂ΔR~i,j−1Δt2)∂ba∂Δp~ij=k=i∑j−1(∂ba∂Δv~ikΔt−21ΔR~ikΔt2)=∂ba∂Δp~i,j−1+(∂ba∂Δv~i,j−1Δt−21ΔR~i,j−1Δt2)(15-67)
速度
∂ Δ v ~ i j ∂ b a = − ∑ k = i j − 1 ( Δ R ~ i k Δ t ) = ∂ Δ v ~ i , j − 1 ∂ b a − Δ R ~ i , j − 1 Δ t ∂ Δ v ~ i j ∂ b g = − ∑ k = i j − 1 ( Δ R ~ i k ( a ~ k − b i a ) ∧ ∂ Δ R ~ i k ∂ b g Δ t ) = ∂ Δ v ~ i , j − 1 ∂ b g − ( Δ R ~ i , j − 1 ( a ~ j − 1 − b i a ) ∧ ∂ Δ R ~ i , j − 1 ∂ b g Δ t ) \begin{aligned} &\frac{\partial\Delta\tilde{\boldsymbol{v}}_{ij}}{\partial b^{a}} =-\sum_{k=i}^{j-1}\left(\Delta\tilde{\boldsymbol{R}}_{ik}\Delta t\right) \\ &=\frac{\partial\Delta\tilde{\boldsymbol{v}}_{i,j-1}}{\partial\boldsymbol{b}^a}-\Delta\tilde{\boldsymbol{R}}_{i,j-1}\Delta t \\ &\frac{\partial\Delta\tilde{\boldsymbol{v}}_{ij}}{\partial b^g} =-\sum_{k=i}^{j-1}\left(\Delta\tilde{\boldsymbol{R}}_{ik}\left(\tilde{\boldsymbol{a}}_{k}-\boldsymbol{b}_{i}^{a}\right)^{\wedge}\frac{\partial\Delta\tilde{\boldsymbol{R}}_{ik}}{\partial\boldsymbol{b}^{g}}\Delta t\right) \\ &=\frac{\partial\Delta\tilde{\boldsymbol{v}}_{i,j-1}}{\partial\boldsymbol{b}^{g}}-\left(\Delta\tilde{\boldsymbol{R}}_{i,j-1}\left(\tilde{\boldsymbol{a}}_{j-1}-\boldsymbol{b}_{i}^{a}\right)^{\wedge}\frac{\partial\Delta\tilde{\boldsymbol{R}}_{i,j-1}}{\partial\boldsymbol{b}^{g}}\Delta t\right) \end{aligned} ∂ba∂Δv~ij=−k=i∑j−1(ΔR~ikΔt)=∂ba∂Δv~i,j−1−ΔR~i,j−1Δt∂bg∂Δv~ij=−k=i∑j−1(ΔR~ik(a~k−bia)∧∂bg∂ΔR~ikΔt)=∂bg∂Δv~i,j−1−(ΔR~i,j−1(a~j−1−bia)∧∂bg∂ΔR~i,j−1Δt)
void Preintegrated::IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt){
...
// Update position and velocity jacobians wrt bias correction
// 因为随着时间推移，不可能每次都重新计算雅克比矩阵，所以需要做J(k+1) = j(k) + (~)这类事，分解方式与AB矩阵相同
// 论文作者对forster论文公式的基础上做了变形，然后递归更新，参见 https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/212
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;
...
}
2.4 更新旋转预积分测量
已经利用完旧的旋转，现在来更新
Δ R ~ i j = ∏ k = i j − 1 Exp ( ( ω ~ k − b i g ) Δ t ) \Delta\tilde{\boldsymbol{R}}_{ij}=\prod_{k=i}^{j-1}\operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_k-\boldsymbol{b}_i^g\right)\Delta t\right) ΔR~ij=k=i∏j−1Exp((ω~k−big)Δt)
void Preintegrated::IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt){
...
// Update delta rotation
// Step 2. 构造函数，会根据更新后的bias进行角度积分
IntegratedRotation dRi(angVel, b, dt);
// 强行归一化使其符合旋转矩阵的格式
dR = NormalizeRotation(dR * dRi.deltaR);
...
}
2.5 补充A和B矩阵剩余部分，更新协方差
用最新的预积分测量值补充更新A和B矩阵中剩余的元素。
矩阵A
矩阵B
噪声递推模型实际并不是真的去递推噪声，而是为了递推预积分噪声协方差矩阵！我们用协方差的形式来记录噪声！
用 Σ i j {\Sigma}_{ij} Σij表示预积分噪声协方差矩阵
C（9×9）， Σ η {\Sigma}_{\boldsymbol{\eta}} Ση表示IMU测量噪声协方差Nga（6×6，只有陀螺仪和加速度计的高斯噪声，偏置游走用另外一个变量NgaWalk表示，就是把测量噪声和偏置噪声分开计算）
Σ i j = A j − 1 Σ i , j − 1 A j − 1 ⊺ + B j − 1 Σ η B j − 1 ⊺ \boldsymbol{\Sigma}_{ij}=\boldsymbol{A}_{j-1}\boldsymbol{\Sigma}_{i,j-1}\boldsymbol{A}_{j-1}^{\intercal}+\boldsymbol{B}_{j-1}\boldsymbol{\Sigma}_{\boldsymbol{\eta}}\boldsymbol{B}_{j-1}^{\intercal} Σij=Aj−1Σi,j−1Aj−1⊺+Bj−1ΣηBj−1⊺
void Preintegrated::IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt){
...
// Compute rotation parts of matrices A and B
// 补充AB矩阵
A.block<3, 3>(0, 0) = dRi.deltaR.transpose();
B.block<3, 3>(0, 0) = dRi.rightJ * dt;
// 小量delta初始为0，更新后通常也为0，故省略了小量的更新
// Step 3.更新协方差，frost经典预积分论文的第63个公式，推导了噪声（ηa, ηg）对dR dV dP 的影响
C.block<9, 9>(0, 0) = A * C.block<9, 9>(0, 0) * A.transpose() + B * Nga * B.transpose(); // B矩阵为9*6矩阵 Nga 6*6对角矩阵，3个陀螺仪噪声的平方，3个加速度计噪声的平方
// 这一部分最开始是0矩阵，随着积分次数增加，每次都加上随机游走，偏置的信息矩阵
C.block<6, 6>(9, 9) += NgaWalk;
...
}
2.6 旋转预积分量对零偏的雅可比矩阵
旋转预积分量对零偏的雅可比矩阵：
∂ Δ R ~ i j ∂ b g = ∑ k = i j − 1 ( − Δ R ~ k + 1 , j ⊤ J r k Δ t ) = ∑ k = i j − 2 ( − Δ R ~ k + 1 , j ⊤ J r k Δ t ) − Δ R ~ j j ⊤ j r j − 1 Δ t = ∑ k = i j − 2 ( − ( Δ R ~ k + 1 , j − 1 Δ R ~ j − 1 , j ) ⊤ J r k Δ t ) − Δ R ~ j j ⊤ J r j − 1 Δ t = Δ R ~ j , j − 1 ∑ k = i j − 2 ( − ( Δ R ~ k + 1 , j − 1 ) ⊤ J r k Δ t ) − J r j − 1 Δ t = Δ R ~ j , j − 1 ∂ Δ R ~ i , j − 1 ∂ b g − J r j − 1 Δ t \begin{aligned} \frac{\partial\Delta\tilde{\boldsymbol{R}}_{ij}}{\partial b^g}& =\sum_{k=i}^{j-1}\left(-\Delta\tilde{R}_{k+1,j}^{\top}J_{r}^{k}\Delta t\right) \\ &=\sum_{k=i}^{j-2}\left(-\Delta\tilde{\boldsymbol{R}}_{k+1,j}^{\top}\boldsymbol{J}_{r}^{k}\Delta t\right)-\Delta\tilde{\boldsymbol{R}}_{jj}^{\top}\boldsymbol{j}_{r}^{j-1}\Delta t \\ &\begin{aligned}=\sum_{k=i}^{j-2}\left(-\left(\Delta\tilde{\boldsymbol{R}}_{k+1,j-1}\Delta\tilde{\boldsymbol{R}}_{j-1,j}\right)^\top J_{r}^k\Delta t\right)-\Delta\tilde{\boldsymbol{R}}_{jj}^\top J_{r}^{j-1}\Delta t\end{aligned} \\ &=\Delta\tilde{\boldsymbol{R}}_{j,j-1}\sum_{k=i}^{j-2}\left(-\left(\Delta\tilde{\boldsymbol{R}}_{k+1,j-1}\right)^\top\boldsymbol{J}_r^k\Delta t\right)-\boldsymbol{J}_r^{j-1}\Delta t \\ &=\Delta\tilde{\boldsymbol{R}}_{j,j-1}\frac{\partial\Delta\tilde{\boldsymbol{R}}_{i,j-1}}{\partial\boldsymbol{b}^{g}}-\boldsymbol{J}_{r}^{j-1}\Delta t \end{aligned} ∂bg∂ΔR~ij=k=i∑j−1(−ΔR~k+1,j⊤JrkΔt)=k=i∑j−2(−ΔR~k+1,j⊤JrkΔt)−ΔR~jj⊤jrj−1Δt=k=i∑j−2(−(ΔR~k+1,j−1ΔR~j−1,j)⊤JrkΔt)−ΔR~jj⊤Jrj−1Δt=ΔR~j,j−1k=i∑j−2(−(ΔR~k+1,j−1)⊤JrkΔt)−Jrj−1Δt=ΔR~j,j−1∂bg∂ΔR~i,j−1−Jrj−1Δt
// Update rotation jacobian wrt bias correction
// 计算偏置的雅克比矩阵，r对bg的导数，∂ΔRij/∂bg = (ΔRjj-1) * ∂ΔRij-1/∂bg - Jr(j-1)*t
// 论文作者对forster论文公式的基础上做了变形，然后递归更新，参见 https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/212
// ? 为什么先更新JPa、JPg、JVa、JVg最后更新JRg? 答：这里必须先更新dRi才能更新到这个值，但是为什么JPg和JVg依赖的上一个JRg值进行更新的？
JRg = dRi.deltaR.transpose() * JRg - dRi.rightJ * dt;
// Total integrated time
// 更新总时间
dT += dt;
}
3 预积分零偏更新
利用上面求得预积分测量对零偏的雅可比进行预积分测量更新
Δ R ~ ‾ i j ≐ Δ R ~ i j Exp ( ∂ Δ R ~ i j ∂ b g δ b i g ) \begin{aligned} &\Delta\overline{\tilde{R}}_{ij} \doteq\Delta\tilde{\boldsymbol{R}}_{ij}\operatorname{Exp}\left(\frac{\partial\Delta\tilde{\boldsymbol{R}}_{ij}}{\partial\boldsymbol{b}^{g}}\delta\boldsymbol{b}_{i}^{g}\right) \\ \end{aligned} ΔR~ij≐ΔR~ijExp(∂bg∂ΔR~ijδbig)
/**
* @brief 根据新的偏置计算新的dR
* @param b_ 新的偏置
* @return dR
*/
Eigen::Matrix3f Preintegrated::GetDeltaRotation(const Bias &b_)
{
std::unique_lock<std::mutex> lock(mMutex);
// 计算偏置的变化量
Eigen::Vector3f dbg;
dbg << b_.bwx - b.bwx, b_.bwy - b.bwy, b_.bwz - b.bwz;
// 考虑偏置后，dR对偏置线性化的近似求解,邱笑晨《预积分总结与公式推导》P13～P14
// Forster论文公式（44）yP17也有结果（但没有推导），后面两个函数GetDeltaPosition和GetDeltaPosition也是基于此推导的
return NormalizeRotation(dR * Sophus::SO3f::exp(JRg * dbg).matrix());
}
Δ v ~ ‾ i j ≐ Δ v ~ i j + ∂ Δ v ~ i j ∂ b g δ b i g + ∂ Δ v ~ i j ∂ b a δ b i a \Delta\overline{\tilde{v}}_{ij} \doteq\Delta\tilde{\boldsymbol{v}}_{ij}+\frac{\partial\Delta\tilde{\boldsymbol{v}}_{ij}}{\partial\boldsymbol{b}^{g}}\delta\boldsymbol{b}_{i}^{g}+\frac{\partial\Delta\tilde{\boldsymbol{v}}_{ij}}{\partial\boldsymbol{b}^{a}}\delta\boldsymbol{b}_{i}^{a} \\ Δv~ij≐Δv~ij+∂bg∂Δv~ijδbig+∂ba∂Δv~ijδbia
/**
* @brief 根据新的偏置计算新的dV
* @param b_ 新的偏置
* @return dV
*/
Eigen::Vector3f Preintegrated::GetDeltaVelocity(const Bias &b_)
{
std::unique_lock<std::mutex> lock(mMutex);
Eigen::Vector3f dbg, dba;
dbg << b_.bwx - b.bwx, b_.bwy - b.bwy, b_.bwz - b.bwz;
dba << b_.bax - b.bax, b_.bay - b.bay, b_.baz - b.baz;
// 考虑偏置后，dV对偏置线性化的近似求解,邱笑晨《预积分总结与公式推导》P13，JPg和JPa在预积分处理中更新
return dV + JVg * dbg + JVa * dba;
}
Δ p ~ ‾ i j = ˙ Δ p ~ i j + ∂ Δ p ~ i j ∂ b g δ b i g + ∂ Δ p ~ i j ∂ b a δ b i a \Delta\overline{\tilde{p}}_{ij} \dot{=}\Delta\tilde{\boldsymbol{p}}_{ij}+\frac{\partial\Delta\tilde{\boldsymbol{p}}_{ij}}{\partial\boldsymbol{b}^g}\delta\boldsymbol{b}_{i}^g+\frac{\partial\Delta\tilde{\boldsymbol{p}}_{ij}}{\partial\boldsymbol{b}^a}\delta\boldsymbol{b}_{i}^a Δp~ij=˙Δp~ij+∂bg∂Δp~ijδbig+∂ba∂Δp~ijδbia
/**
* @brief 根据新的偏置计算新的dP
* @param b_ 新的偏置
* @return dP
*/
Eigen::Vector3f Preintegrated::GetDeltaPosition(const Bias &b_)
{
std::unique_lock<std::mutex> lock(mMutex);
Eigen::Vector3f dbg, dba;
dbg << b_.bwx - b.bwx, b_.bwy - b.bwy, b_.bwz - b.bwz;
dba << b_.bax - b.bax, b_.bay - b.bay, b_.baz - b.baz;
// 考虑偏置后，dP对偏置线性化的近似求解,邱笑晨《预积分总结与公式推导》P13，JPg和JPa在预积分处理中更新
return dP + JPg * dbg + JPa * dba;
}
4 预积分残差雅可比计算
这个在G2oTypes.h，有时间补充吧
