VINS 细节系列 - 初始姿态 g2R() 问题

目录

一、相关概念

二、初始化

重力对齐

理论部分

程序部分

双目加IMU的初始化

理论部分

程序部分

---

一、相关概念

假设有IMU坐标系和相机坐标系如下：

 

IMU 和 相机有规定其本身的x,y,z轴的方向

1、imu 一般规定右前下分别为x,y,z轴

2、相机一般认为，z轴平行于镜头朝外，x轴与像平面的水平方向一致，从左到右，y轴与像平面垂直方向一致，从上到下

 

假如： 相机坐标系作为参考坐标系，解算出了 imu坐标系 在 cam坐标系 下的位姿： R ，t

t  代表了平移向量，为imu坐标系原点在相机坐标系的位置；
R 代表的含义可以有以下解释：

1. 相机坐标系如何旋转才能转到IMU坐标系 --------- 坐标系的变换
2. 表示imu坐标系中的点 通过 这个旋转矩阵 变换到相机坐标系中去 --------- 点的变换
3. imu坐标系 在 相机坐标系 的 姿态   （这个更关键，后面会说明白）

注意：坐标系的变换和点的变换是 “相反”的过程；个人感觉这个坐标系转换的概念还是要清晰的！

 

二、初始化

重力对齐

我们习惯把  第 一帧相机坐标系当作参考坐标系，因为此时的IMU测量的重力加速度还没有进行优化，没有办法与重力加速度 g 对齐，实属无奈之举！

 

理论部分

我们用第一次MU测量的加速度 g0 =[-9.4 −2.3 0.4]，这时候第一帧imu大概是这个样子，坐标系为 C0

因为程序中，这个加速度 g0 是两帧之间IMU加速度的平均值得到的，故这个IMU坐标系也可以看作第一帧相机坐标系；

假设我们要和理想IMU坐标系重力  g = [0 0 1] 对齐， 参考图如下：

注意：为什么是[0 0 9.8]而不是[0 0 -9.8] ？因为vins的公式推算中，g都是用-号，因此这个相当于是一个朝上的重力

第一部：根据两个向量 g、g0 求出 矩阵R0 （这个是有对应公式的；这里没有t0是因为 他们的原点是一样的，只是为了便于区分，图才分为两个）；

R0 便是  C0坐标系在 理想坐标系下的姿态；这里我们想一下，这里的R0仅仅是姿态，我们上一步根据重力对齐求出来了，但是这个姿态在

理想坐标系下还是那种 偏航角 yaw 不是0的状态 ，比如：20度；

有人就会有疑问：C0坐标系下的点  g = R0 ✖g0 , 这个不就对齐了嘛？这个....这个有点偷换概念了， 我也迷糊了一会，还是想通了（自己认为）

我们再看一下第一节中R的概念：

1. 相机坐标系如何旋转才能转到IMU坐标系 --------- 坐标系的变换
2. 表示imu坐标系中的点 通过 这个旋转矩阵 变换到相机坐标系中去 --------- 点的变换
3. imu坐标系 在 相机坐标系 的 姿态   （这个更关键，后面会说明白）

          他们是独立的表达，可不能混淆！我们用的是 姿态 这个概念，

（这个可以理解吧，可能会优点迷惑，这个是两个坐标系的相对变化，后面的C1，C2...Cn 他们之间的姿态都是我们要求的，只是会最终转换到C0参考系下）

 现在我们想把R0的偏航角yaw 在 理想坐标系中为0，怎么办？很好办呀，求出来，旋转补偿一下就行了，也是后面程序对应的内容！

假设C0绕着理想坐标系的Z轴旋转 -yaw, 对应的矩阵为R1，则  R0 = R1*R0 便是 C0坐标系在理想坐标系下 偏航角为 0的 姿态！

有人又问：为什么偏航角yaw要转换成0 ？自己想吧！

 

程序部分

/-----------------------------------------------------------------程序---------------------------------------------------------------------/

//初始第一个imu位姿
void Estimator::initFirstIMUPose(vector<pair<double, Eigen::Vector3d>> &accVector)
{
    printf("init first imu pose\n");
    initFirstPoseFlag = true;
   
    //计算加速度的均值
    Eigen::Vector3d averAcc(0, 0, 0);
    int n = (int)accVector.size();
    for(size_t i = 0; i < accVector.size(); i++)
    {
        averAcc = averAcc + accVector[i].second;
    }
    averAcc = averAcc / n;
    printf("averge acc %f %f %f\n", averAcc.x(), averAcc.y(), averAcc.z());

    Matrix3d R0 = Utility::g2R(averAcc);
    double yaw = Utility::R2ypr(R0).x();
    R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;//另初始的航向为0
    Rs[0] = R0;
    cout << "init R0 " << endl << Rs[0] << endl;
}

子程序 g2R ()

Eigen::Matrix3d Utility::g2R(const Eigen::Vector3d &g)
{
    Eigen::Matrix3d R0;
    Eigen::Vector3d ng1 = g.normalized();
    Eigen::Vector3d ng2{0, 0, 1.0};
    R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix();
    
    double yaw = Utility::R2ypr(R0).x();
    R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;
    
    return R0;
}

/-----------------------------------------------------------------程序---------------------------------------------------------------------/

得到的 R0 ​存入 滑动窗口 的世界坐标系下的旋转 Rs[0]；上面我们说的理想坐标系，也是世界坐标系！

所以Ps  Vs  是世界坐标系下的位置和速度；Rs （Rw_bk）是 bk 到 w 的转换矩阵！

自己疑问的地方：

 double yaw = Utility::R2ypr(R0).x();
 R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;

为什么出现两次？还奇怪，想了几个小时，也考到了博客出处的解释，自己有质疑！ 本人的解释如下，如有问题还请提出来！

 

双目加IMU的初始化

理论部分

对陀螺仪bias的校正

 

程序部分

/-----------------------------------------------------------------程序---------------------------------------------------------------------/

        if(STEREO && USE_IMU)
        {
            //有了深度之后就可以进行PnP求解
            f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
            
            // 双目三角化
            f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
            
            if (frame_count == WINDOW_SIZE)
            {
                map<double, ImageFrame>::iterator frame_it;
                int i = 0;
                for (frame_it = all_image_frame.begin(); frame_it != all_image_frame.end(); frame_it++)
                {
                    frame_it->second.R = Rs[i];
                    frame_it->second.T = Ps[i];
                    i++;
                }
                solveGyroscopeBias(all_image_frame, Bgs);
                for (int i = 0; i <= WINDOW_SIZE; i++)
                {
                    pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
                }
                optimization();
                updateLatestStates();     //让此时刻的值都等于上一时刻的值,用来更新状态
                solver_flag = NON_LINEAR;
                slideWindow();
                ROS_INFO("Initialization finish!");
             }
          }

/-----------------------------------------------------------------程序---------------------------------------------------------------------/