【SLAM学习笔记】4-ORB_SLAM3关键源码分析② G2oTypes（一）

最新推荐文章于 2021-11-21 22:32:29 发布

口哨糖youri

最新推荐文章于 2021-11-21 22:32:29 发布

阅读量396

点赞数

分类专栏： SLAM 文章标签：其他

本文链接：https://blog.csdn.net/qq_47891062/article/details/120817243

版权

SLAM 专栏收录该内容

13 篇文章 23 订阅

订阅专栏

2021SC@SDUSC

1.代码分析

1.1 G2oTypes.h

G2oTypes.h的主要结构如下(对于类的作用已注释）：
在这里插入图片描述

1.2 G2oTypes.cc

局部地图中imu的局部地图优化（此时已经初始化完毕不需要再优化重力方向与尺度）

EdgeInertial::EdgeInertial(IMU::Preintegrated *pInt):JRg(Converter::toMatrix3d(pInt->JRg)),
    JVg(Converter::toMatrix3d(pInt->JVg)), JPg(Converter::toMatrix3d(pInt->JPg)), JVa(Converter::toMatrix3d(pInt->JVa)),
    JPa(Converter::toMatrix3d(pInt->JPa)), mpInt(pInt), dt(pInt->dT)
{
    // 准备工作，把预积分类里面的值先取出来，包含信息的是两帧之间n多个imu信息预积分来的
    // This edge links 6 vertices
    // 6元边
    resize(6);
    // 1. 定义重力
    g << 0, 0, -IMU::GRAVITY_VALUE;
    // 2. 读取协方差矩阵的前9*9部分的逆矩阵，该部分表示的是预积分测量噪声的协方差矩阵
    cv::Mat cvInfo = pInt->C.rowRange(0, 9).colRange(0, 9).inv(cv::DECOMP_SVD);
    // 转成eigen Matrix9d
    Matrix9d Info;
    for (int r = 0; r < 9; r++)
        for (int c = 0; c < 9; c++)
            Info(r, c) = cvInfo.at<float>(r, c);
    // 3. 强制让其成为对角矩阵
    Info = (Info + Info.transpose()) / 2;
    // 4. 让特征值很小的时候置为0，再重新计算信息矩阵（暂不知这么操作的目的是什么，先搞清楚操作流程吧）
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 9, 9>> es(Info);
    Eigen::Matrix<double, 9, 1> eigs = es.eigenvalues(); // 矩阵特征值
    for (int i = 0; i < 9; i++)
        if (eigs[i] < 1e-12)
            eigs[i] = 0;
    // asDiagonal 生成对角矩阵
    Info = es.eigenvectors() * eigs.asDiagonal() * es.eigenvectors().transpose();
    setInformation(Info);
}

计算误差

void EdgeInertial::computeError()
{
    // 计算残差
    // TODO Maybe Reintegrate inertial measurments when difference between linearization point and current estimate is too big
    const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);
    const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);
    const VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[2]);
    const VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[3]);
    const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]);
    const VertexVelocity* VV2 = static_cast<const VertexVelocity*>(_vertices[5]);
    const IMU::Bias b1(VA1->estimate()[0],VA1->estimate()[1],VA1->estimate()[2],VG1->estimate()[0],VG1->estimate()[1],VG1->estimate()[2]);
    const Eigen::Matrix3d dR = Converter::toMatrix3d(mpInt->GetDeltaRotation(b1));
    const Eigen::Vector3d dV = Converter::toVector3d(mpInt->GetDeltaVelocity(b1));
    const Eigen::Vector3d dP = Converter::toVector3d(mpInt->GetDeltaPosition(b1));

    const Eigen::Vector3d er = LogSO3(dR.transpose()*VP1->estimate().Rwb.transpose()*VP2->estimate().Rwb);
    const Eigen::Vector3d ev = VP1->estimate().Rwb.transpose()*(VV2->estimate() - VV1->estimate() - g*dt) - dV;
    const Eigen::Vector3d ep = VP1->estimate().Rwb.transpose()*(VP2->estimate().twb - VP1->estimate().twb
                                                               - VV1->estimate()*dt - g*dt*dt/2) - dP;

    _error << er, ev, ep;
}