VINS 细节系列 - 坐标转换关系

前言

在学习VINS-Mono过程中，对初始化代码中的坐标转换关系做出了一些推导，特意写了博客记录一下，主要记录大体的变量转换关系。

相机和IMU的外参

若需要VINS标定旋转外参，则进入以下代码：

    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[0] = calib_ric;
        RIC[0] = calib_ric;
        ESTIMATE_EXTRINSIC = 1;
    }

 

这个值是固定的，因为相机和IMU固定安装在同一载体上，转换矩阵不变！这个矩阵可以通过在线估计的方式 估计出来，也可以人为估计出来！

 

全局sfm

    int l;
    if (!relativePose(relative_R, relative_T, l))
    {
        ROS_INFO("Not enough features or parallax; Move device around");
        return false;
    }

    GlobalSFM sfm;
    if(!sfm.construct(frame_count + 1, Q, T, l,
              relative_R, relative_T,
              sfm_f, sfm_tracked_points))
    {
        ROS_DEBUG("global SFM failed!");
        marginalization_flag = MARGIN_OLD;
        return false;
    }

	for (int i = 0; i < frame_num; i++)
	{
		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();
	}
	for (int i = 0; i < frame_num; i++)
	{
		T[i] = -1 * (q[i] * Vector3d(c_translation[i][0], c_translation[i][1], c_translation[i][2]));
	}

这些位姿具体表示的转换关系为：

 

用恢复出的三角点 pnp 求解出 all_image_frame 所有帧的位姿：

    for (int i = 0; frame_it != all_image_frame.end( ); frame_it++)
    {
       //省略
       ...
       frame_it->second.R = R_pnp * RIC[0].transpose();
       frame_it->second.T = T_pnp; 
    }

不妨记为：

速度、重力和尺度因子

    for (int i = 0; i <= frame_count; i++)
    {
        Matrix3d Ri = all_image_frame[Headers[i].stamp.toSec()].R;
        Vector3d Pi = all_image_frame[Headers[i].stamp.toSec()].T;
        Ps[i] = Pi;
        Rs[i] = Ri;
        all_image_frame[Headers[i].stamp.toSec()].is_key_frame = true;
    }

这些位姿此时表示为：

特征点的深度

真实尺度下的R、P、V和特征点深度

    for (frame_i = all_image_frame.begin(); frame_i != all_image_frame.end(); frame_i++)
    {
        if(frame_i->second.is_key_frame)
        {
            kv++;
            Vs[kv] = frame_i->second.R * x.segment<3>(kv * 3);
        }
    }

4.feature中的每个特征点深度estimated_depth都乘以s,得到在实际尺度下的特征点深度

    for (frame_i = all_image_frame.begin(); frame_i != all_image_frame.end(); frame_i++)
    {
        if(frame_i->second.is_key_frame)
        {
            kv++;
            Vs[kv] = frame_i->second.R * x.segment<3>(kv * 3);
        }
    }

R、P、V转换到世界坐标系

  Matrix3d R0 = Utility::g2R(g);
    double yaw = Utility::R2ypr(R0 * Rs[0]).x();
    R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;

    g = R0 * g;
    Matrix3d rot_diff = R0;

    for (int i = 0; i <= frame_count; i++)
    {
        Ps[i] = rot_diff * Ps[i];
        Rs[i] = rot_diff * Rs[i];
        Vs[i] = rot_diff * Vs[i];
    }