欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

VINS 细节系列 - 双目+IMU初始化

程序员文章站 2022-04-16 21:30:08
...

目录

一、相关概念

二、初始化

重力对齐

理论部分

程序部分

双目加IMU的初始化

理论部分

程序部分


一、相关概念

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

 

VINS 细节系列 - 双目+IMU初始化

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

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

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

 

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

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

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

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

 

二、初始化

重力对齐

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

 

理论部分

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

VINS 细节系列 - 双目+IMU初始化

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

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

VINS 细节系列 - 双目+IMU初始化

注意:为什么是[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];上面我们说的理想坐标系,也是世界坐标系!

自己疑问的地方:

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

为什么出现两次?

 

双目加IMU的初始化

理论部分

对陀螺仪bias的校正

VINS 细节系列 - 双目+IMU初始化

 

程序部分

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

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

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

 

 

相关标签: 视觉SLAM