ORB-SLAM3算法学习—Tracking线程—IMU预积分
文章目录
- 0 总述
- 1 VI-SLAM系统中关于IMU需要的参数
- 2 IMU数据相关的类
- 3 IMU数据流的传递过程
-
- 3.1 IMU数据流获取
- 3.2预积分处理
0 总述
预积分的计算原理主要是根据物理运动公式计算，即加速度，速度和位移之间的计算关系。在ORB-SLAM3中，IMU预积分最早是从第2帧开始的，因为是使用当前帧和上一帧之间的IMU数据，所以只能从第二帧开始。
1 VI-SLAM系统中关于IMU需要的参数
对于前三个参数需要进行标定，标定工具大概有kalibr_allan和VINS的imu_utils
- 与相机之间的旋转和平移矩阵，主要是和左相机之间的关系
- IMU线速度和角速度的白噪声
- IMU线速度和角速度的随机游走噪声
# Transformation from camera 0 to body-frame (imu)
Tbc: !!opencv-matrix
rows: 4
cols: 4
dt: f
data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,
-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,
0.0, 0.0, 0.0, 1.0]
# IMU noise
IMU.NoiseGyro: 1.7e-04 # 1.6968e-04
IMU.NoiseAcc: 2.0e-03 # 2.0000e-3
IMU.GyroWalk: 1.9393e-05
IMU.AccWalk: 3.e-03 # 3.0000e-3
IMU.Frequency: 200
2 IMU数据相关的类
在ImuTypes.h中定义了IMU一些相关的类，包括
- IMU测量值类
Point，每个IMU Point包含3轴线加速度，3轴角速度，时间戳，共7维信息 - IMU偏置
Bias，类成员包含3轴加速度和3轴角速度等6个维度的bias，是一个随时间变化的量，先简单的理解为测量值 = 真实值 + Bias + 白噪声 - IMU标定数据
Calib，类成员包括与相机的外惨Tbc，线加速度和角速度的高斯白噪声，线加速度和角速度的随机噪声(随机游走) - IMU预积分
Preintegrated，重头戏，进行IMU预积分的相关操作
3 IMU数据流的传递过程
3.1 IMU数据流获取
IMU数据在外层ROS节点程序中得到,在ORB-SLAM3中将上一帧时间戳 到 当前帧时间戳 之间的IMU数据定义为一帧数据，ROS节点程序订阅IMU数据，并将IMU的3轴加速度和3轴角速度以及时间戳封装成类对象的形式，然后和当前帧图像一起被送进SLAM系统
vector<ORB_SLAM3::IMU::Point> vImuMeas;
mpImuGb->mBufMutex.lock();
if(!mpImuGb->imuBuf.empty())
{
// Load imu measurements from buffer
vImuMeas.clear();
while(!mpImuGb->imuBuf.empty() && mpImuGb->imuBuf.front()->header.stamp.toSec()<=tImLeft)
{
double t = mpImuGb->imuBuf.front()->header.stamp.toSec();
cv::Point3f acc(mpImuGb->imuBuf.front()->linear_acceleration.x, mpImuGb->imuBuf.front()->linear_acceleration.y, mpImuGb->imuBuf.front()->linear_acceleration.z);
cv::Point3f gyr(mpImuGb->imuBuf.front()->angular_velocity.x, mpImuGb->imuBuf.front()->angular_velocity.y, mpImuGb->imuBuf.front()->angular_velocity.z);
//cv::Point3f acc(mpImuGb->imuBuf.front()->linear_acceleration.y, -mpImuGb->imuBuf.front()->linear_acceleration.z, mpImuGb->imuBuf.front()->linear_acceleration.x);
//cv::Point3f gyr(mpImuGb->imuBuf.front()->angular_velocity.y, -mpImuGb->imuBuf.front()->angular_velocity.z, mpImuGb->imuBuf.front()->angular_velocity.x);
vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc,gyr,t));
mpImuGb->imuBuf.pop();
}
}
mpImuGb->mBufMutex.unlock();
3.2预积分处理
在track()函数中，是否进行IMU预积分有一个判断,首先是当前的SLAM模式需要是IMU模式，另外mbCreatedMap这个变量需要是false
if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && !mbCreatedMap)
{
PreintegrateIMU();
}
通过在代码中遍历，来看一下mbCreatedMap这个状态的变化过程
mbCreatedMap是tracking的成员变量，在tracking.h中被定义，在tracking构造函数中被初始化为false，只有在Tracking.cc->CreateMapInAtlas()函数被调用的时候，mbCreatedMap才会被调用，主要有以下几种情况：
- 当前SLAM系统的状态
mState!=NO_IMAGES_YET，也就是只要不是第一帧的状态时，当前帧的时间戳出现异常会创建新的子地图，mbCreatedMap会变成true，然后下一帧不进行预积分 - 如果上一帧跟踪失败被判定
mState == LOST，但是当前活跃地图中已经存在了较多的关键帧具有保留价值，需要创建新的活跃地图 - 上一帧跟踪失败了，
mState == LOST或者mState = RECENTLY_LOST，SLAM系统尝试进行重定位或者使用了其他的补救措施，但是没有救回来，导致在跟踪局部地图之后状态机依然是mState == LOST，此时如果地图中关键帧超过10 并且 纯视觉模式 或 虽然是IMU模式但是已经完成IMU初始化了，保存当前地图，创建新的地图
综上，只要不出现上述新建活跃子地图的情况，预积分会一直进行。
下面看一下预积分的具体内容PreintegrateIMU()
1. 如果是第一帧图像帧的情况，IMU的数据为空，直接返回
// Step 1.拿到两两帧之间待处理的预积分数据，组成一个集合
// 上一帧不存在,说明两帧之间没有imu数据，不进行预积分
// mbImuPreintegrated标志位设置为真
if(!mCurrentFrame.mpPrevFrame)
{
Verbose::PrintMess("non prev frame ", Verbose::VERBOSITY_NORMAL);
mCurrentFrame.setIntegrated();// 设置mbImuPreintegrated标志位设置为真
return;
}
2. 初始化函数预积分器的构造
和当前帧对应的IMU数据是上一帧时间戳和当前帧时间戳之间的IMU数据，因此对于第一帧图像不进行相应的预积分，从预积分函数出来后会进入初始化函数中，并使用默认的偏置和IMU参数为当前帧构造预积分器。
IMU::Bias()这种通过默认构造函数得到的角速度和加速度偏置信息全为0,mpImuCalib为IMU的外惨和噪声相关的参数，固定值
mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib);
mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF;
3. 数据检查
到了第二帧的时候就开始有IMU数据了，开始进行预积分。首先经过一次筛选，将ros例子程序中传入的IMU数据，放入tracking的成员变量mvImuFromLastFrame。
// 该处的作用是将mlQueueImuData放入mvImuFromLastFrame中
// mvImuFromLastFrame里的last是相对于下一帧图像说的，ORB-SLAM中对于变量的定义就是用过之后就变成LAST
while(true){
// 数据还没有时,会等待一段时间,直到mlQueueImuData中有imu数据.一开始不需要等待
bool bSleep = false;
{
unique_lock<mutex> lock(mMutexImuQueue);
if(!mlQueueImuData.empty())
{
// 拿到第一个imu数据作为起始数据
IMU::Point* m = &mlQueueImuData.front();
cout.precision(17);//设置一下数据的精度
// if 该条IMU数据时间戳 < 上一帧时间戳 - 1/IMU频率
// imu起始数据会比当前帧的前一帧时间戳早,如果相差0.001则舍弃这个imu数据(每次只需要当前帧和上一帧之间的IMU数据)
if(m->t < mCurrentFrame.mpPrevFrame->mTimeStamp - mImuPer)
{
mlQueueImuData.pop_front();
}else if(m->t < mCurrentFrame.mTimeStamp - mImuPer)// 将时间戳小于当前帧的IMU数据存起来，存一条弹出一条
{
mvImuFromLastFrame.push_back(*m);
mlQueueImuData.pop_front();
}else{
// 如果IMU数据时间戳大于等于当前帧时间戳了，把最后一条数据获取后剩下的IMU数据就不再需要了
mvImuFromLastFrame.push_back(*m);
break;}
}else{
break;
bSleep = true;}}
if(bSleep)
usleep(500);
}
4. 为当前帧构造预积分器
由上一帧预积分后的偏置，外惨数据，高斯白噪声和随机游走为当前帧构造预积分器(问题，预积分的偏置信息是怎么进行更新的？？？)
基于标定数据初始化高斯白噪和随机游走的协方差矩阵
基于偏置初始化预积分器，将旋转，速度和位移的初值初始化为单位阵或者0,设置预积分过程中的雅格比矩阵，信息矩阵等(Initialize(b_))
IMU::Preintegrated* pImuPreintegratedFromLastFrame = new IMU::Preintegrated(mLastFrame.mImuBias,mCurrentFrame.mImuCalib);
Preintegrated::Preintegrated(const Bias &b_, const Calib &calib)
{
Nga = calib.Cov;//噪声协方差矩阵
NgaWalk = calib.CovWalk;//随机游走协方差矩阵
Initialize(b_);//根据bias初始化预积分相关的变量，bias改变时预积分会改变
}
5. 为预积分计算两次相邻时刻IMU数据的平均加速度、平均角速度和相隔时间
对于和当前帧对应的一帧IMU数据，第一条IMU数据和最后一条IMU数据并不会严格地和上一阵以及当前帧时间戳对齐，在ORB-SLAM3中对这种情况分别进行了不同的处理
// 针对预积分位置的不同做不同中值积分的处理
IMU0----------F_last---------IMU1---IMU2---IMU3------------IMU_n-1---F2_cur---IMU_n
t_i0----------t_f_l ---------t_i1---t_i2---t_i3------------t_i_n-1---t_f_c ---t_i_n
a_i0-------------------------a_i1---a_i2---a_i3------------a_i_n-1------------a_i_n
w_i0-------------------------w_i1---w_i2---w_i3------------w_i_n-1------------w_i_n
|-----tini----| |
|-------------tab------------|
首先是最简单的部分，即上面示意图中IMU1--IMU_n-1的部分，这部分的IMU数据完全在两个相邻帧的时间戳之间，直接取两次测量的平均值即可
else if(i<(n-1))
{
// 当IMU数据的时间戳完全在两个图像帧时间戳之内时，不需要再进行补偿
// 此时直接求两个时刻的平均加速度，角速度
acc = (mvImuFromLastFrame[i].a+mvImuFromLastFrame[i+1].a)*0.5f;
angVel = (mvImuFromLastFrame[i].w+mvImuFromLastFrame[i+1].w)*0.5f;
tstep = mvImuFromLastFrame[i+1].t - mvImuFromLastFrame[i].t;
}
然后处理靠近上一帧的数据，即上图示意图中的IMU0
设当前时刻imu的加速度ai0，下一时刻加速度ai1，时间间隔tab
正常情况下时为了求上一帧到当前时刻imu的一个平均加速度，但是imu时间不会正好落在上一帧的时刻，需要做补偿，要求得ai0时刻到上一帧F_last这段时间加速度的改变量
有了这个改变量将其加到ai0上之后就可以表示上一帧时的加速度了。其中ai0 - (ai1-ai0)*(tini/tab)为上一帧时刻的加速度,再加上ai1之后除以2,就为这段时间的加速度平均值
其中tstep表示a1到上一帧的时间间隔，a0 - (a1-a0)*(tini/tab)这个式子中tini可以是正也可以是负表示时间上的先后，(a1-a0)也是一样，多种情况下这个式子依然成立
if((i==0) && (i<(n-1)))
{
float tab = mvImuFromLastFrame[i+1].t - mvImuFromLastFrame[i].t;// 两IMU数据帧的时间差delta_t，t_i1-t_i0
float tini = mvImuFromLastFrame[i].t - mCurrentFrame.mpPrevFrame->mTimeStamp;// 第i帧imu距离上一图像帧的时间差，t_i0-t_f_l
acc = (mvImuFromLastFrame[i].a - (mvImuFromLastFrame[i+1].a-mvImuFromLastFrame[i].a) * (tini/tab) + mvImuFromLastFrame[i+1].a) * 0.5f;
angVel = (mvImuFromLastFrame[i].w + mvImuFromLastFrame[i+1].w - (mvImuFromLastFrame[i+1].w - mvImuFromLastFrame[i].w)*(tini/tab)) * 0.5f;
tstep = mvImuFromLastFrame[i+1].t - mCurrentFrame.mpPrevFrame->mTimeStamp;//=tab - tini
}
最后处理靠近当前帧的最后一个IMU数据，即上面示意图中的IMU_n
else if((i>0) && (i==(n-1)))//同样的如果到了最后一个IMU数据，也可能会发生该数据不能完全和当前帧时间戳对齐的情况
{
float tab = mvImuFromLastFrame[i+1].t-mvImuFromLastFrame[i].t;// t_i_n - t_i_n-1
float tend = mvImuFromLastFrame[i+1].t-mCurrentFrame.mTimeStamp;
acc = (mvImuFromLastFrame[i].a+mvImuFromLastFrame[i+1].a-
(mvImuFromLastFrame[i+1].a-mvImuFromLastFrame[i].a)*(tend/tab))*0.5f;
angVel = (mvImuFromLastFrame[i].w+mvImuFromLastFrame[i+1].w-
(mvImuFromLastFrame[i+1].w-mvImuFromLastFrame[i].w)*(tend/tab))*0.5f;
tstep = mCurrentFrame.mTimeStamp - mvImuFromLastFrame[i].t;
}
6.根据每一个平均加速度、平均角速度和时间戳进行预积分，同时更新噪声
主要函数：ImuTypes.cc->IntegrateNewMeasurement(...)
对于每一帧的IMU数据：
- 首先更新位置，因为其依赖于上一时刻的速度和旋转
dP = dP + dV*dt + 0.5f*dR*acc*dt*dt;
- 其次更新速度，因为其依赖于上一时刻的旋转
dV = dV + dR*acc*dt;
- 最后更新旋转
dR = NormalizeRotation(dR*dRi.deltaR);
同时更新预积分需要的协方差矩阵，以及求导时需要的雅格比矩阵。协方差矩阵用于优化时作为信息矩阵，雅格比矩阵用于更新位姿信息。在tracking线程，当IMU成功初始化时会直接通过IMU的数据更新当前帧的信息，具体函数为Tracking::PredictStateIMU()，其中GetImuPosition()，GetImuRotation()，GetVelocity()即为获取位姿和速度的相关信息。
void Preintegrated::IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt)
{
// 将当前帧IMU数据封装一下
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
// Step 1.构造协方差矩阵
/**
*
* 0(3x3) 和 I(3x3)分别表示3x3的0矩阵和单位阵
* delta_R(i,j-1)表示i时刻到j-1时刻的旋转变化量的测量值，带有噪声
* (a_j-1 - b_i)表示不带噪声的加速度信息，-delta_R(i,j-1)*(a_j-1 - b_i)^delta_t表示v = delta_R*a*t，对于位移同理
*
* |delta_R(i,j-1) 0(3x3) 0(3x3)| 旋转噪声协方差(3x3)
*A_j-1 = |-delta_R(i,j-1)*(a_j-1 - b_i)^delta_t I(3x3) 0(3x3)| 速度噪声协方差(3x3)
* |-0.5*delta_R(i,j-1)*(a_j-1 - b_i)^(delta_t*delta_t) I(3x3) I(3x3)|9x9 位移噪声协方差(3x3)
*/
/**
* |J_j-1*delta_t 0(3x3) |
* B_j-1 = |0(3x3) delta_R(i,j-1)*delta_t |
* |0(3x3) 0.5*delta_R(i,j-1)*(delta_t*delta_t)|9x6
*/
// 噪声矩阵的传递矩阵，这部分用于计算i到j-1历史噪声或者协方差，初始化为单位阵
// 9×9是因为旋转、速度和位移都在xyz上具有一个噪声分量，一共9维，对应的协方差矩阵就是9*9
Eigen::Matrix<float,9,9> A;
A.setIdentity();
// 噪声矩阵的传递矩阵，这部分用于计算j-1新的噪声或协方差，这两个矩阵里面的数都是当前时刻的，计算主要是为了下一时刻使用
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;
// 记录平均加速度和角速度
// dT是本次积分的累计时间，dt是两个IMU数据之间的时间间隔
// 平均加速度 = (IMU时间差*上一时刻IMU的平均加速度 + 两个IMU数据的旋转*当前时刻的平均加速度*两相邻时刻的IMU时间差)，avgA = (v0 + dv)/(dT+dt)
// 这里应该就是不断的累计求平均加速度和角速度，因为速度是一个矢量所以旋转这里需要乘以一个dR
avgA = (dT*avgA + dR*acc*dt)/(dT+dt);//avgA = (v0 + dv)/(dT+dt)
avgW = (dT*avgW + accW*dt)/(dT+dt);//avgW = (θ0 + dθ)/(dT+dt)
// Update delta position dP and velocity dV (rely on no-updated delta rotation)
// 基于还未更新的上一时刻的旋转更新相邻时刻IMU的累计位移和速度
// 位移P = P0 + v0*t + 1/2*a*t*t 速度V = v0 + a*t
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)
// 根据η_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;
// 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;
// Update delta rotation
// Step 2. 构造函数，会根据更新后的bias进行角度积分
IntegratedRotation dRi(angVel,b,dt);
// 强行归一化使其符合旋转矩阵的格式
dR = NormalizeRotation(dR*dRi.deltaR);
// 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;
// Update covariance
// 小量delta初始为0，更新后通常也为0，故省略了小量的更新
// Update covariance
// 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个加速度计噪声的平方
C.block<6,6>(9,9) += NgaWalk;// 这一部分最开始是0矩阵，随着积分次数增加，每次都加上随机游走，偏置的信息矩阵
// Update rotation jacobian wrt bias correction
// 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;
}