IMU和视觉融合学习笔记_imu和单目摄像头融合-优快云博客

利用纯视觉信息进行位姿估计，对运动物体、光照干扰、场景纹理缺失等情况，定位效果不够鲁棒。当下，视觉与IMU融合(VI-SLAM）逐渐成为常见的多传感器融合方式。视觉信息与IMU 数据进行融合，根据融合方式同样可分为基于滤波器和基于优化两类。按照是否把图像特征信息加入状态向量，又可以分为松融合与紧融合两类。

双目相机加入imu模块的的好处，是解决了高速运动中间数据处理的问题。相机运动过程中间出现模糊，两帧之间重叠区域太少，因此很难做特征点匹配。相机自己又能在慢速运动中间解决 imu的漂移问题，这两者是互补的。在之前研究视觉和IMU的基础上，开展IMU和视觉融合学习，并做记录。

在这里插入图片描述

一、IMU和视觉融合的方法

1、IMU

IMU以高频率(100HZ或200HZ)输出载体的角速度w和线加速度a，解算出高频率(100HZ或200HZ)的载体速度V、位置P以及旋转R

2、相机

零偏和噪声会比较大，以至于长时间使用后偏移的就很快，但是如果使用高精惯导，这个漂移误差会降低些，因为它是一种积分状态，从开始时间一直在持续积分，积分到不再使用为止，也就是它的V、P、R，他们分别每一个时刻都有一个误差，这个误差会产生迭代，所以长时间使用后就会漂移，

相机以30Hz或20Hz获得场景中的图像信息，利用图像中的特征信息，解算载体的旋转和平移
相机可以获得丰富的环境信息，并在长时间内的漂移误差较小，但在快速运动或旋转的环境中容易发生跟踪丢失的情况，且在面对挑战环境时定位精度会明显下降

3、融合的目标

融合的目标就是进行一个相互的补偿，主要有三个目标可以进行相互的补偿：

利用视觉里程计对IMU的累积漂移进行补偿，降低惯导的漂移误差
对于单目视觉传感器，可以利用IMU进行场景深度的校正，缓解单目相机尺度不确定性问题
IMU的输出与环境无关，不受环境变化的约束，利用IMU与视觉进行补偿可以提高视觉里程计位姿估计的鲁棒性

在这里插入图片描述

note：IMU加进视觉里面，并不是提高了视觉的定位精度，比如ORB-SLAM3的纯视觉定位精度是高于视觉+IMU的定位精度的，纯视觉是更高精度的里程计，过程相当于一个高精度融合低精度的，所以最后多模态的定位精度没有纯视觉的高，所以视觉+IMU提高的是系统的鲁棒性，并非提高精度

4、IMU如何和视觉传感器实现融合

（1）松耦合

松耦合是每一个传感器算出一个轨迹，对算出的结果进行再一次的融合

（2）紧耦合

紧耦合是在估计的时候，就是把IMU的状态，视觉传感器的状态等传感器的状态放在一起进行位姿估计，最后融合出来是只存在一个轨迹误差，其实不需要单独计算各个传感器的轨迹。
在这里插入图片描述

现在大多数研究使用的都是紧耦合的状态，如果系统特别大，视觉惯性SLAM所有导航模块中间的一个小分支，那么更多的用的是松耦合的情况，因为要避免单个模块失效的情况。

现在大多数系统上看到的问题都是由基于优化的，比如VINS、ORB-SLAM，都是基于优化算法的紧耦合。滤波只考虑当前的状态，相当于“得过且过”，想要得到当前的状态，只需要直达搜上一个状态，是一步一步推算出来的；优化过程是长期的一个过程，想要得到当前的状态，从过去很久之前到当前的所有的一个状态变化

以前的情况是基于优化的算法计算量比较大，基于滤波的计算量稍小一些，但是发展中，基于滤波的种方法也不仅仅值依靠前一个状态，也涉及了一个滑动窗模型，而基于优化的方法，使用稀疏边缘化的方式，实现了计算量的降低，从而减负，优化算法的实时性还是不错的

5、IMU如何优化纯视觉SLAM

（1）单目尺度优化

单目尺度可以理解为比例尺，计算视觉的轨迹需要特征的匹配，特征匹配就是每两个图像帧之间非常快速的运动，纯视觉开始运动的时候比较依赖前几帧选择出来的尺度，慢慢进行迭代。

遇到大场景的情况下，运动速度非常的剧烈，这个时候尺度是非常的不准的，这个时候引入IMU，IMU预积分的过程中是可以解算出PRV，IMU单独的模块可以解算出载体的平移旋转和速度，IMU解算出来的平移不是靠特征匹配的方式得到的，和场景是没有关系的，所以IMU在短时间内效果还是不错的，长时间就很很容易漂移（比如说100Hz，也就是0.01秒之内，对平移来说还是相对而言比较准确的）。

载体上是固连的，相机和IMU两个模块，那么我们就可以通过这两个模块，一个是已知的、计算的还算比较准确的平移，一个是通过图像匹配估计出来的平移，这样就可以计算出来这两个之间的一个尺度，
在这里插入图片描述

尺度的大小主要与相机的平移运动有关，IMU可以解算出PRV，因此可以利用IMU估计得到的平移优化纯视觉的平移尺度

（2）视觉惯性联合优化

当我们视觉的信息在模糊的情况下，整个的联合优化的过程，可以保证轨迹估计继续进行，IMU预积分会存在一些估计的误差，而视觉存在投影误差，PnP、3D到3D投影误差，2D到3D的投影误差，把这两个同时放到一个优化方程里面，我们想要缩小所有的残差，进一步的到当前更好的当前状态X，相当于实现了扩大考虑的因素，

在这里插入图片描述
如果是一个纯视觉的里程计，那就是C这一部分，

IMU的话就是加了一个B，用了一个边缘化的操作。
在这里插入图片描述

二、IMU和视觉的初始化及参数估计方法

1、VI初始化需要初始化那些参数？

（1）视觉

确定载体的初始位姿、初始关键帧以及初始空间点的位置

保留在算法中的始终都是关键帧，确定载体的初始位姿为母关键帧，第一个关键帧设置为i，当前的位姿为单位阵，设置为初始关键，此刻确定的是初始关键帧所对应的初始空间点的位置确定好着三个量之后，完成的视觉初始化

（2）IMU

估计IMU的偏置、异构传感器的时延、重力方向对准

IMU的偏置分为两个：陀螺仪的偏置、加速度计的偏置
现在的研究大部分没有考虑到异构传感器的很长的时延

（3）VI

传感器旋转平移外参、单目尺度因子

传感器旋转平移外参就是所谓的标定

标定算法现在还是再出很多好论文，标定所发也就是这视觉、IMU和VI这三方面的内容，也可以从这几方面入手，写一下自己的标定算法

2、IMU标定

（1）陀螺仪

$\tilde{ω}_{m} 是测量量；{ω}_{t} 是真值；{b}_{ω} 是零偏；$

${n}_{ω}是高斯白噪声，出场预设的，不会去拿来标定$
在这里插入图片描述

（2）加速度计测得的线加速度

$\tilde{a _{ω} }是测得的值，R_W^B ( a_{wt}-a_{wg}+ b_{a}+n_{a} )是真值，$
$加速度计测的是重力方向的加速度，需要把世界坐标系转化到 B 系，$
$后面是{b}_{a} 是零偏； {n}_{a}是高斯白噪声$
在这里插入图片描述

3、ORB-SLAM3的IMU初始化

先进行视觉初始化，然后IMU状态变量是和Map紧密关联的，而这在一定程度上也就导致了IMU和Map的耦合，不易分开

void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
{
    if (mbResetRequested)
        return;

    float minTime;
    int nMinKF;
    if (mbMonocular)
    {
        minTime = 2.0;
        nMinKF = 10;
    }
    else
    {
        minTime = 1.0;
        nMinKF = 10;
    }


    if(mpAtlas->KeyFramesInMap()<nMinKF)
        return;

    // Retrieve all keyframe in temporal order
    list<KeyFrame*> lpKF;
    KeyFrame* pKF = mpCurrentKeyFrame;
    while(pKF->mPrevKF)
    {
        lpKF.push_front(pKF);
        pKF = pKF->mPrevKF;
    }
    lpKF.push_front(pKF);
    vector<KeyFrame*> vpKF(lpKF.begin(),lpKF.end());

    if(vpKF.size()<nMinKF)
        return;

    mFirstTs=vpKF.front()->mTimeStamp;
    if(mpCurrentKeyFrame->mTimeStamp-mFirstTs<minTime)
        return;

    bInitializing = true;

    while(CheckNewKeyFrames())
    {
        ProcessNewKeyFrame();
        vpKF.push_back(mpCurrentKeyFrame);
        lpKF.push_back(mpCurrentKeyFrame);
    }

    const int N = vpKF.size();
    IMU::Bias b(0,0,0,0,0,0);

    // Compute and KF velocities mRwg estimation
    if (!mpCurrentKeyFrame->GetMap()->isImuInitialized())
    {
        Eigen::Matrix3f Rwg;
        Eigen::Vector3f dirG;
        dirG.setZero();
        for(vector<KeyFrame*>::iterator itKF = vpKF.begin(); itKF!=vpKF.end(); itKF++)
        {
            if (!(*itKF)->mpImuPreintegrated)
                continue;
            if (!(*itKF)->mPrevKF)
                continue;

            dirG -= (*itKF)->mPrevKF->GetImuRotation() * (*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity();
            Eigen::Vector3f _vel = ((*itKF)->GetImuPosition() - (*itKF)->mPrevKF->GetImuPosition())/(*itKF)->mpImuPreintegrated->dT;
            (*itKF)->SetVelocity(_vel);
            (*itKF)->mPrevKF->SetVelocity(_vel);
        }

        dirG = dirG/dirG.norm();
        Eigen::Vector3f gI(0.0f, 0.0f, -1.0f);
        Eigen::Vector3f v = gI.cross(dirG);
        const float nv = v.norm();
        const float cosg = gI.dot(dirG);
        const float ang = acos(cosg);
        Eigen::Vector3f vzg = v*ang/nv;
        Rwg = Sophus::SO3f::exp(vzg).matrix();
        mRwg = Rwg.cast<double>();
        mTinit = mpCurrentKeyFrame->mTimeStamp-mFirstTs;
    }
    else
    {
        mRwg = Eigen::Matrix3d::Identity();
        mbg = mpCurrentKeyFrame->GetGyroBias().cast<double>();
        mba = mpCurrentKeyFrame->GetAccBias().cast<double>();
    }

    mScale=1.0;

    mInitTime = mpTracker->mLastFrame.mTimeStamp-vpKF.front()->mTimeStamp;

    std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
    Optimizer::InertialOptimization(mpAtlas->GetCurrentMap(), mRwg, mScale, mbg, mba, mbMonocular, infoInertial, false, false, priorG, priorA);

    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();

    if (mScale<1e-1)
    {
        cout << "scale too small" << endl;
        bInitializing=false;
        return;
    }

    // Before this line we are not changing the map
    {
        unique_lock<mutex> lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate);
        if ((fabs(mScale - 1.f) > 0.00001) || !mbMonocular) {
            Sophus::SE3f Twg(mRwg.cast<float>().transpose(), Eigen::Vector3f::Zero());
            mpAtlas->GetCurrentMap()->ApplyScaledRotation(Twg, mScale, true);
            mpTracker->UpdateFrameIMU(mScale, vpKF[0]->GetImuBias(), mpCurrentKeyFrame);
        }

        // Check if initialization OK
        if (!mpAtlas->isImuInitialized())
            for (int i = 0; i < N; i++) {
                KeyFrame *pKF2 = vpKF[i];
                pKF2->bImu = true;
            }
    }

    mpTracker->UpdateFrameIMU(1.0,vpKF[0]->GetImuBias(),mpCurrentKeyFrame);
    if (!mpAtlas->isImuInitialized())
    {
        mpAtlas->SetImuInitialized();
        mpTracker->t0IMU = mpTracker->mCurrentFrame.mTimeStamp;
        mpCurrentKeyFrame->bImu = true;
    }

    std::chrono::steady_clock::time_point t4 = std::chrono::steady_clock::now();
    if (bFIBA)
    {
        if (priorA!=0.f)
            Optimizer::FullInertialBA(mpAtlas->GetCurrentMap(), 100, false, mpCurrentKeyFrame->mnId, NULL, true, priorG, priorA);
        else
            Optimizer::FullInertialBA(mpAtlas->GetCurrentMap(), 100, false, mpCurrentKeyFrame->mnId, NULL, false);
    }

    std::chrono::steady_clock::time_point t5 = std::chrono::steady_clock::now();

    Verbose::PrintMess("Global Bundle Adjustment finished\nUpdating map ...", Verbose::VERBOSITY_NORMAL);

    // Get Map Mutex
    unique_lock<mutex> lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate);

    unsigned long GBAid = mpCurrentKeyFrame->mnId;

    // Process keyframes in the queue
    while(CheckNewKeyFrames())
    {
        ProcessNewKeyFrame();
        vpKF.push_back(mpCurrentKeyFrame);
        lpKF.push_back(mpCurrentKeyFrame);
    }

    // Correct keyframes starting at map first keyframe
    list<KeyFrame*> lpKFtoCheck(mpAtlas->GetCurrentMap()->mvpKeyFrameOrigins.begin(),mpAtlas->GetCurrentMap()->mvpKeyFrameOrigins.end());

    while(!lpKFtoCheck.empty())
    {
        KeyFrame* pKF = lpKFtoCheck.front();
        const set<KeyFrame*> sChilds = pKF->GetChilds();
        Sophus::SE3f Twc = pKF->GetPoseInverse();
        for(set<KeyFrame*>::const_iterator sit=sChilds.begin();sit!=sChilds.end();sit++)
        {
            KeyFrame* pChild = *sit;
            if(!pChild || pChild->isBad())
                continue;

            if(pChild->mnBAGlobalForKF!=GBAid)
            {
                Sophus::SE3f Tchildc = pChild->GetPose() * Twc;
                pChild->mTcwGBA = Tchildc * pKF->mTcwGBA;

                Sophus::SO3f Rcor = pChild->mTcwGBA.so3().inverse() * pChild->GetPose().so3();
                if(pChild->isVelocitySet()){
                    pChild->mVwbGBA = Rcor * pChild->GetVelocity();
                }
                else {
                    Verbose::PrintMess("Child velocity empty!! ", Verbose::VERBOSITY_NORMAL);
                }

                pChild->mBiasGBA = pChild->GetImuBias();
                pChild->mnBAGlobalForKF = GBAid;

            }
            lpKFtoCheck.push_back(pChild);
        }

        pKF->mTcwBefGBA = pKF->GetPose();
        pKF->SetPose(pKF->mTcwGBA);

        if(pKF->bImu)
        {
            pKF->mVwbBefGBA = pKF->GetVelocity();
            pKF->SetVelocity(pKF->mVwbGBA);
            pKF->SetNewBias(pKF->mBiasGBA);
        } else {
            cout << "KF " << pKF->mnId << " not set to inertial!! \n";
        }

        lpKFtoCheck.pop_front();
    }

    // Correct MapPoints
    const vector<MapPoint*> vpMPs = mpAtlas->GetCurrentMap()->GetAllMapPoints();

    for(size_t i=0; i<vpMPs.size(); i++)
    {
        MapPoint* pMP = vpMPs[i];

        if(pMP->isBad())
            continue;

        if(pMP->mnBAGlobalForKF==GBAid)
        {
            // If optimized by Global BA, just update
            pMP->SetWorldPos(pMP->mPosGBA);
        }
        else
        {
            // Update according to the correction of its reference keyframe
            KeyFrame* pRefKF = pMP->GetReferenceKeyFrame();

            if(pRefKF->mnBAGlobalForKF!=GBAid)
                continue;

            // Map to non-corrected camera
            Eigen::Vector3f Xc = pRefKF->mTcwBefGBA * pMP->GetWorldPos();

            // Backproject using corrected camera
            pMP->SetWorldPos(pRefKF->GetPoseInverse() * Xc);
        }
    }

    Verbose::PrintMess("Map updated!", Verbose::VERBOSITY_NORMAL);

    mnKFs=vpKF.size();
    mIdxInit++;

    for(list<KeyFrame*>::iterator lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++)
    {
        (*lit)->SetBadFlag();
        delete *lit;
    }
    mlNewKeyFrames.clear();

    mpTracker->mState=Tracking::OK;
    bInitializing = false;

    mpCurrentKeyFrame->GetMap()->IncreaseChangeIndex();

    return;
}