![d335d1479cb18fce002897b1dedefb59.png](https://img-blog.csdnimg.cn/img_convert/d335d1479cb18fce002897b1dedefb59.png)
视觉部分流程图
![5a20cb06350c892e54db81bb9415f0a2.png](https://img-blog.csdnimg.cn/img_convert/5a20cb06350c892e54db81bb9415f0a2.png)
![9e42fbe1aaad093044552cbbd516e7d1.png](https://img-blog.csdnimg.cn/img_convert/9e42fbe1aaad093044552cbbd516e7d1.png)
Estimator类
![973647933c0eac901da8cff071812953.png](https://img-blog.csdnimg.cn/img_convert/973647933c0eac901da8cff071812953.png)
本文将重点讲解ProcessImage()函数
![9d7000a5c2b02db268170808c19666d9.png](https://img-blog.csdnimg.cn/img_convert/9d7000a5c2b02db268170808c19666d9.png)
1.检查两帧的视差判断是否为关键帧 f_manager.addFeatureCheckParallax()在 VINS-Mono 代码详细解读——feature_manager.cpp中已经解释
2.IMU预积分 IntegrationBase 在 VINS-Mono 代码详细解读——IMU离散中值预积分 中解释
3.在线标定外参 CalibrationExRotation 在 VINS-Mono 代码详细解读——基础储备:在线Cam到IMU的外参标定 InitialEXRotation类 中已经解释
本节将重点介绍初始化部分。
/**
* @brief 处理图像特征数据
* @Description addFeatureCheckParallax()添加特征点到feature中,计算点跟踪的次数和视差,判断是否是关键帧
* 判断并进行外参标定
* 进行视觉惯性联合初始化或基于滑动窗口非线性优化的紧耦合VIO
* @param[in] image 某帧所有特征点的[camera_id,[x,y,z,u,v,vx,vy]]s构成的map,索引为feature_id
* @param[in] header 某帧图像的头信息
* @return void
*/
void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const std_msgs::Header &header)
{
ROS_DEBUG("new image coming ------------------------------------------");
ROS_DEBUG("Adding feature points %lu", image.size());
// 1. 通过检测两帧之间的视差决定次新帧是否作为关键帧
if (f_manager.addFeatureCheckParallax(frame_count, image, td))//添加之前检测到的特征点到feature容器中,计算每一个点跟踪的次数,以及它的视差
marginalization_flag = MARGIN_OLD;//=0
else
marginalization_flag = MARGIN_SECOND_NEW;//=1
ROS_DEBUG("this frame is--------------------%s", marginalization_flag ? "reject" : "accept");
ROS_DEBUG("%s", marginalization_flag ? "Non-keyframe" : "Keyframe");
ROS_DEBUG("Solving %d", frame_count);
ROS_DEBUG("number of feature: %d", f_manager.getFeatureCount());
Headers[frame_count] = header;
// 2. 填充imageframe的容器以及更新临时预积分初始值
ImageFrame imageframe(image, header.stamp.toSec());//ImageFrame类包括特征点、时间、位姿Rt、预积分对象pre_integration、是否关键帧
imageframe.pre_integration = tmp_pre_integration;// tmp_pre_integration是之前IMU 预积分计算的
all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe));// map<double, ImageFrame> all_image_frame;
tmp_pre_integration = new IntegrationBase{
acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};//更新临时预积分初始值
// 3. 如果没有外参则标定IMU到相机的外参
if(ESTIMATE_EXTRINSIC == 2)
{
ROS_INFO("calibrating extrinsic param, rotation movement is needed");
if (frame_count != 0)
{
//得到两帧之间归一化特征点
vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
Matrix3d calib_ric;
//标定从camera到IMU之间的外参数
if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
{
ROS_WARN("initial extrinsic rotation calib success");
ROS_WARN_STREAM("initial extrinsic rotation: " << endl << calib_ric);
ric[0] = calib_ric;
RIC[0] = calib_ric;
ESTIMATE_EXTRINSIC = 1;// 完成外参标定
}
}
}
// 4. 初始化
if (solver_flag == INITIAL)//初始化
{
//frame_count是滑动窗口中图像帧的数量,一开始初始化为0,滑动窗口总帧数WINDOW_SIZE=10
//确保有足够的frame参与初始化
if (frame_count == WINDOW_SIZE)
{
bool result = false;
//有外参且当前帧时间戳大于初始化时间戳0.1秒,就进行初始化操作
if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)
{
// 4.1 重要!!! 视觉惯性联合初始化
result = initialStructure();
// 4.2 更新初始化时间戳
initial_timestamp = header.stamp.toSec();
}
if(result)//初始化成功
{
// 先进行一次滑动窗口非线性优化,得到当前帧与第一帧的位姿
solver_flag = NON_LINEAR;// 初始化更改为非线性
solveOdometry(); // 4.3 非线性化求解里程计
slideWindow();// 4.4 滑动窗
f_manager.removeFailures();// 4.5 剔除feature中估计失败的点(solve_flag == 2)0 haven't solve yet; 1 solve succ; 2 solve fail;
ROS_INFO("Initialization finish!");
// 4.6 初始化窗口中PVQ
last_R = Rs[WINDOW_SIZE];
last_P = Ps[WINDOW_SIZE];
last_R0 = Rs[0];
last_P0 = Ps[0];
}
else
slideWindow();// 初始化失败则直接滑动窗口
}
else
frame_count++;// 图像帧数量+1
}
else// 5. 紧耦合的非线性优化
{
TicToc t_solve;
solveOdometry();// 5.1 非线性化求解里程计
ROS_DEBUG("solver costs: %fms", t_solve.toc());
//5.2 故障检测与恢复,一旦检测到故障,系统将切换回初始化阶段
if (failureDetection())
{
ROS_WARN("failure detection!");
failure_occur = 1;
clearState();// 清空状态
setParameter();// 重设参数
ROS_WARN("system reboot!");
return;
}
TicToc t_margin;
slideWindow(); // 5.3 滑动窗口
f_manager.removeFailures();// 5.4 移除失败的
ROS_DEBUG("marginalization costs: %fms", t_margin.toc());
// prepare output of VINS
key_poses.clear();
for (int i = 0; i <= WINDOW_SIZE; i++)
key_poses.push_back(Ps[i]);// 关键位姿的位置 Ps
last_R = Rs[WINDOW_SIZE];
last_P = Ps[WINDOW_SIZE];
last_R0 = Rs[0];
last_P0 = Ps[0];
}
}
initialStructure()初始化函数
视觉结构初始化过程至关重要,多传感器融合过程中,当单个传感器数据不确定性较高,需要依赖其他传感器降低不确定性。先对纯视觉SFM初始化相机位姿,再和IMU对齐。
主要分为1、纯视觉SFM估计滑动窗内相机位姿和路标点逆深度。2、视觉惯性联合校准,SFM与IMU积分对齐。
![4d04d51ba00ee71eca34ab055c957c6e.png](https://img-blog.csdnimg.cn/img_convert/4d04d51ba00ee71eca34ab055c957c6e.png)
![f6859cc626141334fa5d6db7beac5f26.png](https://img-blog.csdnimg.cn/img_convert/f6859cc626141334fa5d6db7beac5f26.png)
![2a303cfe5f16e63109edf784ccefca43.png](https://img-blog.csdnimg.cn/img_convert/2a303cfe5f16e63109edf784ccefca43.png)
代码部分
![6f0373920523760b666488f6a4c9aa35.png](https://img-blog.csdnimg.cn/img_convert/6f0373920523760b666488f6a4c9aa35.png)
首先纯视觉SFM初始化sfm.construct()函数,之后视觉惯性联合初始化visualInitialAlign()函数。
视觉初始化入口在
bool Estimator::initialStructure()
初始化变量
Quaterniond Q[frame_count + 1];//旋转四元数Q
Vector3d T[frame_count + 1]; // 平移矩阵T
map<int, Vector3d> sfm_tracked_points; //存储SFM重建出特征点的坐标
vector<SFMFeature> sfm_f;// SFMFeature三角化状态、特征点索引、平面观测、位置坐标、深度
首先定义视觉SFM最重要的几个变量,旋转Q和平移T,map容器sfm_tracked_points保存SFM重建出的路标3D坐标,最重要的是SFMFeature类型的vector容器sfm_f。SFMFeature数据结构为:
// SFM 数据结构
struct SFMFeature
{
bool state;//特征点的状态&#