VINS_Fusion代码注释(三)--------------------图像处理部分-----------------------------
VINS_Fusion程序注释(一) rosNodeTest.cpp
VINS_Fusion代码注释(二)---------IMU预积分部分---------------
1、具体程序处理顺序
rosNodeTest.cpp(sync_process)->inputimage()->processMeasurements()->分为两部分
a.processIMU() //上次博客注释
b.processImage() //这次要讲的图像处理部分
2、具体代码
void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const double header)
{
ROS_DEBUG("new image coming ------------------------------------------");
ROS_DEBUG("Adding feature points %lu", image.size());
//关键帧的判断依据是rotation-compensated过后的parallax足够大,并且tracking上的feature足够多;关键帧会保留在当前Sliding Window中,marginalize掉Sliding Window中最旧的状态,如果是非关键帧则优先marginalize掉。
//VINS里为了控制优化计算量,在实时情况下,只对当前帧之前某一部分帧进行优化,而不是全部历史帧。局部优化帧的数量就是窗口大小。为了维持窗口大小,需要去除旧的帧添加新的帧,也就是边缘化 Marginalization。到底是删去最旧的帧(MARGIN_OLD)还是删去刚刚进来窗口倒数第二帧(MARGIN_SECOND_NEW),就需要对当前帧与之前帧进行视差比较,如果是当前帧变化很小,就会删去倒数第二帧,如果变化很大,就删去最旧的帧。VINS 里把特征点管理和检查视差放在了同一个函数里,先添加特征点,再进行视差检查。
if (f_manager.addFeatureCheckParallax(frame_count, image, td))
{
marginalization_flag = MARGIN_OLD;
//printf("keyframe\n");
}
else
{
marginalization_flag = MARGIN_SECOND_NEW;
//printf("non-keyframe\n");
}
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;
ImageFrame imageframe(image, header);
imageframe.pre_integration = tmp_pre_integration;
all_image_frame.insert(make_pair(header, imageframe));
tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
/*
当外参完全不知道,VINS可以在线进行估计(rotation),先在processImage内进行初步估计,在后续优化时,在optimize函数中进行优化。
*/
//外参校准函数
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;
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,单目情况下,ric内只有一个值
ric[0] = calib_ric;
RIC[0] = calib_ric;
ESTIMATE_EXTRINSIC = 1;//若外参标定成功,则设置flag为1.
}
}
}
if (solver_flag == INITIAL)
{
// monocular + IMU initilization
if (!S