vins-mono坐标系处理和初始化

最新推荐文章于 2024-06-11 23:58:29 发布

slam让我头疼

最新推荐文章于 2024-06-11 23:58:29 发布

阅读量978

点赞数

分类专栏：学习笔记

本文链接：https://blog.csdn.net/weixin_38216573/article/details/117470248

版权

学习笔记专栏收录该内容

17 篇文章 2 订阅

订阅专栏

http://www.cxyzjd.com/article/huanghaihui_123/103075107
求与a向量正交的向量：先指定一b向量，用b向量减去b向量在a向量的投影、方向与a向量相同的向量，即为与a向量垂直的B向量

在这里插入图片描述

MatrixXd TangentBasis(Vector3d &g0)
{
    Vector3d b, c;
    Vector3d a = g0.normalized();
    Vector3d tmp(0, 0, 1);
    if(a == tmp)
        tmp << 1, 0, 0;
    b = (tmp - a * (a.transpose() * tmp)).normalized();
    c = a.cross(b);
    MatrixXd bc(3, 2);
    bc.block<3, 1>(0, 0) = b;
    bc.block<3, 1>(0, 1) = c;
    return bc;
}

https://blog.csdn.net/AnChenliang_1002/article/details/113090599

vins-mono直接得到的是相对世界系的位姿

 //通过将重力旋转到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];
    }