萌新学VINS-Fusion（二）------初始化
最近在忙别的事情，博客迟迟没更新，现在终于放假了，今天把初始化部分来分析一下。
源码：https://github.com/HKUST-Aerial-Robotics/VINS-Fusion
processImage函数
在介绍初始化之前，我们先从后端接收到数据开始。processImage函数就是后端解算位姿的函数。
该函数的参数分别为上一讲最后得到的特征容器，和header中存储的时标。首先调用addFeatureCheckParallax（）函数，该函数的作用分为两个方面，首先就是将特征容器里特征按照一定的方式重新整理后加入到feature_manager中进行管理，这里VF（VINS-Fusion缩写）与VM（VINS-MONO缩写）feature的管理方式基本是相同的，唯一的区别在于FeaturePerFrame结构体中加入了右侧图像特征点的信息。
如果你之前阅读过VM的代码，那么它的特征组织方式你就一定较为清楚，它的特征组织方式如下图
该函数的第二个部分主要是计算视差，来判断该帧与上一帧相似度是否过高，相似度高的则不保留为关键帧，后面直接marg掉，相似度不高的则保留为关键帧，滑窗将最老的帧marg掉。
这个函数之后，会进行一个是否在线标定外参（相机到imu）的判断，通常情况下，默认外参已知，这里不再分析，后续可能单独写一篇来讲解VINS里的外参标定。
接下来正式开始介绍初始化，VF里面初始化方式有三种，分别为单目+IMU、纯双目、双目+IMU。
单目+IMU初始化
单目+IMU初始化是VM的初始化方式，很多大佬在自己博客里有很多介绍（依然可以去看我上一篇博客里面给出的链接），采用的是视觉和IMU松耦合的方式，简要介绍一下其过程
首先要关键帧填满滑窗，SFM求解滑窗中所有帧的位姿，和所有路标点的位置，然后和IMU预积分的值对其，求解重力方向、尺度因子、陀螺零偏及每一帧的速度。
纯双目初始化
纯双目初始化是VF新加的，与双目+IMU的初始化相似度很大，代码展示就几行，如下
if(STEREO && !USE_IMU)
{
f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);//利用pnp计算位姿
f_manager.triangulate(frame_count, Ps, Rs, tic, ric);//三角化三维坐标
optimization();//非线性最小二乘优化重投影误差
if(frame_count == WINDOW_SIZE)
{
solver_flag = NON_LINEAR;
slideWindow();
ROS_INFO("Initialization finish!");
}
}
虽然上面initFramePoseByPnP函数在triangulate函数前面，但是第一次计算时，由于没有任何三维坐标信息，故估计位姿完成不了，还是会先进行三角化计算。所以我们先看triangulate函数中关于双目三角化的部分
if(STEREO && it_per_id.feature_per_frame[0].is_stereo)
{
int imu_i = it_per_id.start_frame;
Eigen::Matrix leftPose;
Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];//利用imu的位姿计算左相机位姿
Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];//利用imu的位姿计算左相机位姿
leftPose.leftCols<3>() = R0.transpose();
leftPose.rightCols<1>() = -R0.transpose() * t0;
//cout << "left pose " << leftPose << endl;
Eigen::Matrix rightPose;
Eigen::Vector3d t1 = Ps[imu_i] + Rs[imu_i] * tic[1];//利用imu的位姿计算左相机位姿
Eigen::Matrix3d R1 = Rs[imu_i] * ric[1];//利用imu的位姿计算左相机位姿
rightPose.leftCols<3>() = R1.transpose();
rightPose.rightCols<1>() = -R1.transpose() * t1;
//cout << "right pose " << rightPose << endl;
Eigen::Vector2d point0, point1;
Eigen::Vector3d point3d;
point0 = it_per_id.feature_per_frame[0].point.head(2);
point1 = it_per_id.feature_per_frame[0].pointRight.head(2);
//cout << "point0 " << point0.transpose() << endl;
//cout << "point1 " << point1.transpose() << endl;
triangulatePoint(leftPose, rightPose, point0, point1, point3d);//利用svd方法三角化
Eigen::Vector3d localPoint;//得到imu坐标系下的三维点坐标
localPoint = leftPose.leftCols<3>() * point3d + leftPose.rightCols<1>();//求得左相机坐标系下的坐标
double depth = localPoint.z();
if (depth > 0)
it_per_id.estimated_depth = depth;
else
it_per_id.estimated_depth = INIT_DEPTH;
/*
Vector3d ptsGt = pts_gt[it_per_id.feature_id];
printf("stereo %d pts: %f %f %f gt: %f %f %f \n",it_per_id.feature_id, point3d.x(), point3d.y(), point3d.z(),
ptsGt.x(), ptsGt.y(), ptsGt.z());
*/
continue;
}
虽然我们不用imu数据，但是我们的坐标系定义在imu上，所以需要经过外参转换一下位姿，还需要注意，相机位姿和相机坐标系之间存在一个求逆的过程，并且左右视图三角化这样求得的localPoint是在imu坐标系下的位置，还需要转化到左相机坐标系下，得到左相机坐标系下的深度。
一旦有了深度，当下一帧照片来到以后就可以利用pnp求解位姿了。initFramePoseByPnP代码如下
if(frameCnt > 0)
{
vector pts2D;
vector pts3D;
for (auto &it_per_id : feature)
{
if (it_per_id.estimated_depth > 0)
{
int index = frameCnt - it_per_id.start_frame;
if((int)it_per_id.feature_per_frame.size() >= index + 1)
{
Vector3d ptsInCam = ric[0] * (it_per_id.feature_per_frame[0].point * it_per_id.estimated_depth) + tic[0];
Vector3d ptsInWorld = Rs[it_per_id.start_frame] * ptsInCam + Ps[it_per_id.start_frame];//得到世界系下的三维坐标
cv::Point3f point3d(ptsInWorld.x(), ptsInWorld.y(), ptsInWorld.z());
cv::Point2f point2d(it_per_id.feature_per_frame[index].point.x(), it_per_id.feature_per_frame[index].point.y());//当前帧的归一化平面坐标
pts3D.push_back(point3d);
pts2D.push_back(point2d);
}
}
}
Eigen::Matrix3d RCam;
Eigen::Vector3d PCam;
// trans to w_T_cam
RCam = Rs[frameCnt - 1] * ric[0];
PCam = Rs[frameCnt - 1] * tic[0] + Ps[frameCnt - 1];
if(solvePoseByPnP(RCam, PCam, pts2D, pts3D))
{
// trans to w_T_imu
Rs[frameCnt] = RCam * ric[0].transpose();
Ps[frameCnt] = -RCam * ric[0].transpose() * tic[0] + PCam;
Eigen::Quaterniond Q(Rs[frameCnt]);
//cout << "frameCnt: " << frameCnt << " pnp Q " << Q.w() << " " << Q.vec().transpose() << endl;
//cout << "frameCnt: " << frameCnt << " pnp P " << Ps[frameCnt].transpose() << endl;
}
}
该代码断分为两部分，前部分先判断当前特征中那些已经三角化出深度的点，计算出世界系坐标存入pts3D，相应的当前帧的归一化平面坐标存入pts2D，之后由外参转化出上一阵的相机位姿，然后进行solvePoseByPnP运算，求解当前帧的位姿，以便后面的三角化，当然要转化成imu坐标系下的位姿。solvePoseByPnP函数代码我就不贴在这里了，主要就是进行一系列的坐标系转化后，利用opencv自带的solvePnP函数解算位姿。如果对各位姿变化很懵，建议参考VM的博客慢慢从头推一遍。
除了以上两个函数，还有optimization优化函数，就是优化重投影误差，在这里不做详细介绍了。
双目+IMU初始化
目前为止有两个版本的双目+IMU初始化，VF最开始发布时候，双目+IMU的初始化与单目的是一样的，最近更新后，就和纯双目的几乎一样了，唯一不同地方，是当帧数填满滑窗后，对IMU陀螺零偏进行了估计。代码量很少，如下
// stereo + IMU initilization
if(STEREO && USE_IMU)
{
f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
if (frame_count == WINDOW_SIZE)
{
map::iterator frame_it;
int i = 0;
for (frame_it = all_image_frame.begin(); frame_it != all_image_frame.end(); frame_it++)
{
frame_it->second.R = Rs[i];
frame_it->second.T = Ps[i];
i++;
}
solveGyroscopeBias(all_image_frame, Bgs);
for (int i = 0; i <= WINDOW_SIZE; i++)
{
pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
}
solver_flag = NON_LINEAR;
optimization();
slideWindow();
ROS_INFO("Initialization finish!");
}
}