VINS-Mono初始化中的坐标转换

淩亂°似流年 2023-03-04 15:28 1阅读 0赞

### VINS-Mono初始化中的坐标转换 ###

*   *  0.引言
     *  1.外参.相机与IMU
     *  2.全局SFM
     *  3.速度、重力和尺度因子

## 0.引言 ##

*  [参考][Link 1]

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ZiXzk0MTIxOQ_size_16_color_FFFFFF_t_70_pic_center]

## 1.外参.相机与IMU ##

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

//标定从camera到IMU之间的旋转矩阵
    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;
    }

旋转外参  r i c \[ 0 \] ric\[0\] ric\[0\] 是 camera 坐标系到 body 坐标系的转换关系，记为 R b , c R\_\{b,c\} Rb,c ，平移外参在未后期优化前，默认为 0，记为 t b , c t\_\{b,c\} tb,c .

## 2.全局SFM ##

首先选定一个 camera 参考坐标系 c l c\_l cl:

//保证具有足够的视差,由F矩阵恢复Rt
    //第l帧是从第一帧开始到滑动窗口中第一个满足与当前帧的平均视差足够大的帧(视差>30,内点>12)，会作为参考帧到下面的全局sfm使用
    //此处的relative_R，relative_T为当前帧(窗口内最后一帧)到参考帧（第l帧,多数情况是窗口内第一帧）的坐标系变换Rt
    if (!relativePose(relative_R, relative_T, l))
    {
        
        ROS_INFO("Not enough features or parallax; Move device around");
        return false;
    }

设 WZ为滑动窗口大小。 c l c\_l cl是滑动窗口中第一帧开始到第一个满足与当前帧的平均视差足够大的帧(视差>30,内点>12)。relative\_R、relative\_T 表示当前帧到  c l c\_l cl 的R、t

/**
     * @brief   纯视觉sfm，求解窗口中的所有图像帧的位姿和特征点坐标
     * @param[in]   frame_num	窗口总帧数（frame_count + 1）
     * @param[out]  q 	窗口内图像帧的旋转四元数q（相对于第l帧）
     * @param[out]	T 	窗口内图像帧的平移向量T（相对于第l帧）
     * @param[in]  	l 	第l帧
     * @param[in]  	relative_R	当前帧到第l帧的旋转矩阵
     * @param[in]  	relative_T 	当前帧到第l帧的平移向量
     * @param[in]  	sfm_f		所有特征点
     * @param[out]  sfm_tracked_points 所有在sfm中三角化的特征点ID和坐标
     * @return  bool true:sfm求解成功
    */
    bool GlobalSFM::construct(int frame_num, Quaterniond* q, Vector3d* T, int l,
    			  const Matrix3d relative_R, const Vector3d relative_T,
    			  vector<SFMFeature> &sfm_f, map<int, Vector3d> &sfm_tracked_points)
    {
        
    
    	//三角化frame0和frame1间所有对应点，三角化出来的点是世界坐标系下的3D点
    	void GlobalSFM::triangulateTwoFrames(int frame0, Eigen::Matrix<double, 3, 4> &Pose0, 
    									 int frame1, Eigen::Matrix<double, 3, 4> &Pose1,
    									 vector<SFMFeature> &sfm_f)
    
    ...省略...
    
    	//输出是优化过后的相机位姿
    	//这里得到的是第l帧坐标系到各帧的变换矩阵，应将其转变为各帧在第l帧坐标系上的位姿
    	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]));
    	}

在 initial\_sfm.cpp的`GlobalSFM::construct()`中，三角化求的路标点便以  c l c\_l cl （也是很多讲解资料中的 c 0 c\_0 c0） 帧为参考系。经过 ceres 优化后得到滑窗中图像帧的位姿为  q \[ i \] 、 T \[ i \] q\[i\]、T\[i\] q\[i\]、T\[i\]:

R c l , c 0 , R c l , c 1 , … , R c l , c W Z − 1 , R c l , c W Z R\_\{c\_\{l\}, c\_\{0\}\}, R\_\{c\_\{l\}, c\_\{1\}\}, \\ldots, R\_\{c\_\{l\}, c\_\{WZ-1\}\}, R\_\{c\_\{l\}, c\_\{WZ\}\} Rcl,c0,Rcl,c1,…,Rcl,cWZ−1,Rcl,cWZ P c l , c 0 , P c l , c 1 , … , P c l , c W Z − 1 , P c l , c W Z P\_\{c\_\{l\}, c\_\{0\}\}, P\_\{c\_\{l\}, c\_1\}, \\ldots, P\_\{c\_\{l\}, c\_\{WZ-1\}\}, P\_\{c\_\{l\}, c WZ\} Pcl,c0,Pcl,c1,…,Pcl,cWZ−1,Pcl,cWZ

以 c l c\_l cl(即 c 0 c\_0 c0)为原点作为世界坐标系，故上式还可以写为(世界坐标系下，各帧的位姿)：

R w l , w 0 , R w l , w 1 , … , R w l , w W Z − 1 , R w l , w W Z R\_\{w\_\{l\}, w\_\{0\}\}, R\_\{w\_\{l\}, w\_\{1\}\}, \\ldots, R\_\{w\_\{l\}, w\_\{WZ-1\}\}, R\_\{w\_\{l\}, w\_\{WZ\}\} Rwl,w0,Rwl,w1,…,Rwl,wWZ−1,Rwl,wWZ P w l , w 0 , P w l , w 1 , … , P w l , w W Z − 1 , P w l , c W Z P\_\{w\_\{l\}, w\_\{0\}\}, P\_\{w\_\{l\}, w\_1\}, \\ldots, P\_\{w\_\{l\}, w\_\{WZ-1\}\}, P\_\{w\_\{l\}, c WZ\} Pwl,w0,Pwl,w1,…,Pwl,wWZ−1,Pwl,cWZ

**后续看到 c l c\_l cl均可看作世界坐标系原点！**

对于所有的图像帧，包括不在滑动窗口中的，提供初始的RT估计，然后solvePnP进行优化,得到每一帧的姿态（用恢复出的三角点 pnp 求解出 all\_image\_frame 所有帧的位姿）：

for (int i = 0; frame_it != all_image_frame.end( ); frame_it++)
    {
        
    ...省略...
    
    	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::cv2eigen(r, tmp_R_pnp);//pnp求解出来的是世界坐标系到相机坐标系的变换
    	//这里也同样需要将坐标变换矩阵转变成图像帧位姿，并转换为IMU坐标系的位姿
    	R_pnp = tmp_R_pnp.transpose();//转为，相机坐标系到世界坐标系的变换
    	MatrixXd T_pnp;
    	cv::cv2eigen(t, T_pnp);
    	T_pnp = R_pnp * (-T_pnp);
    	frame_it->second.R = R_pnp * RIC[0].transpose();
    	frame_it->second.T = T_pnp;
    }

记为（此处的结果是 R w , I R\_\{w,I\} Rw,I）：

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 W Z − 1 , … , R c c , 98 , R c l , b W Z R\_\{c\_\{l\}, 0\},R\_\{c\_\{l\}, 1\}, \\ldots, R\_\{c\_\{l\}, b\_\{0\}\}, R\_\{c\_\{l\}, 9\}, \\ldots, R\_\{c\_\{l\}, b\_\{1\}\}, \\ldots, R\_\{c\_\{l\}, b\_\{W Z-1\}\}, \\ldots, R\_\{c\_\{c\}, 98\}, R\_\{c\_\{l\}, b\_\{W Z\}\} Rcl,0,Rcl,1,…,Rcl,b0,Rcl,9,…,Rcl,b1,…,Rcl,bWZ−1,…,Rcc,98,Rcl,bWZ P c l , 0 , P c l , 1 , … , P c l , c 0 , P c l , 9 r , … , P c l , c 1 , … , P c l , c W Z − 1 , … , P c l , 98 , P c l , c W Z P\_\{c\_\{l\}, 0\}, P\_\{c\_\{l\}, 1\}, \\ldots, P\_\{c l, c 0\}, P\_\{c l\}, 9\_\{r\}, \\ldots, P\_\{c l, c\_\{1\}\}, \\ldots, P\_\{c l, c W Z-1\}, \\ldots, P\_\{c l\}, 98, P\_\{c l\}, c\_\{W Z\} Pcl,0,Pcl,1,…,Pcl,c0,Pcl,9r,…,Pcl,c1,…,Pcl,cWZ−1,…,Pcl,98,Pcl,cWZ

附：

//特征点id，矫正后归一化平面的3D点(x,y,z=1)，像素2D点(u,v)，像素的速度(vx,vy)，
    map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image
    
    //键是图像帧的时间戳，值是图像帧类
    //图像帧类可由图像帧的特征点与时间戳构造，此外还保存了位姿Rt，预积分对象pre_integration，是否是关键帧。
    //Rt是图像帧的位姿，而不是求解PNP时所用的坐标系变换矩阵
    map<double, ImageFrame> all_image_frame
    class ImageFrame
    {
        
        public:
            ImageFrame(){
        };
            ImageFrame(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>>& _points, double _t):t{
        _t},is_key_frame{
        false}
            {
        
                points = _points;
            };
            map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>> > > points;
            double t;
            Matrix3d R;
            Vector3d T;
            IntegrationBase *pre_integration;
            bool is_key_frame;
    };

## 3.速度、重力和尺度因子 ##

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ZiXzk0MTIxOQ_size_16_color_FFFFFF_t_70_pic_center]  
在 initial\_alignment.cpp 中，使用 all\_image\_frame 所有帧间的关系求出陀螺仪 bias、所有帧在body下的速度、 c l c\_l cl 帧下的重力因子、尺度因子：

//计算陀螺仪偏置，尺度，重力加速度和速度
    bool result = VisualIMUAlignment(all_image_frame, Bgs, g, x);
    
    
    bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x)
    {
        
        solveGyroscopeBias(all_image_frame, Bgs);//计算陀螺仪偏置
    
        if(LinearAlignment(all_image_frame, g, x))//计算尺度，重力加速度和速度
            return true;
        else 
            return false;
    }

v 0 , v 1 , … , v b 0 , v 9 , … , v b 1 , … , v b W Z − 1 , … , v 98 , v b W Z , g c l , s v\_\{0\}, v\_\{1\}, \\ldots, v\_\{b 0\}, v\_\{9\}, \\ldots, v\_\{b\_\{1\}\}, \\ldots, v\_\{b\_\{W Z-1\}\}, \\ldots, v\_\{98\} ,v\_\{b\_\{W Z\}\}, g\_\{c\_\{l\}\}, s v0,v1,…,vb0,v9,…,vb1,…,vbWZ−1,…,v98,vbWZ,gcl,s  
即是： X I = \[ v b 0 b 0 , v b 1 b 0 , ⋯   , v b n b a n , g c 1 , s \] ∈ R 3 ( n + 1 ) + 3 + 1 X\_\{I\}=\\left\[v\_\{b\_\{0\}\}^\{b\_\{0\}\}, v\_\{b\_\{1\}\}^\{b\_\{0\}\}, \\cdots, v\_\{b\_\{n\}\}^\{b\_\{a\_\{n\}\}\}, g^\{c 1\}, s\\right\] \\in \\mathbb\{R\}^\{3(n+1)+3+1\} XI=\[vb0b0,vb1b0,⋯,vbnban,gc1,s\]∈R3(n\+1)\+3\+1  
此后第 0 帧到第 WZ 帧的  R s R\_s Rs、 R s R\_s Rs 被赋值：

/**
     * @brief   视觉惯性联合初始化
     * @Description 陀螺仪的偏置校准(加速度偏置没有处理) 计算速度V[0:n] 重力g 尺度s
     *              更新了Bgs后，IMU测量量需要repropagate  
     *              得到尺度s和重力g的方向后，需更新所有图像帧在世界坐标系下的Ps、Rs、Vs
     * @return  bool true：成功
     */
    bool Estimator::visualInitialAlign()
    {
        
        TicToc t_g;
        VectorXd x;
    
        //计算陀螺仪偏置，尺度，重力加速度和速度
        bool result = VisualIMUAlignment(all_image_frame, Bgs, g, x);
        if(!result)
        {
        
            ROS_DEBUG("solve g failed!");
            return false;
        }
    
        // 得到所有图像帧的位姿Ps、Rs，并将其置为关键帧
        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;
        }
    
        //将所有特征点的深度置为-1
        VectorXd dep = f_manager.getDepthVector();
        for (int i = 0; i < dep.size(); i++)
            dep[i] = -1;
        f_manager.clearDepth(dep);
    
        //重新计算特征点的深度
        Vector3d TIC_TMP[NUM_OF_CAM];
        for(int i = 0; i < NUM_OF_CAM; i++)
            TIC_TMP[i].setZero();
        ric[0] = RIC[0];
        f_manager.setRic(ric);
        f_manager.triangulate(Ps, &(TIC_TMP[0]), &(RIC[0]));
    
        double s = (x.tail<1>())(0);
    
        //陀螺仪的偏置bgs改变，重新计算预积分
        for (int i = 0; i <= WINDOW_SIZE; i++)
        {
        
            pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
        }
        
        //将Ps、Vs、depth尺度s缩放
        for (int i = frame_count; i >= 0; i--)
            //Ps转变为第i帧imu坐标系到第0帧imu坐标系的变换
            Ps[i] = s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0]);
        
        int kv = -1;
        map<double, ImageFrame>::iterator frame_i;
        for (frame_i = all_image_frame.begin(); frame_i != all_image_frame.end(); frame_i++)
        {
        
            if(frame_i->second.is_key_frame)
            {
        
                kv++;
                //Vs为优化得到的速度
                Vs[kv] = frame_i->second.R * x.segment<3>(kv * 3);
            }
        }
    
        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;
        }
    
        //通过将重力旋转到z轴上，得到世界坐标系与摄像机坐标系c0之间的旋转矩阵rot_diff
        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 * Rs[0].transpose();
        Matrix3d rot_diff = R0;
        //所有变量从参考坐标系c0旋转到世界坐标系w
        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];
        }
    
        ROS_DEBUG_STREAM("g0     " << g.transpose());
        ROS_DEBUG_STREAM("my R0  " << Utility::R2ypr(Rs[0]).transpose()); 
    
        return true;
    }

P s P\_s Ps:世界坐标系下机体的平移量 p w b \\mathbf p\_\{w\}^\{b\} pwb;  
 R s R\_s Rs:世界坐标系下机体的旋转量 q w b \\mathbf q\_\{w\}^\{b\} qwb;  
 V s V\_s Vs:世界坐标系机体的速度量 v w b \\mathbf v\_\{w\}^\{b\} vwb;

此函数中坐标系变换：

1.   R c l , c 0 , R c l , c 1 , … , R c l , c W Z − 1 , R c l , c W Z R\_\{c\_\{l\}, c\_\{0\}\}, R\_\{c\_\{l\}, c\_\{1\}\}, \\ldots, R\_\{c\_\{l\}, c\_\{WZ-1\}\}, R\_\{c\_\{l\}, c\_\{WZ\}\} Rcl,c0,Rcl,c1,…,Rcl,cWZ−1,Rcl,cWZ P c l , c 0 , P c l , c 1 , … , P c l , c W Z − 1 , P c l , c W Z P\_\{c\_\{l\}, c\_\{0\}\}, P\_\{c\_\{l\}, c\_1\}, \\ldots, P\_\{c\_\{l\}, c\_\{WZ-1\}\}, P\_\{c\_\{l\}, c WZ\} Pcl,c0,Pcl,c1,…,Pcl,cWZ−1,Pcl,cWZ
2.   R b l , b 0 , R b l , b 1 , … , R b l , b W Z − 1 , R b l , b W Z R\_\{b\_\{l\}, b\_\{0\}\}, R\_\{b\_\{l\}, b\_\{1\}\}, \\ldots, R\_\{b\_\{l\}, b\_\{WZ-1\}\}, R\_\{b\_\{l\}, b\_\{WZ\}\} Rbl,b0,Rbl,b1,…,Rbl,bWZ−1,Rbl,bWZ P b l , b 0 , P b l , b 1 , … , P b l , b W Z − 1 , P b l , b W Z P\_\{b\_\{l\}, b\_\{0\}\}, P\_\{b\_\{l\}, b\_1\}, \\ldots, P\_\{b\_\{l\}, b\_\{WZ-1\}\}, P\_\{b\_\{l\}, b WZ\} Pbl,b0,Pbl,b1,…,Pbl,bWZ−1,Pbl,bWZ
3.   R w l , w 0 , R w l , w 1 , … , R w l , w W Z − 1 , R w l , w W Z R\_\{w\_\{l\}, w\_\{0\}\}, R\_\{w\_\{l\}, w\_\{1\}\}, \\ldots, R\_\{w\_\{l\}, w\_\{WZ-1\}\}, R\_\{w\_\{l\}, w\_\{WZ\}\} Rwl,w0,Rwl,w1,…,Rwl,wWZ−1,Rwl,wWZ P w l , w 0 , P w l , w 1 , … , P w l , w W Z − 1 , P w l , w W Z P\_\{w\_\{l\}, w\_\{0\}\}, P\_\{w\_\{l\}, w\_1\}, \\ldots, P\_\{w\_\{l\}, w\_\{WZ-1\}\}, P\_\{w\_\{l\}, w WZ\} Pwl,w0,Pwl,w1,…,Pwl,wWZ−1,Pwl,wWZ  
    这里为真的世界坐标系了。

[Link 1]: https://blog.csdn.net/qq_43343688/article/details/106471750#commentBox
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ZiXzk0MTIxOQ_size_16_color_FFFFFF_t_70_pic_center]: /images/20230208/e0c21df177144133a6356adc4ed86504.png