VINS-mono代码阅读 -- 初始化（二）

上次写到了PnP优化滑窗内所有帧的位姿，接着上次没写完的初始化部分。
在写新的部分之前，我们对前面求的旋转和平移什么的做一个简单的总结吧。

relativePose()
	用recoverPose() 恢复出了 参考帧到当前帧 的旋转和平移
	Rotation = R.transpose();
    Translation = -R.transpose() * T;
    然后就通过变换，relative_R, relative_T 就是 当前帧到参考帧 的旋转和平移

这一部分我一直没太搞懂，就是说q[l] * Quaterniond(relative_R)是当前帧在参考坐标系下的位姿，但是不应该是参考帧的位姿乘参考帧到当前帧的位姿吗？我理解的刚好是反的，有人给我解释一下的吗？
$$q_{c_l}^{c_l}⨂q_{c_i}^{c_l}=q_{c_i}^{c_l}$$

sfm.construct()
	q[l].w() = 1;
	q[l].x() = 0;
	q[l].y() = 0;
	q[l].z() = 0;
	T[l].setZero();
	
根据relative_R relative_T,得到 当前帧在参考坐标系下的位姿
	q[frame_num - 1] = q[l] * Quaterniond(relative_R);
	T[frame_num - 1] = relative_T;	

在这里，又进行了一次变换，c_Quat c_Rotation c_Translation 在变换之后表示的参考帧到当前帧的旋转，平移
	c_Quat[l] = q[l].inverse();
	c_Rotation[l] = c_Quat[l].toRotationMatrix();
	c_Translation[l] = -1 * (c_Rotation[l] * T[l]);
Pose[i]表示第l帧到第i帧的变换矩阵 [R | T]  参考帧到当前帧
	Pose[l].block<3, 3>(0, 0) = c_Rotation[l];
	Pose[l].block<3, 1>(0, 3) = c_Translation[l];
	
	c_Quat[frame_num - 1] = q[frame_num - 1].inverse();	
	c_Rotation[frame_num - 1] = c_Quat[frame_num - 1].toRotationMatrix();
	c_Translation[frame_num - 1] = -1 * (c_Rotation[frame_num - 1] * T[frame_num - 1]);
	Pose[frame_num - 1].block<3, 3>(0, 0) = c_Rotation[frame_num - 1];
	Pose[frame_num - 1].block<3, 1>(0, 3) = c_Translation[frame_num - 1];
	
滑窗里l帧后的每一帧，先和前一帧PnP求解位姿，得到位姿后再和！！！最新帧！！！进行三角化
		if (i > l)
		{
			Matrix3d R_initial = c_Rotation[i - 1];
			Vector3d P_initial = c_Translation[i - 1];
这里得到的是第l帧到第i帧的旋转和平移  参考帧到当前帧
			if(!solveFrameByPnP(R_initial, P_initial, i, sfm_f))
				return false;
			//R_initial 从3d-> 2d的旋转

			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];
		}


BA部分：这是加入的优化变量
		c_translation[i][0] = c_Translation[i].x();
		c_translation[i][1] = c_Translation[i].y();
		c_translation[i][2] = c_Translation[i].z();
		c_rotation[i][0] = c_Quat[i].w();
		c_rotation[i][1] = c_Quat[i].x();
		c_rotation[i][2] = c_Quat[i].y();
		c_rotation[i][3] = c_Quat[i].z();
		
这里优化得到的是第l帧坐标系到各帧的变换矩阵，应将其转变为每一帧在第l帧坐标上的位姿
需要获取各帧在l帧坐标系下的位姿，所以需要inverse操作，然后把特征点在第l帧坐标系下的3d坐标计算出来
		q[i].w() = c_rotation[i][0]; 
		q[i].x() = c_rotation[i][1]; 
		q[i].y() = c_rotation[i][2]; 
		q[i].z() = c_rotation[i][3]; 
		q[i] = q[i].inverse();
		
T[i] = -1 * (q[i] * Vector3d(c_translation[i][0], c_translation[i][1], c_translation[i][2]));

写完总结一下：

• q[l] T[l] 把第l帧看作参考坐标系，根据当前帧到参考帧的relative_R, relative_T，得到当前帧在参考坐标系下的位姿。这个参数是在下一步中使用的。
• pose[l] 存储的是参考帧到各帧的位姿变换。
• c_Quat[l], c_Rotation[l], c_Translation[l] 表示的是 参考帧到各帧的旋转和平移。
• c_quat[l], c_rotation[l], c_translation[l] 是在BA的时候使用的变量。

前一部分的代码求出了滑窗内所有帧

4. solvePnP

和之前一样，我们也把这部分代码按小的功能拆开来看。
对所有的图像帧
（1）如果是滑窗内部的帧，就设置为关键帧，并将他们转换到当前帧的IMU坐标系下

        if((frame_it->first) == Headers[i].stamp.toSec())
        { 
            frame_it->second.is_key_frame = true;
            frame_it->second.R = Q[i].toRotationMatrix() * RIC[0].transpose();
            frame_it->second.T = T[i];
            i++;
            continue;
        }
        if((frame_it->first) > Headers[i].stamp.toSec())
        {
            i++;
        }

这里计算出来的就是$b_k$到$l$的旋转和平移
$$R_{c_k}^{c_l}{R}_{b_k}^{c_k}=R_{b_k}^{c_l}$$
（2）不在滑窗内的帧，求出他么对应的IMU坐标系到第l帧的旋转和平移

		//注意： 这里的Q 和T 是图像帧的位姿，而不是求解PNP时所使用的坐标变化矩阵，两者具有对称关系
        Matrix3d R_inital = (Q[i].inverse()).toRotat ionMatrix();
        Vector3d P_inital = - R_inital * T[i];
        cv::eigen2cv(R_inital, tmp_r);
        cv::Rodrigues(tmp_r, rvec);
        cv::eigen2cv(P_inital, t);

        frame_it->second.is_key_frame = false;
		//获取PNP需要用到的 存储每个特征点三维点和图像坐标的 vector
        vector<cv::Point3f> pts_3_vector;
        vector<cv::Point2f> pts_2_vector;
        for (auto &id_pts : frame_it->second.points)
        {
            int feature_id = id_pts.first;
            for (auto &i_p : id_pts.second)
            {
                it = sfm_tracked_points.find(feature_id);
                if(it != sfm_tracked_points.end())
                {
                    Vector3d world_pts = it->second;
                    cv::Point3f pts_3(world_pts(0), world_pts(1), world_pts(2));
                    pts_3_vector.push_back(pts_3);
                    Vector2d img_pts = i_p.second.head<2>();
                    cv::Point2f pts_2(img_pts(0), img_pts(1));
                    pts_2_vector.push_back(pts_2);
                }
            }
        }
        cv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);  
		//保证特征点数 >5 ,这样就可以用5点法求解
        if(pts_3_vector.size() < 6)
        {
            cout << "pts_3_vector size " << pts_3_vector.size() << endl;
            ROS_DEBUG("Not enough points for solve pnp !");
            return false;
        }
		
        if (! cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1))
        {
            ROS_DEBUG("solve pnp fail!");
            return false;
        }
		//旋转向量转换成旋转矩阵
        cv::Rodrigues(rvec, r);
        MatrixXd R_pnp,tmp_R_pnp;
		//将矩阵从cv::Mat转换成eigen::Matrix类型
        cv::cv2eigen(r, tmp_R_pnp);
		//这里也需要将坐标变换矩阵变成 图像帧位姿， 并转换为IMU坐标系的位姿
        R_pnp = tmp_R_pnp.transpose();

        MatrixXd T_pnp;
        cv::cv2eigen(t, T_pnp);
        T_pnp = R_pnp * (-T_pnp);
        //将求得的旋转和平移存储起来，这里RIC[0].transpose() 是相机到IMU的旋转，这样就把相机坐标系中求出的位姿旋转到IMU坐标系去了
		//bk -> l的旋转和平移
		frame_it->second.R = R_pnp * RIC[0].transpose();
        frame_it->second.T = T_pnp;

最后两行，这块传入的是bk->l的旋转平移。

如果有错误的地方，请大家一定提出来
对于这部分的坐标变换，还是很绕的，还是需要花大量的心思来搞清楚的。
下一部分的visualInitialAlign(), 是这段代码的重头戏，为表示尊重，重开一章来写吧。