前言
在前面文章中梳理过vins-mono在后端优化中边缘化约束的构建，和上篇文章不同，本文主要讲解在把待边缘化的状态量丢出滑窗后需要进行哪些操作。
滑窗时机
在vins-mono中滑窗函数运行有三种情况。从整个vins-mono运行时间过程看，第一种情况发生在初始化过程中，在系统运行后图像帧很快填满滑窗，然后进行initialStructure()函数，关于此函数在初始化详解中已经详细介绍。在这个函数返回false时候（求解all_image_frame位姿失败 || 视觉惯性对齐失败）就会进行滑窗操作，尽可能的保留关键帧然后重新初始化。第二种情况发生在初始化成功后，并且经过优化后，进行滑窗操作。第三种情况是在初始化之后，在正常运行过程中，优化处理之后进行滑窗操作。后两种情况类似，都是在优化之后把某一帧移除滑窗，第二种情况是初始化结束到正常运行的过渡。下面详细介绍滑窗函数。
slideWindow()函数详解
滑窗操作主要在该函数中完成，函数的功能就是把次新帧或者滑窗内最老帧从滑窗内抹除并把滑窗内相关的地图点进行管理权转接。首先是次新帧是关键帧的情况，该情况需要将滑窗内的第0帧的相关状态量从滑窗内移除：
if (marginalization_flag == MARGIN_OLD)//次新帧是关键帧
{
double t_0 = Headers[0].stamp.toSec();
back_R0 = Rs[0];
back_P0 = Ps[0];
if (frame_count == WINDOW_SIZE)
{
//一帧一帧交换
for (int i = 0; i < WINDOW_SIZE; i++)
{
Rs[i].swap(Rs[i + 1]);
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]);
Headers[i] = Headers[i + 1];
Ps[i].swap(Ps[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];
Vs[WINDOW_SIZE] = Vs[WINDOW_SIZE - 1];
Rs[WINDOW_SIZE] = Rs[WINDOW_SIZE - 1];
Bas[WINDOW_SIZE] = Bas[WINDOW_SIZE - 1];
Bgs[WINDOW_SIZE] = Bgs[WINDOW_SIZE - 1];
//预积分量置0
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();
if (true || solver_flag == INITIAL)
{
//手动释放堆区的预积分量
map<double, ImageFrame>::iterator it_0;
it_0 = all_image_frame.find(t_0);
delete it_0->second.pre_integration;
it_0->second.pre_integration = nullptr;
for (map<double, ImageFrame>::iterator it = all_image_frame.begin(); it != it_0; ++it)
{
if (it->second.pre_integration)
delete it->second.pre_integration;
it->second.pre_integration = NULL;
}
all_image_frame.erase(all_image_frame.begin(), it_0);
all_image_frame.erase(t_0);
}
slideWindowOld();
}
}在这种情况下，需要把一些容器内的状态量往前挪一位，对于滑窗内最后的状态量，时间戳、位姿、速度、两个偏置状态量保持和滑窗倒数第二帧一致，和imu相关的一些状态量，包括最后一帧预积分、时间序列容器、加速度容器、角速度容器等都进行清空。
此外，在all_image_frame中的预积分量也要都清空，只不过这里不能直接erase掉，要把堆区开辟的内存手动释放，这点就推荐使用智能指针，免去内存泄漏或者野指针的风险，操作也会更简单。
最后slideWindowOld函数需要进行一些状态量管理权的交接工作：
void Estimator::slideWindowOld()
{
sum_of_back++;
bool shift_depth = solver_flag == NON_LINEAR ? true : false;//如果初始化过了，由于地图点深度是第一次观测到该地图点所在图像帧的深度 要进行管理权转接
if (shift_depth)
{
Matrix3d R0, R1;
Vector3d P0, P1;
//back是被移除的帧的位姿
R0 = back_R0 * ric[0];//转到camera系
R1 = Rs[0] * ric[0];//当前最老的camera系位姿
P0 = back_P0 + back_R0 * tic[0];
P1 = Ps[0] + Rs[0] * tic[0];
f_manager.removeBackShiftDepth(R0, P0, R1, P1);//管理权交接
}
else
f_manager.removeBack();
}如果当前系统状态已经经过了优化，也就是solver_flag是NON_LINEAR状态，那么需要处理的是滑窗内第0帧观测到的地图点要进行管理权交接，地图点第一次被观测到的图像帧不是第0帧的情况需要把他们的start_frame减一即可。在上面代码中，[R0,P0]、[R1,P1]分别是没进行滑窗时候的第0帧的Rwc Pwc和经过滑窗操作（把相关状态量在滑窗内前移一位）后得到的新的R'wc P'wc，把这两组参数传到removeBackShiftDepth函数中：
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++;
if (it->start_frame != 0)
it->start_frame--;
else
{
Eigen::Vector3d uv_i = it->feature_per_frame[0].point; //原先的第一帧归一化坐标
it->feature_per_frame.erase(it->feature_per_frame.begin());
if (it->feature_per_frame.size() < 2)
{
feature.erase(it);
continue;
}
else
{
Eigen::Vector3d pts_i = uv_i * it->estimated_depth;//原先第0帧相机系下的坐标
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);
if (dep_j > 0)//更新3d点深度
it->estimated_depth = dep_j;
else
it->estimated_depth = INIT_DEPTH;
}
}
// remove tracking-lost feature after marginalize
/*
if (it->endFrame() < WINDOW_SIZE - 1)
{
feature.erase(it);
}
*/
}
}这个函数实现原理也比较简单，如果地图点的起始观测帧不是第0帧，那么就把起始帧索引减一；对于0索引是地图点的起始索引的地图点，首先把其在原始第0帧下的归一化坐标记录下，然后erase掉feature_per_frame的begin，再判断抹除这个frame后还有没有其他的观测frame，如果没有，就把这个特征点给erase掉；如果除了原来的第0帧还有其他图像帧能观测到该地图点，就把该点转到新的第0帧的相机坐标系下，并更新深度值，完成简单的管理权转移。
对于次新帧是非关键帧，滑窗处理思想类似：
if (frame_count == WINDOW_SIZE)
{
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);
}
Headers[frame_count - 1] = Headers[frame_count];
Ps[frame_count - 1] = Ps[frame_count];
Vs[frame_count - 1] = Vs[frame_count];
Rs[frame_count - 1] = Rs[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函数也是一个管理权转移的函数：
void Estimator::slideWindowNew()
{
sum_of_front++;
f_manager.removeFront(frame_count);
}在这个函数中，起作用的是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)
{
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);
if (it->feature_per_frame.size() == 0)
feature.erase(it);
}
}
}在这个函数中会遍历所有特征点，如果特征点起始观测帧是倒数第一帧，那么就把它的start_frame减一；如果其他特征点起始帧在最后一帧前面并且feature_per_frame中包含倒数第二帧，那么就把里面的倒数第二帧观测信息erase掉；如果erase掉后观测帧只有一个的特征点，直接把这个特征点从特征点管理器中erase掉，因为它已经不能构成约束。
后端其他补充
至此，关于滑窗操作就完成了，下面梳理一下关于vins-mono在优化和滑窗前后的一些整理操作：
1. 首先是在优化结束后，进行滑窗操作前，需要判断优化后的状态量是否合理，failureDetection()函数是判断状态量的合理函数。状态量不合理的情况主要有：加速度计或陀螺仪偏置过大、总位移过大、竖直方向位移过大和旋转角度过大。作者将特征点追踪个数过少这个条件注释了，可能是为了让图像暂时跟丢情况依靠imu继续运行一段时间。如果返回true，系统确实出了问题，vins会将所有状态量复位，该置零的置零，该erase的就释放掉。
bool Estimator::failureDetection()
{
if (f_manager.last_track_num < 2)
{
ROS_INFO(" little feature %d", f_manager.last_track_num);
//return true;
}
if (Bas[WINDOW_SIZE].norm() > 2.5)
{
ROS_INFO(" big IMU acc bias estimation %f", Bas[WINDOW_SIZE].norm());
return true;
}
if (Bgs[WINDOW_SIZE].norm() > 1.0)
{
ROS_INFO(" big IMU gyr bias estimation %f", Bgs[WINDOW_SIZE].norm());
return true;
}
Vector3d tmp_P = Ps[WINDOW_SIZE];
if ((tmp_P - last_P).norm() > 5)
{
ROS_INFO(" big translation");
return true;
}
if (abs(tmp_P.z() - last_P.z()) > 1)
{
ROS_INFO(" big z translation");
return true;
}
Matrix3d tmp_R = Rs[WINDOW_SIZE];
Matrix3d delta_R = tmp_R.transpose() * last_R;
Quaterniond delta_Q(delta_R);
double delta_angle;
delta_angle = acos(delta_Q.w()) * 2.0 / 3.14 * 180.0;
if (delta_angle > 50)
{
ROS_INFO(" big delta_angle ");
//return true;
}
return false;
}2.在优化、滑窗之后，需要将未能三角化并恢复尺度的地图点抹除。
void FeatureManager::removeFailures()
{
for (auto it = feature.begin(), it_next = feature.begin();
it != feature.end(); it = it_next)
{
it_next++;
if (it->solve_flag == 2)
feature.erase(it);
}
}本专栏关于vins-mono里程计部分全部梳理完成，只有完全弄懂这些才能自己进行二次加工实现特定场景的应用，之后还会更新回环检测内容。slamer永不言弃！笔者坚信，slam从业者社会需求不会像无人驾驶刚出来那会体量那么大，但是这门技术是会被一直需要的，而且用到的技术、思想是可以在很多技术领域应用的，所以如果真的热爱，向前冲就是了！
欢迎评论、私信、点赞、收藏！转载请注明出处。
608
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
