初始化并优化位姿后，接下来做的事是将这些位姿更新给上一帧，我们来看下updateLatestStates()源码：
void Estimator::updateLatestStates()
{
// 锁定mPropagate，确保对最新状态的更新是线程安全的
mPropagate.lock();
// 更新最新的时间戳，等于当前帧的时间戳加上时间延迟td
latest_time = Headers[frame_count] + td;
// 更新最新的位置信息
latest_P = Ps[frame_count];
// 更新最新的姿态信息（四元数）
latest_Q = Rs[frame_count];
// 更新最新的速度信息
latest_V = Vs[frame_count];
// 更新最新的加速度偏置
latest_Ba = Bas[frame_count];
// 更新最新的陀螺仪偏置
latest_Bg = Bgs[frame_count];
// 更新最新的加速度传感器数据
latest_acc_0 = acc_0;
// 更新最新的陀螺仪传感器数据
latest_gyr_0 = gyr_0;
// 锁定mBuf，确保对IMU数据队列的访问是线程安全的
mBuf.lock();
// 创建加速度数据队列
queue<pair<double, Eigen::Vector3d>> tmp_accBuf = accBuf;
// 创建陀螺仪数据队列
queue<pair<double, Eigen::Vector3d>> tmp_gyrBuf = gyrBuf;
// 解锁mBuf
mBuf.unlock();
// 遍历加速度和陀螺仪数据队列，执行快速预测
while(!tmp_accBuf.empty())
{
// 取出队列中的时间戳
double t = tmp_accBuf.front().first;
// 取出队列中的加速度数据
Eigen::Vector3d acc = tmp_accBuf.front().second;
// 取出队列中的陀螺仪数据
Eigen::Vector3d gyr = tmp_gyrBuf.front().second;
// 调用fastPredictIMU函数进行快速IMU预测
fastPredictIMU(t, acc, gyr);
// 从队列中移除已处理的数据
tmp_accBuf.pop();
tmp_gyrBuf.pop();
}
// 解锁mPropagate
mPropagate.unlock();
}这个函数的主要作用是更新估计器的最新状态。通过加锁机制确保线程安全，提取最新的位姿、速度、偏置以及传感器数据，并利用IMU数据队列中的数据进行快速预测。
我们来看下快速预测的函数fastPredictIMU()
//利用惯性传感器的数据，通过积分的方法对系统的姿态、位置和速度进行快速预测更新
//确保在每次新的传感器数据到来时都能进行及时的状态预测
void Estimator::fastPredictIMU(double t, Eigen::Vector3d linear_acceleration, Eigen::Vector3d angular_velocity)
{
// 计算当前时间与最新时间的时间差
double dt = t - latest_time;
// 更新最新时间为当前时间
latest_time = t;
// 计算未补偿重力的初始加速度
//latest_Q 是最新的姿态四元数，latest_acc_0 是最新的加速度计读数，latest_Ba 是加速度计的偏置，g 是重力加速度向量。计算的 un_acc_0 是在当前姿态下的线性加速度减去重力的初始值。
Eigen::Vector3d un_acc_0 = latest_Q * (latest_acc_0 - latest_Ba) - g;
// 计算未补偿的陀螺仪读数
//latest_gyr_0 是最新的陀螺仪读数，angular_velocity 是当前陀螺仪读数，latest_Bg 是陀螺仪的偏置。计算的 un_gyr 是未补偿的陀螺仪读数。
Eigen::Vector3d un_gyr = 0.5 * (latest_gyr_0 + angular_velocity) - latest_Bg;
// 更新姿态四元数
//Utility::deltaQ 是一个辅助函数，用于计算旋转四元数的增量。更新后的姿态四元数 latest_Q 是基于当前未补偿陀螺仪读数和时间差 dt 计算得到的。
latest_Q = latest_Q * Utility::deltaQ(un_gyr * dt);
// 计算未补偿重力的当前加速度
//linear_acceleration 是当前的加速度计读数，latest_Ba 是加速度计的偏置。计算的 un_acc_1 是在更新后的姿态下的线性加速度减去重力的当前值。
Eigen::Vector3d un_acc_1 = latest_Q * (linear_acceleration - latest_Ba) - g;
// 计算平均加速度
//计算 un_acc_0 和 un_acc_1 的平均值，得到未补偿的平均加速度 un_acc
Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
// 更新位置，包含了位移、速度和加速度的积分
//latest_P 是当前的位置，latest_V 是当前的速度，dt 是时间差，un_acc 是未补偿的平均加速度。通过二次积分公式更新位置。
latest_P = latest_P + dt * latest_V + 0.5 * dt * dt * un_acc;
// 更新速度
//通过一次积分公式更新速度
latest_V = latest_V + dt * un_acc;
// 更新最新的加速度传感器数据
latest_acc_0 = linear_acceleration;
// 更新最新的陀螺仪传感器数据
latest_gyr_0 = angular_velocity;
}该函数的预测过程是基于已有的IMU数据来进行的，利用了加速度计和陀螺仪传感器的数据来进行状态（包括位置、姿态、速度等）的预测。这个过程有助于在新的IMU数据到来之前，对系统的当前状态进行快速更新和预测，从而提高系统的实时性和连续性。
而对于滑动窗口函数slideWindow()
我们先来看下源码再说下笔者个人理解：
void Estimator::slideWindow()
{
// 创建一个计时对象，用于记录边缘化操作所需的时间
TicToc t_margin;
// 如果边缘化标志是 MARGIN_OLD，表示要移除滑动窗口中的最老帧
if (marginalization_flag == MARGIN_OLD)
{
// 记录最老帧的时间戳和姿态
double t_0 = Headers[0];
back_R0 = Rs[0];
back_P0 = Ps[0];
// 如果当前帧数等于滑动窗口大小，说明滑动窗口已满，需要移除最老帧
if (frame_count == WINDOW_SIZE)
{
// 将窗口中的每一帧数据向前移动一位
for (int i = 0; i < WINDOW_SIZE; i++)
{
Headers[i] = Headers[i + 1];
Rs[i].swap(Rs[i + 1]);
Ps[i].swap(Ps[i + 1]);
if(USE_IMU)
{
std::swap(pre_integrations[i], pre_integrations[i + 1]);
dt_buf[i].swap(dt_buf[i + 1]);
linear_acceleration_buf[i].swap(linear_acceleration_buf[i + 1]);
angular_velocity_buf[i].swap(angular_velocity_buf[i + 1]);
Vs[i].swap(Vs[i + 1]);
Bas[i].swap(Bas[i + 1]);
Bgs[i].swap(Bgs[i + 1]);
}
}
// 将窗口中的最后一帧的数据复制到倒数第二帧的位置
Headers[WINDOW_SIZE] = Headers[WINDOW_SIZE - 1];
Ps[WINDOW_SIZE] = Ps[WINDOW_SIZE - 1];
Rs[WINDOW_SIZE] = Rs[WINDOW_SIZE - 1];
if(USE_IMU)
{
Vs[WINDOW_SIZE] = Vs[WINDOW_SIZE - 1];
Bas[WINDOW_SIZE] = Bas[WINDOW_SIZE - 1];
Bgs[WINDOW_SIZE] = Bgs[WINDOW_SIZE - 1];
// 删除最后一帧的预积分信息，并创建一个新的预积分对象
delete pre_integrations[WINDOW_SIZE];
pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};
// 清空最后一帧的时间差、线性加速度和角速度的缓存
dt_buf[WINDOW_SIZE].clear();
linear_acceleration_buf[WINDOW_SIZE].clear();
angular_velocity_buf[WINDOW_SIZE].clear();
}
// 如果在初始阶段，或者solver_flag为INITIAL
if (true || solver_flag == INITIAL)
{
// 删除最老帧的预积分信息，并从所有图像帧的记录中删除该帧
//定义一个迭代器
map<double, ImageFrame>::iterator it_0;
//在 all_image_frame 中查找时间戳为 t_0 的帧，并将迭代器 it_0 指向该帧的位置。t_0 是最老帧的时间戳。
it_0 = all_image_frame.find(t_0);
//删除预积分信息
delete it_0->second.pre_integration;
//删除图像帧记录
all_image_frame.erase(all_image_frame.begin(), it_0);
}
// 调用 slideWindowOld 函数处理滑动窗口中最老帧的边缘化
slideWindowOld();
}
}
// 如果边缘化标志不是 MARGIN_OLD，表示要移除滑动窗口中的最新帧
else
{
// 如果当前帧数等于滑动窗口大小，说明滑动窗口已满，需要移除最新帧
if (frame_count == WINDOW_SIZE)
{
// 将最新帧的数据复制到倒数第二帧的位置
Headers[frame_count - 1] = Headers[frame_count];
Ps[frame_count - 1] = Ps[frame_count];
Rs[frame_count - 1] = Rs[frame_count];
if(USE_IMU)
{
// 将最新帧的IMU数据添加到倒数第二帧的预积分信息中
for (unsigned int i = 0; i < dt_buf[frame_count].size(); i++)
{
double tmp_dt = dt_buf[frame_count][i];
Vector3d tmp_linear_acceleration = linear_acceleration_buf[frame_count][i];
Vector3d tmp_angular_velocity = angular_velocity_buf[frame_count][i];
pre_integrations[frame_count - 1]->push_back(tmp_dt, tmp_linear_acceleration, tmp_angular_velocity);
dt_buf[frame_count - 1].push_back(tmp_dt);
linear_acceleration_buf[frame_count - 1].push_back(tmp_linear_acceleration);
angular_velocity_buf[frame_count - 1].push_back(tmp_angular_velocity);
}
// 将最新帧的速度、加速度偏置和角速度偏置复制到倒数第二帧的位置
Vs[frame_count - 1] = Vs[frame_count];
Bas[frame_count - 1] = Bas[frame_count];
Bgs[frame_count - 1] = Bgs[frame_count];
// 删除最后一帧的预积分信息，并创建一个新的预积分对象
delete pre_integrations[WINDOW_SIZE];
pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};
// 清空最后一帧的时间差、线性加速度和角速度的缓存
dt_buf[WINDOW_SIZE].clear();
linear_acceleration_buf[WINDOW_SIZE].clear();
angular_velocity_buf[WINDOW_SIZE].clear();
}
// 调用 slideWindowNew 函数处理滑动窗口中最新帧的边缘化
slideWindowNew();
}
}
}该函数主要功能是管理滑动窗口中的帧。当滑动窗口的大小达到预设的最大值时，它负责移除最老的帧或最新的帧，以便腾出空间用于新加入的帧。
根据边缘化标志位来判断移除窗口内最老帧还是最新帧：
边缘化最老帧：当滑动窗口已满且需要边缘化最老帧时，将窗口中的每一帧数据向前移动一位，并处理IMU预积分信息，然后调用 slideWindowOld() 进行进一步处理。
边缘化最新帧：当滑动窗口已满且需要边缘化最新帧时，将最新帧的数据复制到倒数第二帧的位置，并处理IMU预积分信息，然后调用 slideWindowNew() 进行进一步处理。
这样就实现了对滑动窗口中帧的管理，确保系统能够处理新的图像和IMU数据，同时保持窗口内帧的数量恒定。
slideWindowOld()源码：
void Estimator::slideWindowOld()
{
//增加最老帧计数
sum_of_back++;
//判断是否需要深度调整
// 判断当前求解器状态是否为非线性状态，如果是，shift_depth 为 true，否则为 false
bool shift_depth = solver_flag == NON_LINEAR ? true : false;
if (shift_depth)
{
// 定义变量 R0 和 R1 表示旋转矩阵，P0 和 P1 表示位移向量
Matrix3d R0, R1;
Vector3d P0, P1;
// 计算 R0 和 R1，分别是滑窗中最老帧和当前最老帧的旋转矩阵，乘以相机到IMU的外参
R0 = back_R0 * ric[0];
R1 = Rs[0] * ric[0];
// 计算 P0 和 P1，分别是滑窗中最老帧和当前最老帧的位置，进行平移变换
P0 = back_P0 + back_R0 * tic[0];
P1 = Ps[0] + Rs[0] * tic[0];
// 调用特征管理器的方法 removeBackShiftDepth 来移除最老帧的特征，并进行深度调整
f_manager.removeBackShiftDepth(R0, P0, R1, P1);
}
else
// 如果不需要深度调整，则直接移除最老帧的特征
f_manager.removeBack();
}slideWindowNew()源码：
void Estimator::slideWindowNew()
{
// 增加最前帧计数
sum_of_front++;
// 从特征管理器中移除当前帧数对应的前帧特征
f_manager.removeFront(frame_count);
}笔者读到这里发现slideWindow() 函数与优化函数中的边缘化处理部分有很多相似之处，主要区别是slideWindow() 负责实际移除特征和帧，会调用一些特定的函数来移除帧和管理特征，具体是 removeBackShiftDepth、removeBack 和 removeFront。，而优化函数更关注于优化过程中的边缘化操作。
对于removeBackShiftDepth()函数：
//管理特征点的起始帧索引：对于不是从最老帧开始的特征点，将其起始帧索引减1。
//移除最老帧上的特征点：对于从最老帧开始的特征点，将其在最老帧中的记录移除，并检查其剩余的观测帧数。如果观测帧数少于2，则移除该特征点。
//深度调整：对于剩余的特征点，计算其在当前帧的估计深度，并更新其深度值。
void FeatureManager::removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P)
{
// 遍历所有的特征点
for (auto it = feature.begin(), it_next = feature.begin();
it != feature.end(); it = it_next)
{
// 预先存储下一个迭代器的位置
it_next++;
// 如果特征点的起始帧不是0，说明该特征点没有出现在最老帧上
if (it->start_frame != 0)
// 将起始帧索引减1，因为最老帧已被移除，所有帧索引需要向前移动
it->start_frame--;
else
{
// 如果特征点的起始帧是0，说明该特征点出现在最老帧上
// 获取最老帧中该特征点的归一化图像平面坐标
Eigen::Vector3d uv_i = it->feature_per_frame[0].point;
// 移除该特征点在最老帧中的记录
it->feature_per_frame.erase(it->feature_per_frame.begin());
// 如果该特征点在移除最老帧后的帧数少于2，说明该特征点没有足够多的观测，移除该特征点
if (it->feature_per_frame.size() < 2)
{
// 从特征列表中移除该特征点
feature.erase(it);
continue;
}
else
{
// 计算该特征点在最老帧上的三维坐标
Eigen::Vector3d pts_i = uv_i * it->estimated_depth;
// 将特征点的三维坐标从最老帧的相机坐标系转换到世界坐标系
Eigen::Vector3d w_pts_i = marg_R * pts_i + marg_P;
// 将特征点的三维坐标从世界坐标系转换到当前帧的相机坐标系
Eigen::Vector3d pts_j = new_R.transpose() * (w_pts_i - new_P);
// 获取特征点在当前帧的深度值
double dep_j = pts_j(2);
// 如果深度值大于0，更新该特征点的估计深度
if (dep_j > 0)
it->estimated_depth = dep_j;
else
// 如果深度值小于等于0，设置该特征点的估计深度为初始深度
it->estimated_depth = INIT_DEPTH;
}
}
// remove tracking-lost feature after marginalize
/*
if (it->endFrame() < WINDOW_SIZE - 1)
{
feature.erase(it);
}
*/
}
}这个函数用于移除滑动窗口中最老帧的特征，并在深度调整情况下处理特征管理。具体来说，它会根据最老帧和当前帧的位姿信息，对特征点进行深度调整。
对于 removeBack()：
void FeatureManager::removeBack()
{
// 遍历所有的特征点
for (auto it = feature.begin(), it_next = feature.begin();
it != feature.end(); it = it_next)
{
// 预先存储下一个迭代器的位置
it_next++;
// 如果特征点的起始帧不是0，说明该特征点没有出现在最老帧上
if (it->start_frame != 0)
// 将起始帧索引减1，因为最老帧已被移除，所有帧索引需要向前移动
it->start_frame--;
else
{
// 如果特征点的起始帧是0，说明该特征点出现在最老帧上
// 移除该特征点在最老帧中的记录
it->feature_per_frame.erase(it->feature_per_frame.begin());
// 如果该特征点在移除最老帧后的观测帧数为0，说明该特征点已经没有任何观测帧，移除该特征点
if (it->feature_per_frame.size() == 0)
// 从特征列表中移除该特征点
feature.erase(it);
}
}
}这没什么好讲的，就是没有深度调整更新的removeBackShiftDepth()函数
removeFront()函数是删除最新帧的函数，跟上一个函数一样
void FeatureManager::removeFront(int frame_count)
{
// 遍历所有的特征点
for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next)
{
// 预先存储下一个迭代器的位置
it_next++;
// 如果特征点的起始帧是当前帧数，说明该特征点出现在当前最新帧上
if (it->start_frame == frame_count)
{
// 将起始帧索引减1，因为最新帧已被移除，所有帧索引需要向前移动
it->start_frame--;
}
else
{
// 计算该特征点在窗口中的位置
int j = WINDOW_SIZE - 1 - it->start_frame;
// 如果特征点的终止帧小于当前最新帧的前一帧，说明该特征点不需要处理，继续下一次迭代
if (it->endFrame() < frame_count - 1)
continue;
// 移除该特征点在当前最新帧中的记录
it->feature_per_frame.erase(it->feature_per_frame.begin() + j);
// 如果该特征点在移除最新帧后的观测帧数为0，说明该特征点已经没有任何观测帧，移除该特征点
if (it->feature_per_frame.size() == 0)
// 从特征列表中移除该特征点
feature.erase(it);
}
}
}
145
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
