【SLAM】VINS-MONO解析——初始化(理论部分)

6.初始化

第一个问题,为什么要初始化?
对于单目系统而言,
(1)视觉系统只能获得二维信息,损失了一维信息(深度),所以需要动一下,也就是三角化才能重新获得损失的深度信息;
(2)但是,这个三角化恢复的深度信息,是个“伪深度”,它的尺度是随机的,不是真实的,所以就需要IMU来标定这个尺度;
(3)要想让IMU标定这个尺度,IMU也需要动一下,得到PVQ的P;
(4)另外,IMU存在bias,视觉获得的旋转矩阵不存在bias,所以可以用视觉来标定IMU的旋转bias;
(5)需要获得世界坐标系这个先验信息,通过初始化能借助g来确定;

对于双目系统而言,
初始化的难度会低一些,因为双目可以一下确定深度,只需要通过g的方向(先验)来确定世界坐标系。

注意,这个初始化只进行一次就够了,或者是在系统重启时执行一次,大部分时候,系统都处于NON_LINEAR的状态。因为,初始化的时候,就能确定尺度scaler和bias初始值,scaler确定后,在初始化获得的这些路标点都是准的了,后续通过PnP或者BA得到的特征点都是真实尺度的了。而bias初始值确定以后,在后续的非线性优化过程中,会实时更新。

6.1 基础原理

初始化的逻辑图如下:
在这里插入图片描述

6.1.1 如果旋转外参数 qbc 未知, 则先估计旋转外参数
实际上讲,这部分并不是vins代码中初始化的内容,而是初始化之前就需要完成的判断和操作,见5.2-4。根据IMU和视觉部分的旋转关系,可以得到下面的关系式:
在这里插入图片描述 在这里插入图片描述在这里插入图片描述将多个帧之间的等式关系一起构建超定方程Ax=0。对A进行svd分解,其中最小奇异值对应的奇异向量便为需要求解的qbc。
虽然这里IMU的旋转部分并没有标定,得到的外参数可能不太准。但是问题不大,因为初始化所占用的总运行时间不长,而更长生命周期的后端会持续的优化这部分的值。

6.1.2 利用SfM确定各个pose和特征点的相对于c0帧的位置关系
这一部分和基于图像的三维重建比较像,可以用三角化和PnP把这一串的ck帧的位姿和特征点位置确定下来(特征点是伪深度),在加上外参数qbc和pbc,一系列bk帧的位姿也确定下来。注意,这里把c0帧作为基础帧,实际上,c0帧旋转一下使gc0和gw方向一致时获得的坐标系就是vins的世界坐标系,也就是先验。在vins代码中,这一部分篇幅很长,但是这一部分也是很重要的部分。
在这里插入图片描述
首先我们先推导论文式(14),所有帧的位姿(Rc0ck,qc0ck)表示相对于第一帧相机坐标系(·)c0。相机到IMU的外参为(Rbc,qbc),得到姿态从相机坐标系转换到IMU坐标系的关系。
在这里插入图片描述
将T展开有成R与p有:
在这里插入图片描述
左侧矩阵的两项写开:
在这里插入图片描述
在这里插入图片描述

6.1.3 利用相机旋转约束标定IMU角速度bias
求解的目标函数如下公式所示:

在这里插入图片描述
在SfM完成且外参数标定完之后,头两个值是已知的了,而且我们假设头两个值是准的。理想状态下,这三个数乘积应该是单位四元数,很可惜,第三个值是IMU预积分得到的,而预积分里面是有bias的。所以,通过最小化这个目标函数的,可以把旋转bias标定出来!
在IMU预积分部分,也就是4.1.1最后的那个公式,有:
在这里插入图片描述
带入到损失函数里,可以得到:
在这里插入图片描述
或者是:
在这里插入图片描述
带入bias的残差后,得到,
在这里插入图片描述
实部没有需要标定的量,所以只用考虑虚部,也就是:
在这里插入图片描述
两侧再乘以 ,可以构造出Ax=B的形式,在采用LDLT分解,就可以求出状态量:
在这里插入图片描述
其实这里不像那种用高斯牛顿法迭求解,而更像是用直接法,也就是矩阵运算的方式来求待优化的状态量,6.1.4也是一样的思路。代具体代码见:initial_aligment.cpp 函数 solveGyroscopeBias()。

接下来是代码解析,
(1)参数的传入和容器的定义

void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)
{
    Matrix3d A;
    Vector3d b;
    Vector3d delta_bg;
    A.setZero();
    b.setZero();
    map<double, ImageFrame>::iterator frame_i;
    map<double, ImageFrame>::iterator frame_j;
    

上式中,A和b对应的是Ax=b。注意,传入的参数是all_image_frame,不仅仅是滑窗内的帧。frame_i和frame_j分别读取all_image_frame中的相邻两帧。

(2)套本小节的最后一个公式,构造Ax=b等式

    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)
    {
        frame_j = next(frame_i);
        MatrixXd tmp_A(3, 3);
        tmp_A.setZero();
        VectorXd tmp_b(3);
        tmp_b.setZero();
        Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
        tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
        tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
        A += tmp_A.transpose() * tmp_A;
        b += tmp_A.transpose() * tmp_b;
    }


注意到本小节的第一个公式了吗,那里有求和,所以需要遍历all_image_frame,然后叠加A和b。

(3)ldlt分解

    delta_bg = A.ldlt().solve(b);

(4)给滑窗内的IMU预积分加入角速度bias

    for (int i = 0; i <= WINDOW_SIZE; i++)
        Bgs[i] += delta_bg;

(5)重新计算所有帧的IMU积分(重要!)

    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++)
    {
        frame_j = next(frame_i);
        frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);
    }
}

repropagate()部分内容见4.3.2.。

6.1.4 利用IMU的平移估计重力/各bk帧速度/尺度scaler
首先要明确需要优化的状态量是什么,是各帧在bk坐标系下的速度,c0帧下的g和SfM的尺度scaler:
在这里插入图片描述
这块有个遗留问题,就是为什么要优化速度呢?
在IMU预积分部分,已经有如下的公式:
在这里插入图片描述
但是w坐标系我们不知道,只知道c0坐标系,所以需要把上面的公式转到c0坐标系上:
在这里插入图片描述
上式中,等号左边减去等号右边就是残差,理想状态下,这个残差是0,那么带入上式得:
在这里插入图片描述
把这个 XX = 0的等式也转为 A x = b这种线性方程组的形式,如下式:
在这里插入图片描述
或者矩阵的形式:
在这里插入图片描述
对于beta而言,也是类似的,

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
那么,把这两个矩阵合体,就能得到论文中的等式:
在这里插入图片描述
同样采用LDLT分解,就能求出状态量:
在这里插入图片描述
接下来是代码解析,

(1)参数的传入和容器的定义

bool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{
    int all_frame_count = all_image_frame.size();
    int n_state = all_frame_count * 3 + 3 + 1; //需要优化的状态量的个数

    MatrixXd A{n_state, n_state};
    A.setZero();
    VectorXd b{n_state};
b.setZero();

    map<double, ImageFrame>::iterator frame_i;
    map<double, ImageFrame>::iterator frame_j;


上式中,A和b对应的是Ax=b。注意,传入的参数是all_image_frame,不仅仅是滑窗内的帧。
frame_i和frame_j分别读取all_image_frame中的相邻两帧。

(2)套本小节的最后一个公式,构造Ax=b等式

    int i = 0;
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++)
    {
        frame_j = next(frame_i);

        MatrixXd tmp_A(6, 10);
        tmp_A.setZero();
        VectorXd tmp_b(6);
        tmp_b.setZero();

        double dt = frame_j->second.pre_integration->sum_dt;

        tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
        tmp_A.block<3, 3>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity();
        tmp_A.block<3, 1>(0, 9) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;    //这里除以100,会导致x偏大100 
        tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0];
        tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();
        tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;
        tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity();
        tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v;

        Matrix<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero();
        cov_inv.setIdentity();

        MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
        VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;

        A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();
        b.segment<6>(i * 3) += r_b.head<6>();

        A.bottomRightCorner<4, 4>() += r_A.bottomRightCorner<4, 4>();
        b.tail<4>() += r_b.tail<4>();

        A.block<6, 4>(i * 3, n_state - 4) += r_A.topRightCorner<6, 4>();
        A.block<4, 6>(n_state - 4, i * 3) += r_A.bottomLeftCorner<4, 6>();
    }


另外需要注意的地方是,**虽然公式中只计算了相邻2帧,但是代码中,它把所有帧都放进去了,因此,A矩阵和b向量是远大于公式中的尺寸,而且在代码中有一个叠加的操作!非线性优化里构造H矩阵和J矩阵也是一样的。**这里是我看代码前比较疑惑的地方。

(3)ldlt分解,得到尺度和g的初始值,并用先验判断

x = A.ldlt().solve(b);

    double s = x(n_state - 1) / 100.0; //把x偏大的100矫正回来
    g = x.segment<3>(n_state - 4);
    
    if(fabs(g.norm() - G.norm()) > 1.0 || s < 0)
    {//如果重力加速度与参考值差太大或者尺度为负则说明计算错误
        return false;
    }


(4) 利用gw的模长已知这个先验条件进一步优化gc0

    RefineGravity(all_image_frame, g, x);
    s = (x.tail<1>())(0) / 100.0;
    (x.tail<1>())(0) = s;
    ROS_DEBUG_STREAM(" refine     " << g.norm() << " " << g.transpose());
    if(s < 0.0 )
        return false;   
    else
        return true;
}

见下一节。虽说是优化gc0,实际上都优化了。

6.1.5 利用gw的模长已知这个先验条件进一步优化gc0
但是,这里有一个bug,就是把g当成3自由度的向量优化,但是实际上g只有2自由度,因为它的模长是已知的。所以,在这里,需要想一个办法,就是怎么样用一个2自由度的表达式来表示3维的向量呢
在这里插入图片描述
在这里插入图片描述
在这里,采用球面坐标进行参数化,也就是用g的模长作为半径画一个半球,上图蓝色线对应的是gc0的测量值的方向(也就是优化前的方向),在这个交点上找到一个切平面,用gc0,b1,b2构造一个坐标系,那么在轴b1和b2上坐标值w1和w2就是我们需要求的量。求出来之后,把等号右边加在一起,就是优化后的gc0值。
注意,b1的方向是由gc0的测量值的方向与[1,0,0]作叉乘得到的,b2的方向是由gc0的测量值的方向与b1作叉乘得到的。

这样,6.1.4公式得到了更新,此时带优化量减少了1维:
在这里插入图片描述
即 ,同样使用LDLT分解:
在这里插入图片描述
其实,在这里,我们可以发现这一部分做的工作和6.1.4是相似的,但是却是在6.1.4的基础上进一步做的工作。

接下来是代码解析,
(1)参数的传入和容器的定义

void RefineGravity(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{
    Vector3d g0 = g.normalized() * G.norm();
    Vector3d lx, ly;
    //VectorXd x;
    int all_frame_count = all_image_frame.size();
    int n_state = all_frame_count * 3 + 2 + 1;

    MatrixXd A{n_state, n_state};
    A.setZero();
    VectorXd b{n_state};
    b.setZero();

    map<double, ImageFrame>::iterator frame_i;
    map<double, ImageFrame>::iterator frame_j;


上式中,A和b对应的是Ax=b。注意,传入的参数是all_image_frame,不仅仅是滑窗内的帧。
frame_i和frame_j分别读取all_image_frame中的相邻两帧。

(2)一共迭代四次求解,并构建且向空间

    for(int k = 0; k < 4; k++)
    {
        MatrixXd lxly(3, 2);
        lxly = TangentBasis(g0);
        ....(3)
    }  

切向空间的构建如下代码所示:

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;
}


(3)套本小节的最后一个公式,构造Ax=b等式

int i = 0;
        for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++)
        {
            frame_j = next(frame_i);

            MatrixXd tmp_A(6, 9);
            tmp_A.setZero();
            VectorXd tmp_b(6);
            tmp_b.setZero();

            double dt = frame_j->second.pre_integration->sum_dt;

            tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
            tmp_A.block<3, 2>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity() * lxly;
            tmp_A.block<3, 1>(0, 8) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;     
            tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0] - frame_i->second.R.transpose() * dt * dt / 2 * g0;

            tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();
            tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;
            tmp_A.block<3, 2>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity() * lxly;
            tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v - frame_i->second.R.transpose() * dt * Matrix3d::Identity() * g0;

            Matrix<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero();
            //cov.block<6, 6>(0, 0) = IMU_cov[i + 1];
            //MatrixXd cov_inv = cov.inverse();
            cov_inv.setIdentity();

            MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
            VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;

            A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();
            b.segment<6>(i * 3) += r_b.head<6>();

            A.bottomRightCorner<3, 3>() += r_A.bottomRightCorner<3, 3>();
            b.tail<3>() += r_b.tail<3>();

            A.block<6, 3>(i * 3, n_state - 3) += r_A.topRightCorner<6, 3>();
            A.block<3, 6>(n_state - 3, i * 3) += r_A.bottomLeftCorner<3, 6>();
        }


(4)ldlt分解,得到优化后的状态量x

            A = A * 1000.0;
            b = b * 1000.0;
            x = A.ldlt().solve(b);
            VectorXd dg = x.segment<2>(n_state - 3);
            g0 = (g0 + lxly * dg).normalized() * G.norm();
            //double s = x(n_state - 1);
    }   
    g = g0;
}

6.1.6 利用gc0和gw确定世界坐标系
(1) 找到 c0 到 w 系的旋转矩阵 Rwc0 = exp([θu])
在这里插入图片描述
gc0已经求出来了,而gw一直就是一个已知的量,因此它们之间的夹角θ是能很快求出来的;
所以我们然后可以用gc0和gw作叉乘得到一个旋转轴u;
最后把c0坐标系,绕着转轴旋转一个θ,就能找到c0 到 w 系的对齐关系,也就是Rwc0 = exp([θu])。

(2) 把所有 c0 坐标系下的变量旋转到 w系下
所有量都乘上Rwc0就可以了。我们定义的c0 与 w 系的原点坐标是重合的。

(3) 把相机平移和特征点尺度恢复到米制单位
初始化大功告成!

VINS-Mono是一种用于六自由度状态估计的系统,它结合了单目摄像头和低成本IMU的数据来进行状态估计。它的初始化过程具有鲁棒性,通过对IMU预积分值和相机观测进行紧耦合的非线性优化,以获得更准确的结果。此外,VINS-Mono还使用紧耦合优化和回环检测模块,以实现重定位和全局一致的地图。它还通过姿态图优化来合并当前地图和之前保存的地图。VINS-Mono已经在公开数据集和真实场景中进行了测试,并与其他先进算法进行了比较,同时也在无人机和手机上进行了应用测试。总的来说,VINS-Mono是一个可靠、完整和通用的系统,适用于高精度定位的应用。\[3\] 关于VINS-Mono的使用经验,一些用户反馈初始化需要设备先运动一段时间,以激励IMU的数据。这对于需要系统一启动就要执行任务的设备来说可能有一定限制。此外,VINS-Mono对设备的标定要求较高,如果标定不好,轨迹可能会出现飞飘的情况。\[2\] #### 引用[.reference_title] - *1* [安装并运行VINS-Mono](https://blog.csdn.net/learning_tortosie/article/details/83182258)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra":{"utm_source":"vip_chatgpt_common_search_pc_result","utm_medium":"distribute.pc_search_result.none-task-cask-2~all~insert_cask~default-1-null.142^v91^insert_down28v1,239^v3^insert_chatgpt"}} ] [.reference_item] - *2* [【SLAMVINS-MONO解析——综述](https://blog.csdn.net/iwanderu/article/details/104617829)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra":{"utm_source":"vip_chatgpt_common_search_pc_result","utm_medium":"distribute.pc_search_result.none-task-cask-2~all~insert_cask~default-1-null.142^v91^insert_down28v1,239^v3^insert_chatgpt"}} ] [.reference_item] - *3* [超详细:VINS-Mono论文中文记录](https://blog.csdn.net/hu_hao/article/details/120257735)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra":{"utm_source":"vip_chatgpt_common_search_pc_result","utm_medium":"distribute.pc_search_result.none-task-cask-2~all~insert_cask~default-1-null.142^v91^insert_down28v1,239^v3^insert_chatgpt"}} ] [.reference_item] [ .reference_list ]
评论 9
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值