最近在忙别的事情，博客迟迟没更新，现在终于放假了，今天把初始化部分来分析一下。

源码：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<double, 3, 4> 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<double, 3, 4> 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<cv::Point2f> pts2D;
        vector<cv::Point3f> 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<double, ImageFrame>::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!");
            }
        }

谢谢大家，比心（有理解不到位或错误，欢迎指正，虚心学习）