Vins_mono重力对齐理解

最新推荐文章于 2024-03-11 14:48:28 发布

Sunshine_xiaodaxia

最新推荐文章于 2024-03-11 14:48:28 发布

阅读量2.3k

点赞数 3

文章标签：自动驾驶 slam

本文链接：https://blog.csdn.net/Sunshine_xiaodaxia/article/details/122205519

版权

该博客探讨了视觉惯性导航系统（VINS）中重力旋转到Z轴上的过程，以及如何利用解算的重力向量计算旋转矩阵。文章指出，通过比较实际观测的重力与理想重力，可以解算出姿态中的pitch和roll。同时，解释了R0矩阵的更新，通过补偿-yaw角度确保旋转正确。内容还涉及了坐标系之间的转换，包括IMU坐标系到世界坐标系的转换，以及在不同坐标系下的位置和速度计算。视频资源进一步阐述了补偿-yaw角度的原因。

摘要由CSDN通过智能技术生成

   // 6 通过将重力旋转到z轴上，得到世界坐标系与摄像机坐标系c0之间的旋转矩阵rot_diff
    // 此处解算得到的g是枢纽帧下的重力
    // 关于vins是4自由度的理解  四自由度应该指的是 x y z yaw  
    // yaw的不可观察性质    采集的g  与真实的g（真实的g 与z轴平行  yaw角无论如何变化  g都不会发生变化）相比较 可以直接解算出 pitch roll
    // 此处解出的R0 应该是枢纽帧坐标系到世界坐标系的旋转矩阵
    Matrix3d R0 = Utility::g2R(g);
    // Rs0  是第0帧IMU坐标系到 枢纽帧camera坐标系的旋转矩阵  R0*Rs[0]  表示第0帧body坐标系到世界坐标系的旋转矩阵
    //获得body坐标系相对于世界坐标系（第0帧body坐标系）的航向角        
    // 注意转变思维  作者认为c0（第0帧）的坐标系就是世界坐标系
    // Rs[0]   Tl_camera_b0   R0:T_world_camera
     double yaw = Utility::R2ypr(R0 * Rs[0]).x();        //
    // 前面可以看到
    R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;  //此处的R0是枢纽帧相机坐标系到世界坐标系的转换关系
    g = R0 * g;  
    //Matrix3d rot_diff = R0 * Rs[0].transpose();
    Matrix3d rot_diff = R0;
    for (int i = 0; i <= frame_count; i++)
    {
        // 那么此处的rot_diff应该是第1帧图片imu坐标系到世界坐标系下的旋转
        // ps[i]   表示的是第i帧与第0帧位置在枢纽帧相机坐标系下的 位置差
        // 仍然是相对于枢纽帧相机坐标系的变化
        Ps[i] = rot_diff * Ps[i];  //将相机坐标系下观察到第k帧 与第零帧的位置差  转换到世界坐标系下
        // Rs 最开始是第i帧imu坐标系到枢纽帧camera坐标系的相对旋转
        // 那么此处rot_diff应该是  枢纽帧camera坐标系到世界坐标系的旋转
        Rs[i] = rot_diff * Rs[i];
        // 由于vs所在的坐标系是枢纽帧的camera坐标系   所以此处我认为rot_diff表示的是camera坐标系到世界坐标系下的旋转
        Vs[i] = rot_diff * Vs[i];

        // 但是若都是这么表示  Ps[i]到底能反应什么呢？
    }