VINS-Mono初始化过程中的坐标转换关系

最新推荐文章于 2022-12-08 21:26:54 发布

温柔只给意中人，

最新推荐文章于 2022-12-08 21:26:54 发布

阅读量942

点赞数 2

分类专栏： VINS-Mono 文章标签： slam 自动驾驶计算机视觉

本文链接：https://blog.csdn.net/qq_43343688/article/details/106471750

版权

VINS-Mono 专栏收录该内容

1 篇文章 0 订阅

订阅专栏

前言

在学习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;
    }

旋转外参 $ric\left[0\right]$ 是 camera 坐标系到 body 坐标系的转换关系，记为 $R_{b,c}$ ，平移外参在未后期优化前，默认为 $0$ ，记为 $t_{b,c}$ 。

全局sfm

首先选定一个 camera 参考坐标系 $c_l$ ：

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

设 WZ为滑动窗口大小。 $c_l$ 为滑窗中第 $l$ 帧到第 WZ 帧满足一定视差（三角化时相对更准确）并可以解出两帧之间位姿 relative_R、relative_T 的图像帧（一般就是 $c_0$ 帧）。relative_R、relative_T 表示的是第 WZ帧到第 $l$ 帧的变化。

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

在 initial_sfm.cpp 中，三角化求的路标点便以 $c_l$ 帧为参考系。经过一次 ceres 优化后得到滑窗中图像帧的位姿为 $q\left[i\right]$ 、 $T\left[i\right]$ ：

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

这些位姿具体表示的转换关系为：
$R_{c_{l},c_{0}}$ 、 $R_{c_{l},c_{1}}$ 、… 、 $R_{c_{l},c_{WZ-1}}$ 、 $R_{c_{l},c_{WZ}}$ ;
$P_{c_{l},c_{0}}$ 、 $P_{c_{l},c_{1}}$ 、… 、 $P_{c_{l},c_{WZ-1}}$ 、 $P_{c_{l},c_{WZ}}$ 。

用恢复出的三角点 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; 
    }

不妨记为：
$R_{c_{l},0}$ 、 $R_{c_{l},1}$ 、… 、 $R_{c_{l},b_{0}}$ 、 $R_{c_{l},9}$ 、… 、 $R_{c_{l},b_{1}}$ 、… 、 $R_{c_{l},b_{WZ-1}}$ 、… 、 $R_{c_{l},98}$ 、 $R_{c_{l},b_{WZ}}$ ;
$P_{c_{l},0}$ 、 $P_{c_{l},1}$ 、… 、 $P_{c_{l},c_{0}}$ 、 $P_{c_{l},9}$ 、… 、 $P_{c_{l},c_{1}}$ 、… 、 $P_{c_{l},c_{WZ-1}}$ 、… 、 $P_{c_{l},98}$ 、 $P_{c_{l},c_{WZ}}$ 。

速度、重力和尺度因子

在 initial_alignment.cpp 中，使用 all_image_frame 所有帧间的关系求出陀螺仪 bias、所有帧在body下的速度、 $c_l$ 帧下的重力因子以及尺度因子：
$v_{0}$ 、 $v_{1}$ 、… 、 $v_{b_{0}}$ 、 $v_{9}$ 、… 、 $v_{b_{1}}$ 、… 、 $v_{b_{WZ-1}}$ 、… 、 $v_{98}$ 、 $v_{b_{WZ}}$ 、 $g_{c_{l}}$ 、 $s$ 。
此后第 0 帧到第 WZ 帧的 $R s$ 、 $P s$ 被赋值：

    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_{c_{l},b_{0}}$ 、 $R_{c_{l},b_{1}}$ 、… 、 $R_{c_{l},b_{WZ-1}}$ 、 $R_{c_{l},b_{WZ}}$ ;
$P_{c_{l},c_{0}}$ 、 $P_{c_{l},c_{1}}$ 、… 、 $P_{c_{l},c_{WZ-1}}$ 、 $P_{c_{l},c_{WZ}}$ 。

特征点的深度

在 void triangulate(Vector3d Ps[ ],Vector3d tic[ ],Matrix3d ric[ ]) 函数中三角化求出特征点的深度。若特征点的深度值 estimated_depth小于0，则进行三角化求解。
一般来说，三角化的过程如下，已知多个视角匹配的归一化平面上的特征点 ${u,u^,,...\}$ 以及对应的多个视觉的投影矩阵 ${P,P^’,…\}$ ;

$\quad\quad\quad\quad\quad\quad\left[\begin{matrix}u_x\\u_y\\1\end{matrix}\right]=\lambda[R\quad t]\left[\begin{matrix}x\\y\\z\\1\end{matrix}\right]=\lambda\left[\begin{matrix}p_1\quad p_2\quad p_3\quad p_4 \\p_5\quad p_6\quad p_7\quad p_8\\p_9\quad p_{10}\quad p_{11}\quad p_{12}\end{matrix}\right]\left[\begin{matrix}x\\y\\z\\1\end{matrix}\right]=\lambda\left[\begin{matrix}P^T_0\\P^T_1\\P^T_2\end{matrix}\right]X$
$\left\{\begin{matrix}u_x=\lambda P^T_0X\\u_y=\lambda P^T_1X\\1\ =\ \lambda P^T_2X\end{matrix}\right.\Rightarrow\left[\begin{matrix}P^T_0-u_xP^T_2\\P^T_1-u_yP^T_2\end{matrix}\right]X=0$

那么 n 个匹配的特征点可构建 2n*4 大小的矩阵，SVD 求解得到 3D 点。
在VINS这个三角化的代码中，分成两种情况分析。

初始化没完成，此时 $P_{b_0,c_0}=0$ ，对 feature 中每个特征点：
若特征点的 start_frame 为0，
$t0=Ps[0]_{c_l,c_0}+Rs[0]_{c_l,b_0}*P_{b_0,c_0}=P_{c_l,c_0}$ ;
$R0=Rs[0]_{c_l,b_0}*R_{b_0,c_0}=R_{c_l,c_0}$ ;
对 feature_per_frame 的特征点的不同坐标：
第 0 帧： $t1=t0\ ; R1=R0\ ;\ t=0$ 即 $P_{c_0,c_0};$ $R = I$ 即 $R_{c_0,c_0};$
第 1 帧： $t1=Ps[1]_{c_l,c_1}+Rs[1]_{c_l,b_1}*P_{b_1,c_1}=P_{c_l,c_1}\ ;$
$\qquad\quad R1=Rs[1]_{c_l,b_1}*R_{b_1,c_1}=R_{c_l,c_1}\ ;$
$\qquad\quad\ t=R^{-1}_{c_l,c_0}(P_{c_l,c_1}-P_{c_l,c_0})=P_{c_0,c_1};$
$\qquad\quad R=R^{-1}_{c_l,c_0}*R_{c_l,c_1}=R_{c_0,c_1} ;$
代码中的投影矩阵 P是 $c_0$ 到 $c_1$ 的位姿变换；
第 2 帧： $t=P_{c_0,c_2};$ $R=R_{c_0,c_2} ;$
投影矩阵 P是 $c_0$ 到 $c_2$ 的位姿变换；
…
若特征点的 start_frame 为 1，2…，依次类推。
可以推出 SVD 求解得到的是每个特征点在其 start_frame 帧下的深度z。
初始化完成了，此时 $P_{b_0,c_0}$ 不为 0，P、V、Q皆转换到世界坐标系下，为 body 坐标系到世界坐标系的变换关系。
对 feature中每个特征点：
若特征点的 start_frame 为 0，
$t0=Ps[0]_{w,b_0}+Rs[0]_{w,b_0}*P_{b_0,c_0}=P_{w,c_0}$ ;
$R0=Rs[0]_{w,b_0}*R_{b_0,c_0}=R_{w,c_0}$ ;
第 0 帧： $t1=t0\ ; R1=R0\ ;\ t=0$ 即 $P_{c_0,c_0};$ $R = I$ 即 $R_{c_0,c_0};$
第 1 帧： $t1=Ps[1]_{c_l,c_1}+Rs[1]_{c_l,b_1}*P_{b_1,c_1}=P_{c_l,c_1}\ ;$
$\qquad\quad R1=Rs[1]_{c_l,b_1}*R_{b_1,c_1}=R_{c_l,c_1}\ ;$
$\qquad\quad\ t=R^{-1}_{c_l,c_0}(P_{c_l,c_1}-P_{c_l,c_0})=P_{c_0,c_1};$
$\qquad\quad R=R^{-1}_{c_l,c_0}*R_{c_l,c_1}=R_{c_0,c_1} ;$
代码中的投影矩阵 P 是 $c_0$ 到 $c_1$ 的位姿变换；
第 2 帧： $t=P_{c_0,c_2};$ $R=R_{c_0,c_2} ;$
投影矩阵 P 是 $c_0$ 到 $c_2$ 的位姿变换；
若特征点的 start_frame 为 1，2…，依次类推。
同样可以推出 SVD 求解得到的是每个特征点在其 start_frame 帧下的深度z。

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

此时在滑窗中：
1. $R$ 表示的仍是各body坐标系到 $c_l$ 的转换关系：
$\quad$ $Rs[0]:Rs[0]_{c_{l},b_{0}}$ ;
$\quad$ $Rs[1]:Rs[1]_{c_{l},b_{1}}$ ;
$\quad$ …
$\quad$ $Rs[WZ]:Rs[WZ]_{c_{l},b_{WZ}}$ ;
2. $P$ 通过转换，表示body坐标系到 $c_l$ 的转换关系：

    for (int i = frame_count; i >= 0; i--)
    {
        Ps[i] = s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0]);
    }

$\quad$ $Ps[0]:s*Ps[0]_{{c_l},{c_0}}-Rs[0]_{{c_l},{b_0}}*Ps[0]_{{b_0},{c_0}}-(s*Ps[0]_{{c_l},{c_0}}-Rs[0]_{{c_l},{b_0}}*Ps[0]_{{b_0},{c_0}})=0即s*Ps[0]_{{c_l},{b_0}}$ ;
$\quad$ $Ps[1]:s*Ps[1]_{{c_l},{c_1}}-Rs[1]_{{c_l},{b_1}}*Ps[1]_{{b_1},{c_1}}-(s*Ps[0]_{{c_l},{c_0}}-Rs[0]_{{c_l},{b_0}}*Ps[0]_{{b_0},{c_0}})即s*Ps[1]_{{c_l},{b_1}}$ ;
$\quad$ …
$\quad$ $Ps[WZ]:即s*Ps[WZ]_{{c_l},{b_{WZ}}}$ ;
3. $V$ 被转换以 $c_l$ 为参考坐标系：

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

$\quad$ $Vs[0]:Rs[0]_{c_{l},b_{0}}*v_{b_0}$ ;
$\quad$ $Vs[1]:Rs[1]_{c_{l},b_{1}}*v_{b_1}$ ;
$\quad$ …
$\quad$ $Vs[WZ]:Rs[WZ]_{c_{l},b_{WZ}}*v_{b_{WZ}}$ ;
4.feature中的每个特征点深度estimated_depth都乘以s,得到在实际尺度下的特征点深度。

    for (auto &it_per_id : f_manager.feature)
    {
        it_per_id.used_num = it_per_id.feature_per_frame.size();
        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
            continue;
        it_per_id.estimated_depth *= s;
    }

当知道真实的米制平移时，投影矩阵 ${P,P^’,…\}$ 的平移带有尺度;
$\left[\begin{matrix}u_x\\u_y\\1\end{matrix}\right]=\lambda^{'} [R\quad t]\left[\begin{matrix}x\\y\\z^{'}\\1\end{matrix}\right]=\lambda^{'} \left[\begin{matrix}p_1\quad p_2\quad p_3\quad sp_4 \\p_5\quad p_6\quad p_7\quad sp_8\\p_9\quad p_{10}\quad p_{11}\quad sp_{12}\end{matrix}\right]\left[\begin{matrix}x\\y\\z^{'}\\1\end{matrix}\right]=\lambda^{'} \left[\begin{matrix}p_1\quad p_2\quad p_3\quad p_4 \\p_5\quad p_6\quad p_7\quad p_8\\p_9\quad p_{10}\quad p_{11}\quad p_{12}\end{matrix}\right]\left[\begin{matrix}x\\y\\z^{'}\\s\end{matrix}\right]=\lambda^{'} s\left[\begin{matrix}p_1\quad p_2\quad p_3\quad p_4 \\p_5\quad p_6\quad p_7\quad p_8\\p_9\quad p_{10}\quad p_{11}\quad p_{12}\end{matrix}\right]\left[\begin{matrix}x/s\\y/s\\z^{'}s\\1\end{matrix}\right]=\lambda\left[\begin{matrix}p_1\quad p_2\quad p_3\quad p_4 \\p_5\quad p_6\quad p_7\quad p_8\\p_9\quad p_{10}\quad p_{11}\quad p_{12}\end{matrix}\right]\left[\begin{matrix}x/s\\y/s\\z^{'}/s\\1\end{matrix}\right]$
此时求解的estimator_depth $z^{'}= z*s$ 。

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

1.首先求得将归一化的 $g_{c_l}$ 转换到世界坐标系 $0\ 0\ 1)^T$ 的旋转矩阵 $R_{w,{c_l}}$ ,使用XYZ顺序欧拉角来表示的话不妨记为 $R_{w,{c_l}}=\{Rz_{w,{c_l}}*Ry_{w,{c_l}}*Rx_{w,{c_l}}\}$ 。用{…}表示由欧拉角表示的旋转矩阵。
2.令 $R0=\{Ry_{w,{c_l}}*Rx_{w,{c_l}}\}$ , $R 0$ 是yaw角为0的 $R_{w,{c_l}}$ ,yaw角的变化不影响 $R 0$ 有和 $R_{w,{c_l}}$ 一样的转换重力向量参考系的功能，因为 $0\ 0\ 1)^T$ 没有xy平面的值（除了0），可以参考《如何通俗地解释欧拉角？之后为何要引入四元数？》直观感受一下欧拉角。
3.求得 $R0*Rs[0]_{c_l,b_0}$ 的yaw角，并用XYZ顺序欧拉角{-yaw，0，0}形成旋转矩阵，记作 $Rz^{-1}_{w,b_0}$ ;最终的rot_diff为 $\{Rz^{-1}_{w,b_0}*Ry_{w,{c_l}}*Rx_{w,{c_l}}\}$ 。
4.用rot_diff转换第0帧到第WZ帧的Rs、Ps、Vs。
$Ps[0]_{w,b_0}=\{Rz^{-1}_{w,b_0}*Ry_{w,{c_l}}*Rx_{w,{c_l}}\}*s*Ps[0]_{c_l,b_0}$
$Rs[0]_{w,b_0}=\{Rz^{-1}_{w,b_0}*Ry_{w,{c_l}}*Rx_{w,{c_l}}\}*Rs[0]_{c_l,b_0}$
$Ps[0]_{w,b_0}=\{Rz^{-1}_{w,b_0}*Ry_{w,{c_l}}*Rx_{w,{c_l}}\}*Rs[0]_{c_l,b_0}*v_{b_0}$
…
依次类推得到其它帧的变换。
可见rot_diff将第0帧到WZ帧 $c_l$ 帧坐标系下的Rs、Ps、Vs的水平方向即roll和pitch和变得世界坐标系的roll和pitch一致（yaw不可观），同时让初始帧 $Rs[0]_{w,b_0}$ 的yaw角为0。后期优化时还会将滑窗中帧的yaw角调整到优化前的大小，防止优化值乱漂，这些留着下篇博客在讨论吧。

温柔只给意中人，

关注

2
点赞
踩
7

收藏

觉得还不错? 一键收藏
7
评论
VINS-Mono初始化过程中的坐标转换关系

前言在学习VINS-Mono过程中，对初始化代码中的坐标转换关系做出了一些推导。这也是我第一次写CSDN，会努力写的清晰连贯一些。由于我只是一个SLAM小菜鸟，其中出错之处敬请指正。相机和IMU的外参若需要VINS标定旋转外参，则进入以下代码： if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric)) { ROS
复制链接

扫一扫