上一个函数单目+imu模式初始化函数initialStructure()的末尾有一个函数visualInitialAlign()中有一个函数VisualIMUAlignment()是将imu与视觉融合的函数，我们来看一下和内部函数
先放一下我注释的源码，再来讲一下笔者的理解，让大家学的更方便的同时也防止自己忘
首先是VisualIMUAlignment()函数，比较简单，调用两个函数
bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x)
{
// 调用 solveGyroscopeBias 函数，求解陀螺仪的偏置并更新 Bgs。
//传入所有图像帧和陀螺仪偏置数组 Bgs。该函数用于求解陀螺仪的偏置，并更新 Bgs 数组。
solveGyroscopeBias(all_image_frame, Bgs);
// 调用 LinearAlignment 函数，进行线性对齐，求解重力向量 g 和状态变量 x。
if(LinearAlignment(all_image_frame, g, x))
return true;// 如果线性对齐成功，返回 true。
else
return false;// 如果线性对齐失败，返回 false。
}
按顺序是 solveGyroscopeBias()函数
void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)
{
//定义一个 3x3 的矩阵 A，用于存储线性系统的系数矩阵。
Matrix3d A;
//定义一个 3x1 的向量 b，用于存储线性系统的常数项。
Vector3d b;
//定义一个 3x1 的向量 delta_bg，用于存储计算得到的陀螺仪偏置的增量。
Vector3d delta_bg;
//将矩阵 A 和向量 b 初始化为零
A.setZero();
b.setZero();
//使用两个迭代器 frame_i 和 frame_j 遍历所有的图像帧 all_image_frame
//frame_j 总是指向 frame_i 的下一个图像帧。
map<double, ImageFrame>::iterator frame_i;
map<double, ImageFrame>::iterator frame_j;
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)
{
//对每对相邻的图像帧 frame_i 和 frame_j：
//定义临时矩阵 tmp_A 和临时向量 tmp_b 并初始化为零。
frame_j = next(frame_i);
MatrixXd tmp_A(3, 3);
tmp_A.setZero();
VectorXd tmp_b(3);
tmp_b.setZero();
//核心思想是通过两帧之间的旋转误差来校正陀螺仪偏置
//计算两个相邻图像帧之间的旋转四元数 q_ij
//pre_integration是预积分结果，里面有两帧之间位置、速度和姿态的变化量
Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
//获取预积分结果中的雅可比矩阵块，并存储在 tmp_A 中。
//雅可比矩阵块是预积分量对状态变量（如陀螺仪偏置）的偏导数，它们在优化过程中起到了关键作用。
//雅可比矩阵块描述了预积分量如何随着状态变量的变化而变化，从而提供了优化过程中更新这些状态变量所需的信息。
//雅可比矩阵 J_bg 描述了预积分量对陀螺仪偏置 bg 的敏感度
tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
//计算并存储临时向量 tmp_b。
//在两个相邻帧之间，通过预积分量（例如 delta_q ）和真实的旋转变化（例如 q_ij ），可以构建一个误差项：
//将delta_q 逆与q_ij真实旋转相乘，如果他俩完全一致，结果应为单元四元数，即无旋转，通过这个差异，我们可以得到误差。
tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
//更新线性系统的系数矩阵 A 和常数项 b。
//tmp_A 是预积分量对陀螺仪偏置的偏导数， tmp_b 是旋转误差。
A += tmp_A.transpose() * tmp_A;
b += tmp_A.transpose() * tmp_b;
}
//通过设定误差e使delta_q和q_ij相差最小来构建线性方程
//通过求解线性系统 A * delta_bg = b，计算陀螺仪偏置的增量 delta_bg
delta_bg = A.ldlt().solve(b);
ROS_WARN_STREAM("gyroscope bias initial calibration " << delta_bg.transpose());
//将计算得到的陀螺仪偏置增量 delta_bg 累加到原始陀螺仪偏置 Bgs 中
for (int i = 0; i <= WINDOW_SIZE; i++)
Bgs[i] += delta_bg;
//对所有图像帧重新进行预积分，更新预积分结果。
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++)
{
frame_j = next(frame_i);
frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);
}
}
然后是 LinearAlignment()
//线性对齐（Linear Alignment），用于优化视觉与IMU信息之间的一致性
bool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{
// 获取所有图像帧的数量
int all_frame_count = all_image_frame.size();
// 状态向量的维度，每个帧包括位置（3个自由度）、速度（3个自由度）、重力加速度（3个自由度）、尺度因子（1个自由度）
int n_state = all_frame_count * 3 + 3 + 1;
// 初始化系数矩阵 A 和常数项 b
MatrixXd A{n_state, n_state};
A.setZero();
VectorXd b{n_state};
b.setZero();
// 遍历所有图像帧
map<double, ImageFrame>::iterator frame_i;
map<double, ImageFrame>::iterator frame_j;
int i = 0;
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++)
{
frame_j = next(frame_i);
// 临时的雅可比矩阵和常数向量
MatrixXd tmp_A(6, 10);
tmp_A.setZero();
VectorXd tmp_b(6);
tmp_b.setZero();
double dt = frame_j->second.pre_integration->sum_dt;
// 构建误差项
// 第一个误差项对应位置和尺度因子
//这一部分表示在位置误差项中，当前帧的速度对位置的贡献。根据运动学方程，位置变化与速度成正比，比例系数为时间间隔 dt
tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
//这一部分表示位置误差项中，加速度对位置的贡献。根据运动学方程，位置变化与加速度的关系是 1/2 * 加速度 * dt^2，旋转矩阵 frame_i->second.R.transpose() 用于从世界坐标系转换到当前帧的局部坐标系。
tmp_A.block<3, 3>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity();
//这一部分表示位置误差项中，尺度因子对位置的影响。这里假设尺度因子是一个标量，影响的是两帧之间的位移量，
//表示尺度因子对两帧之间位移量的影响
tmp_A.block<3, 1>(0, 9) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;
//这一部分是位置误差项的常数项，包括了IMU预积分的位移增量 delta_p 以及两个帧之间的相对位置 TIC 的校正量。
tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0];
//cout << "delta_p " << frame_j->second.pre_integration->delta_p.transpose() << endl;
// 第二个误差项对应速度
//这一部分表示在速度误差项中，当前帧的速度对速度误差的贡献。速度误差项中的速度贡献为 -1，表示直接使用速度值。
tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();
//这一部分表示速度误差项中，相邻两帧之间的旋转对速度误差的贡献。这里使用了两个帧的旋转矩阵之间的转换关系。
tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;
//这一部分表示速度误差项中，加速度对速度误差的贡献。根据运动学方程，速度变化与加速度成正比，比例系数为时间间隔 dt，同样考虑了从世界坐标系到局部坐标系的转换。
tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity();
//这一部分是速度误差项的常数项，包括了IMU预积分的速度增量 delta_v。
tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v;
//cout << "delta_v " << frame_j->second.pre_integration->delta_v.transpose() << endl;
// 协方差的逆矩阵，假设单位矩阵
Matrix<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero();
//cov.block<6, 6>(0, 0) = IMU_cov[i + 1];
//MatrixXd cov_inv = cov.inverse();
//cov_inv 是协方差矩阵的逆矩阵。理想情况下，它应该反映IMU噪声的协方差，但这里被设置为单位矩阵，简化了计算。
cov_inv.setIdentity();
// 更新系数矩阵 A 和常数项 b
//tmp_A 是误差项对状态变量的雅可比矩阵，表示误差项相对于状态变量的偏导数
//tmp_b 是误差项的残差。表示预测值与实际值之间的差异。
//加权最小二乘法的目标是最小化加权残差的平方和
//r_A 和 r_b 分别表示加权后的误差项贡献。
MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;
//逐步构建全局线性系统矩阵 A 和向量 b。
//将每个局部线性系统的贡献（即 r_A 和 r_b）添加到全局线性系统中。
//使用 block 和 segment 操作将局部矩阵和向量插入到全局矩阵和向量的相应位置。
A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();//左上角6*6，表示当前帧的位置和速度误差项的贡献。
b.segment<6>(i * 3) += r_b.head<6>();//表示当前帧的位置和速度误差项的贡献
A.bottomRightCorner<4, 4>() += r_A.bottomRightCorner<4, 4>();//右下角4*4，表示重力和尺度因子的部分
b.tail<4>() += r_b.tail<4>();//重力和尺度因子的残差贡献
A.block<6, 4>(i * 3, n_state - 4) += r_A.topRightCorner<6, 4>();//右上角6*4，当前帧位置和速度误差项与全局重力和尺度因子的相互作用部分
A.block<4, 6>(n_state - 4, i * 3) += r_A.bottomLeftCorner<4, 6>();//左下角4*6，全局重力和尺度因子与当前帧位置和速度误差项的相互作用部分
}
// 缩放系数，用于数值稳定性
//对系数矩阵 A 和常数项 b 进行缩放，以提高数值稳定性。这在解决线性系统时是常见的做法，避免小数导致的数值不稳定性。
A = A * 1000.0;
b = b * 1000.0;
// 使用 Cholesky 分解（ldlt()）解线性系统Ax = b，得到优化后的状态向量 x
//ldlt() 是一种高效的分解方法，适用于对称正定矩阵
x = A.ldlt().solve(b);
// 缩放因子 s
//通过取状态向量 x 的最后一个元素并除以100得到
double s = x(n_state - 1) / 100.0;
ROS_DEBUG("estimated scale: %f", s);
// 重力向量 g 的估计值
//通过从状态向量 x 的倒数第四个元素开始提取一个三维向量得到
g = x.segment<3>(n_state - 4);
ROS_DEBUG_STREAM(" result g " << g.norm() << " " << g.transpose());
// 检查估计的重力向量是否合理，以及尺度因子是否有效
//检查估计的重力向量的模长是否与已知的重力加速度 G（通常是9.8 m/s²）相差在0.5以内
//检查尺度因子 s 是否为正值
if(fabs(g.norm() - G.norm()) > 0.5 || s < 0)
{
return false;
}
// 进一步优化重力向量估计
//调用 RefineGravity 函数对重力向量进行进一步优化，以获得更准确的估计
RefineGravity(all_image_frame, g, x);
// 提取优化后的尺度因子 s，方法同前
s = (x.tail<1>())(0) / 100.0;
(x.tail<1>())(0) = s;
ROS_DEBUG_STREAM(" refine " << g.norm() << " " << g.transpose());
//再次检查尺度因子 s 是否为正值。
if(s < 0.0 )
return false; //如果 s 为负，返回 false，表示优化失败
else
return true;//否则返回 true，表示优化成功
}
这其中包含 RefineGravity()函数
//通过优化重力向量 g 来改进其估计值
void RefineGravity(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{
//重力向量初始化: g0 是重力向量的单位化版本，乘以重力的模长 (G.norm())，以确保 g0 的长度是重力加速度。
Vector3d g0 = g.normalized() * G.norm();
Vector3d lx, ly;
//VectorXd x;
int all_frame_count = all_image_frame.size();
//n_state 计算了状态变量的总数，包含每个帧的位置、速度以及2个重力分量和1个尺度因子。
int n_state = all_frame_count * 3 + 2 + 1;
MatrixXd A{n_state, n_state};
A.setZero();
VectorXd b{n_state};
b.setZero();
map<double, ImageFrame>::iterator frame_i;
map<double, ImageFrame>::iterator frame_j;
for(int k = 0; k < 4; k++)
{
MatrixXd lxly(3, 2);
//构建切向基: TangentBasis(g0) 计算出 g0 的两个正交向量 lx 和 ly，用来表示切向基。
lxly = TangentBasis(g0);
int i = 0;
//遍历图像帧
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++)
{
frame_j = next(frame_i);
//tmp_A 是误差项的雅可比矩阵，tmp_b 是误差项的常数项。
MatrixXd tmp_A(6, 9);
tmp_A.setZero();
VectorXd tmp_b(6);
tmp_b.setZero();
//dt 是两帧之间的时间间隔。
double dt = frame_j->second.pre_integration->sum_dt;
//表示位置误差项中，加速度对位置的贡献。
tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
//表示位置误差项中，重力向量的贡献。
tmp_A.block<3, 2>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity() * lxly;
//表示位置误差项中，尺度因子的贡献。
tmp_A.block<3, 1>(0, 8) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;
//位置误差项的常数项，包含 IMU 预积分的位移增量、相对位移校正量和重力校正量。
tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0] - frame_i->second.R.transpose() * dt * dt / 2 * g0;
//表示速度误差项中，位置对速度的贡献
tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();
//表示速度误差项中，旋转对速度的贡献。
tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;
//表示速度误差项中，重力向量的贡献。
tmp_A.block<3, 2>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity() * lxly;
//是速度误差项的常数项，包含 IMU 预积分的速度增量和重力校正量。
tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v - frame_i->second.R.transpose() * dt * Matrix3d::Identity() * g0;
//是速度误差项的常数项，包含 IMU 预积分的速度增量和重力校正量。
Matrix<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero();
//cov.block<6, 6>(0, 0) = IMU_cov[i + 1];
//MatrixXd cov_inv = cov.inverse();
cov_inv.setIdentity();
//加权后的误差项
MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;
//r_A 的左上角 6x6 子矩阵，表示当前帧的位置和速度误差项的贡献。
A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();
//r_b 的前 6 个元素，表示当前帧的位置和速度误差项的常数项。
b.segment<6>(i * 3) += r_b.head<6>();
//r_A 的右下角 3x3 子矩阵，表示重力向量的贡献
A.bottomRightCorner<3, 3>() += r_A.bottomRightCorner<3, 3>();
//r_b 的最后 3 个元素，表示重力向量的常数项。
b.tail<3>() += r_b.tail<3>();
//加上 r_A 的右上角和左下角部分，表示帧间相互作用
A.block<6, 3>(i * 3, n_state - 3) += r_A.topRightCorner<6, 3>();
A.block<3, 6>(n_state - 3, i * 3) += r_A.bottomLeftCorner<3, 6>();
}
A = A * 1000.0;
b = b * 1000.0;
x = A.ldlt().solve(b);//使用 LDLT 分解求解线性系统，得到状态向量 x
//提取出优化后状态向量 x 中的最后两个元素，即包含优化后的重力向量的偏移量。
VectorXd dg = x.segment<2>(n_state - 3);
//lxly 是切向基，dg 是优化后的重力向量的偏移量。
//g0 + lxly * dg 表示在当前重力向量 g0 的基础上，根据优化结果调整重力向量的方向
//.normalized() 确保新的重力向量长度为1，即单位向量。
//* G.norm() 将单位向量缩放到预设的重力加速度 G 的模长。
g0 = (g0 + lxly * dg).normalized() * G.norm();
//double s = x(n_state - 1);
}
//将更新后的重力向量 g0 赋给 g，以便在函数外部使用
g = g0;
}
看完源码，讲一下笔者个人理解， VisualIMUAlignment首先通过这个函数调用solveGyroscopeBias和LinearAlignment两个函数，solveGyroscopeBias这个函数是对帧存储器中所有图像帧拿出来，计算每相邻两帧的旋转变化，这个是通过视觉来计算得到，然后拿到每两帧之间的预积分量关于旋转的部分，有关于旋转的雅可比矩阵和预积分量构建一个超大的线性方程组，也就是线性系统，，通过求解得到陀螺仪的偏置增量，并重新传播预积分，对陀螺仪偏置进行更新和对所有帧的预积分值进行更新，而LinearAlignment函数也是通过遍历所有帧，跟solveGyroscopeBias前边的做法是一样的，只不过这个函数构建的误差项有所不同，是构建了位置误差项，在其中有速度对位置的贡献，加速度对位置的贡献，尺度因子对位置的贡献，还有预积分量中的位移增量相对两帧利用视觉信息得到的TIC的校正量，然后对速度构建误差项，有当前帧速度，相邻两帧的相对旋转，加速度，还有预积分量中的速度增量，也一样构建线性系统，通过求解这个线性系统，解出新的尺度因子s，和更新后的重力向量g，将这一步计算出的s和g送入RefineGravity函数，一样通过构建线性系统并求解，求出更新后的x状态向量和更新后的重力向量g，然后通过检查尺度因子s来评判imu信息和视觉信息融合的成功与否。
在这里笔者理解的视觉与imu信息相融合就是通过构建和求解线性系统，将IMU数据和视觉数据结合起来，利用视觉信息的准确性，优化IMU的陀螺仪偏置和重力向量g，同时调整视觉信息中的尺度因子s。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
