0. 简介
对于ORB-SLAM3而言。如何将代码融入Wheel和GPS是一个挺有意思的事情。通过GPS和Wheel可以非常有效的约束视觉里程计结果。Wheel这块主要就是将速度等信息融合到前端中，类似IMU和视觉帧间的关系。而GPS由于频率不是很高，所以基本是用于全局修正的作用。这部分我们经常使用松耦合的形式，当然也有工作去做了紧耦合相关的工作。
1. Wheel特征添加
这一部分主要的其实就是将wheel的速度特征传入到imu中，以约束两者之间的位移信息。从而提供针对IMU的约束，并可以作为边约束约束G2O的边
1.1 ImuType.h
这一步主要是得是将轮速信息融合到IMU内部去做联合优化，这里面主要用于处理传感器数据融合和运动状态估计。
Point(const cv::Point3f Acc, const cv::Point3f Gyro, const double ×tamp, const double& encoder_v):
a(Acc.x,Acc.y,Acc.z), w(Gyro.x,Gyro.y,Gyro.z), t(timestamp), encoder_v(encoder_v){}
.........
void IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt,const double &encoder_v );
.........
Eigen::DiagonalMatrix<float,6> Nga, NgaWalk;
Eigen::DiagonalMatrix<float,9> Nga_en, NgaWalk_en;
// Values for the original bias (when integration was computed)
Bias b;
Eigen::Matrix3f dR;
Eigen::Vector3f dV, dP;
Eigen::Matrix3f Rbo = Eigen::Matrix3f::Identity(); //encoder 和imu的旋转
Eigen::Vector3d Tbo = {-0.9810000061988831,0,0}; //encoder 和imu的旋转
Eigen::Matrix3f JRg, JVg, JVa, JPg, JPa;
Eigen::Vector3f avgA, avgW;
Eigen::Vector3f encoder_velocity ;
Eigen::Matrix<float,18,18> jacobian_enc;
Eigen::Matrix<float,21,21> covariance_enc;
1.2 ImuType.cc
下面IntegrateNewMeasurement 函数中，该系统整合了惯性测量（来自加速度计和陀螺仪）和可选的编码器读数。此函数的主要目的是根据这些测量数据更新内部状态，以跟踪位置、速度和旋转的变化。
/**
* @brief 初始化预积分
* @param b_ 偏置
*/
void Preintegrated::Initialize(const Bias &b_)
{
dR.setIdentity();
dV.setZero();
dP.setZero();
JRg.setZero();
JVg.setZero();
JVa.setZero();
JPg.setZero();
JPa.setZero();
C.setZero();
Info.setZero();
db.setZero();
encoder_velocity.setZero();
jacobian_enc.setZero();
covariance_enc.setZero();
b = b_;
bu = b_; // 更新后的偏置
avgA.setZero(); // 平均加速度
avgW.setZero(); // 平均角速度
dT = 0.0f;
mvMeasurements.clear(); // 存放imu数据及dt
}
/**
* @brief 根据新的偏置重新积分mvMeasurements里的数据 Optimizer::InertialOptimization调用
*/
void Preintegrated::Reintegrate()
{
std::unique_lock<std::mutex> lock(mMutex);
const std::vector<integrable> aux = mvMeasurements;
//重新进行预积分
Initialize(bu);
bool buseencoder;
buseencoder = true;
if(buseencoder){
for (size_t i = 0; i < aux.size(); i++)
IntegrateNewMeasurement(aux[i].a, aux[i].w, aux[i].t,aux[i].enc);
}
else{
for (size_t i = 0; i < aux.size(); i++)
IntegrateNewMeasurement(aux[i].a, aux[i].w, aux[i].t);}
}
void Preintegrated::IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt,const double &encoder_v)
{
// 保存imu数据，利用中值积分的结果构造一个预积分类保存在mvMeasurements中
mvMeasurements.push_back(integrable(acceleration, angVel, dt,encoder_v));
// 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.构造协方差矩阵
// 噪声矩阵的传递矩阵，这部分用于计算i到j-1历史噪声或者协方差
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;
// 记录平均加速度和角速度
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 eq.(38)
dP = dP + dV * dt + 0.5f * dR * acc * dt * dt;
dV = dV + dR * acc * dt;
Eigen::Vector3f encoder_cast = { static_cast<float>(encoder_v),0,0};
encoder_velocity = encoder_velocity + dR*Rbo* encoder_cast*dt;
//std::cerr<<"encoder_velocity "<<encoder_velocity;
// 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);
// 强行归一化使其符合旋转矩阵的格式
auto dR_past = dR;
dR = NormalizeRotation(dR * dRi.deltaR);
//std::cerr<<"dR_past"<<dR_past<<std::endl;
//std::cerr<<"dR"<<dR<<std::endl;
// 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，故省略了小量的更新
// 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个加速度计噪声的平方
// 这一部分最开始是0矩阵，随着积分次数增加，每次都加上随机游走，偏置的信息矩阵
C.block<6, 6>(9, 9) += NgaWalk;
// 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;
bool buseencoder;
buseencoder = true;
// TODO
if(buseencoder){
Eigen::MatrixXf F = Eigen::MatrixXf ::Zero(12,12);
// F.block<3,3>(0,0) = Eigen::Matrix3f::Identity();
// F.block<3,3>(0,3) = A.block<3, 3>(6, 0);
// F.block<3,3>(0,6) = Eigen::DiagonalMatrix<float, 3>(dt, dt, dt);
//
// F.block<3,3>(3,3) = dRi.deltaR.transpose();
//
// F.block<3,3>(6,3) = A.block<3, 3>(3, 0);
// F.block<3,3>(6,6) = Eigen::Matrix3f::Identity();
//轮速
Eigen::Vector3f encoder_fi = Rbo*encoder_velocity;
Eigen::Matrix3f R_encoder;
R_encoder<< 0, -encoder_fi(2), encoder_fi(1),encoder_fi(2), 0, -encoder_fi(0),-encoder_fi(1), encoder_fi(0), 0;
// F.block<3,3>(9,3) =-dR_past*dt*R_encoder;
// F.block<3,3>(9,9) = Eigen::Matrix3f::Identity();
Eigen::MatrixXf V = Eigen::MatrixXf ::Zero(12,9);
// V.block<3,3>(0,0) = 0.5f * dR_past * dt * dt;
// V.block<3,3>(3,3) = dRi.rightJ * dt;
// V.block<3,3>(6,0) = dR_past * dt;
// V.block<3,3>(9,6) = dR_past*Rbo*dt;
F.block<9,9>(0,0) = A;
F.block<3,3>(9,3) = -dR_past*dt*R_encoder;
F.block<3,3>(9,9) = Eigen::Matrix3f::Identity();
V.block<9,6>(0,0) = B;
V.block<3,3>(9,6) = dR_past*Rbo*dt;
covariance_enc.block<12,12>(0,0) = F*covariance_enc.block<12,12>(0,0)*F.transpose()+V*Nga_en*V.transpose();
covariance_enc.block<9,9>(12,12) += NgaWalk_en;
}
}