VINS-MONO源码学习（三）：初始化initialStructure（SFM部分）

VINS-MONO的初始化主要分为两个部分：纯视觉的SFM和视觉惯导对齐。这篇主要写SFM，下一篇写视觉惯导对齐。
首先用视觉的SFM求解滑窗内所有帧的位姿，和所有路标点的3D位置，然后跟IMU预积分的值对齐，求解重力方向、尺度因子、陀螺仪的bias和每一帧对应的速度。
在初始化之前，我们已经得到了名为all_image_frame的map，里面存有滑窗中的所有图像帧，包括这一帧对应的特征点3D坐标（未知）、预积分（已知）、是否为特征点（已知）、旋转和平移（未知）。
1、通过重力的标准差确保IMU有足够的激励：
如果标准差小于0.25，认为IMU没有足够的激励。

for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
        {
            double dt = frame_it->second.pre_integration->sum_dt;
            Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
            sum_g += tmp_g;
        }
        Vector3d aver_g;
        aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1);
        double var = 0;
        for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
        {
            double dt = frame_it->second.pre_integration->sum_dt;
            Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
            var += (tmp_g - aver_g).transpose() * (tmp_g - aver_g);
            //cout << "frame g " << tmp_g.transpose() << endl;
        }
        var = sqrt(var / ((int)all_image_frame.size() - 1));
        //ROS_WARN("IMU variation %f!", var);
        if(var < 0.25)
        {
            ROS_INFO("IMU excitation not enouth!");
            //return false;
        }

2、创建vector< SFMFeature >
基于之前的feature列表，建立SFMFeature，其中，observation表示观测到该特征点的帧ID和对应的图像坐标。

struct SFMFeature
{
    bool state;
    int id;
    vector<pair<int,Vector2d>> observation;
    double position[3];
    double depth;
};

3、relativePose：
在滑窗中寻找与当前帧的匹配特征点数较多的一个关键帧作为参考帧，通过求基础矩阵（cv::findFundamentalMat）计算当前帧到参考帧的T（cv::recoverPose，得到参考帧到当前帧的变换矩阵），设参考帧为第l帧，旋转和平移分别为relative_R，relative_T。
4、GlobalSFM 求出滑窗中每一帧的相机位姿和路标点3D坐标（也就是上面未知的两个变量）。
在GlobalSFM::construct()中，以参考帧为坐标原点，把参考帧（下标为l）和当前帧（下标为frame_num-1）的旋转和平移存到vector< SFMFeature >中。

q[l].w() = 1;
	q[l].x() = 0;
	q[l].y() = 0;
	q[l].z() = 0;
	T[l].setZero();
	q[frame_num - 1] = q[l] * Quaterniond(relative_R);
	T[frame_num - 1] = relative_T;

由上一步得到的参考帧（第l帧）和最新帧位姿变换，三角化得出特征点的3D坐标，用得到的坐标对其他帧求解PnP，得到其他帧的位姿，然后再使用求出位姿与最新帧继续做三角化，循环往复。

for (int i = l; i < frame_num - 1 ; i++)
	{
		// solve pnp
		if (i > l)
		{
			Matrix3d R_initial = c_Rotation[i - 1];
			Vector3d P_initial = c_Translation[i - 1];
			if(!solveFrameByPnP(R_initial, P_initial, i, sfm_f))
				return false;
			c_Rotation[i] = R_initial;
			c_Translation[i] = P_initial;
			c_Quat[i] = c_Rotation[i];
			Pose[i].block<3, 3>(0, 0) = c_Rotation[i];
			Pose[i].block<3, 1>(0, 3) = c_Translation[i];
		}

		// triangulate point based on the solve pnp result
		triangulateTwoFrames(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f);
	}

对第l帧与最新帧之间的每一帧，使用上面得到的pose，与第l帧再做三角化，恢复3D点。
再对l帧之前的帧求解PnP，并做三角化。
然后根据以上求出的点，对有至少两帧共视但没有被恢复的路标点做三角化恢复。
其中三角化过程（GlobalSFM::triangulateTwoFrames）：

//三角化两帧内的所有对应feature,放入sfm_f中，三角化成功的state设为true
void GlobalSFM::triangulateTwoFrames(int frame0, Eigen::Matrix<double, 3, 4> &Pose0, 
									 int frame1, Eigen::Matrix<double, 3, 4> &Pose1,
									 vector<SFMFeature> &sfm_f)
{
	assert(frame0 != frame1);
	for (int j = 0; j < feature_num; j++)
	{
		if (sfm_f[j].state == true)
			continue;
		bool has_0 = false, has_1 = false;
		Vector2d point0;
		Vector2d point1;
		for (int k = 0; k < (int)sfm_f[j].observation.size(); k++)
		{
			if (sfm_f[j].observation[k].first == frame0)
			{
				point0 = sfm_f[j].observation[k].second;
				has_0 = true;
			}
			if (sfm_f[j].observation[k].first == frame1)
			{
				point1 = sfm_f[j].observation[k].second;
				has_1 = true;
			}
		}
		if (has_0 && has_1)
		{
			Vector3d point_3d;
			triangulatePoint(Pose0, Pose1, point0, point1, point_3d);
			sfm_f[j].state = true;
			sfm_f[j].position[0] = point_3d(0);
			sfm_f[j].position[1] = point_3d(1);
			sfm_f[j].position[2] = point_3d(2);
			//cout << "trangulated : " << frame1 << "  3d point : "  << j << "  " << point_3d.transpose() << endl;
		}							  
	}
}

如果特征点已经三角化成功过，即state==1，则不重复三角化，所以，上面的多次三角化不会重复三角化同一个特征点。如果没有被三角化过，从observation中取出这两帧中这个特诊点的图像坐标，执行函数
triangulatePoint(Pose0, Pose1, point0, point1, point_3d);得到3D最标点。

void GlobalSFM::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
						Vector2d &point0, Vector2d &point1, Vector3d &point_3d)
{
	Matrix4d design_matrix = Matrix4d::Zero();
	design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0);
	design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1);
	design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0);
	design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1);
	Vector4d triangulated_point;
	triangulated_point =
		      design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();
	point_3d(0) = triangulated_point(0) / triangulated_point(3);
	point_3d(1) = triangulated_point(1) / triangulated_point(3);
	point_3d(2) = triangulated_point(2) / triangulated_point(3);
}

关于这部分使用SVD求解三角化的问题，再挖个坑，以后单独写一篇博文。
（参考：https://blog.csdn.net/michaelhan3/article/details/89483148
https://blog.csdn.net/weixin_37953603/article/details/108035773）

求解PnP（GlobalSFM::solveFrameByPnP）：
PnP问题是已知3D点，求解相机的位姿，所以在计算中只取成功三角化之后的点，并且这个点能够在i中观测到。为了确保计算准确，这样的特征点至少要有15个。

for (int j = 0; j < feature_num; j++)
	{
		if (sfm_f[j].state != true)
			continue;
		Vector2d point2d;
		for (int k = 0; k < (int)sfm_f[j].observation.size(); k++)
		{
			if (sfm_f[j].observation[k].first == i)
			{
				Vector2d img_pts = sfm_f[j].observation[k].second;
				cv::Point2f pts_2(img_pts(0), img_pts(1));
				pts_2_vector.push_back(pts_2);
				cv::Point3f pts_3(sfm_f[j].position[0], sfm_f[j].position[1], sfm_f[j].position[2]);
				pts_3_vector.push_back(pts_3);
				break;
			}
		}
	}
if (int(pts_2_vector.size()) < 15)
{
	printf("unstable features tracking, please slowly move you device!\n");
	if (int(pts_2_vector.size()) < 10)
		return false;
}

然后使用cv::solvePnP;完成计算。
类似的，使用PnP求解滑动窗口之外的帧的pose。

之后就是camera与IMU对齐：visualInitialAlign