IMU和视觉融合学习笔记

这篇具有很好参考价值的文章主要介绍了IMU和视觉融合学习笔记。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

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

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

IMU和视觉融合学习笔记,SLAM,视觉SLAM,imu相机,imu,SLAM

一、IMU和视觉融合的方法

1、IMU

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

2、相机

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

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

3、融合的目标

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

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

IMU和视觉融合学习笔记,SLAM,视觉SLAM,imu相机,imu,SLAM

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

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

(1)松耦合

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

(2)紧耦合

紧耦合是在估计的时候,就是把IMU的状态,视觉传感器的状态等传感器的状态放在一起进行位姿估计,最后融合出来是只存在一个轨迹误差,其实不需要单独计算各个传感器的轨迹。
IMU和视觉融合学习笔记,SLAM,视觉SLAM,imu相机,imu,SLAM
IMU和视觉融合学习笔记,SLAM,视觉SLAM,imu相机,imu,SLAM
现在大多数研究使用的都是紧耦合的状态,如果系统特别大,视觉惯性SLAM所有导航模块中间的一个小分支,那么更多的用的是松耦合的情况,因为要避免单个模块失效的情况。

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

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

5、IMU如何优化纯视觉SLAM

(1)单目尺度优化

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

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

载体上是固连的,相机和IMU两个模块,那么我们就可以通过这两个模块,一个是已知的、计算的还算比较准确的平移,一个是通过图像匹配估计出来的平移,这样就可以计算出来这两个之间的一个尺度,
IMU和视觉融合学习笔记,SLAM,视觉SLAM,imu相机,imu,SLAM

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

(2)视觉惯性联合优化

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

IMU和视觉融合学习笔记,SLAM,视觉SLAM,imu相机,imu,SLAM
如果是一个纯视觉的里程计,那就是C这一部分,
IMU和视觉融合学习笔记,SLAM,视觉SLAM,imu相机,imu,SLAM

IMU的话就是加了一个B,用了一个边缘化的操作。
IMU和视觉融合学习笔记,SLAM,视觉SLAM,imu相机,imu,SLAM

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

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

(1)视觉

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

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

(2)IMU

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

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

(3)VI

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

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

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

2、IMU标定

(1)陀螺仪

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

n ω 是高斯白噪声,出场预设的,不会去拿来标定 {n}_{ω}是高斯白噪声,出场预设的,不会去拿来标定 nω是高斯白噪声,出场预设的,不会去拿来标定
IMU和视觉融合学习笔记,SLAM,视觉SLAM,imu相机,imu,SLAM

(2)加速度计测得的线加速度

a ω ~ 是测得的值, R W B ( a w t − a w g + b a + n a ) 是真值, \tilde{a _{ω} }是测得的值,R_W^B ( a_{wt}-a_{wg}+ b_{a}+n_{a} )是真值, aω~是测得的值,RWB(awtawg+ba+na)是真值,
加速度计测的是重力方向的加速度,需要把世界坐标系转化到 B 系, 加速度计测的是重力方向的加速度,需要把世界坐标系转化到B系, 加速度计测的是重力方向的加速度,需要把世界坐标系转化到B系,
后面是 b a 是零偏; n a 是高斯白噪声 后面是{b}_{a} 是零偏; {n}_{a}是高斯白噪声 后面是ba是零偏;na是高斯白噪声
IMU和视觉融合学习笔记,SLAM,视觉SLAM,imu相机,imu,SLAM

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

三、总结

双目相机加入imu模块的的好处,是解决了高速运动中间数据处理的问题。相机运动过程中间出现模糊,两帧之间重叠区域太少,因此很难做特征点匹配。相机自己又能在慢速运动中间解决 imu的漂移问题,这两者是互补的。文章来源地址https://www.toymoban.com/news/detail-606614.html

到了这里,关于IMU和视觉融合学习笔记的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 使用kaliber与imu_utils进行IMU、相机+IMU联合标定

    目录 1 标定工具编译 1.1 IMU标定工具 imu_utils 1.2 相机标定工具 kaliber 2 标定数据录制 3 开始标定 3.1 IMU标定 3.2 相机标定 3.3 相机+IMU联合标定 4 将参数填入ORBSLAM的文件中         标定IMU我们使用imu_utils软件进行标定:         首先我们安装标定软件的依赖项:Eigen、Ceres

    2024年02月07日
    浏览(40)
  • ORB_SLAM3:单目+IMU初始化流程梳理

    单目+IMU模式下,前面的一些配置完成后,处理第一帧图像时: 1、每帧图像都会调用该函数: 目的:使灰度直方图分布较为均匀,从而使整体对比度更强,便于后面特征点的提取等工作; 2、第一帧图像(ni=0)时无IMU数据(vImuMeas容器为空),进入下面的这个函数: 该函数先

    2024年02月10日
    浏览(42)
  • 相机与IMU标定教程

    标定教程 way 1、 imu_utils标定IMU的内参,可以校准IMU的噪声密度和随机游走噪声 2、kalibr包标定相机的内外参数,相机与IMU之间的外参 1.1安装环境 这里使用的包是 imu_utils ,使用这个包可以校准IMU的噪声密度和随机游走噪声 step1: 安装ceres库 下载编译 ceres-solver step2: 安装 cod

    2023年04月18日
    浏览(34)
  • Cartographer算法2D激光雷达与IMU融合建图

     上一篇文章讲了cartographer算法手持雷达建图的参数调试,这篇进一步讲如何融合2D雷达与IMU采用cartographer算法进行slam建图。 cartographer算法手持二维激光雷达建图(不使用里程计及IMU) https://blog.csdn.net/wangchuchua/article/details/127268037?spm=1001.2014.3001.5502 思岚s1激光雷达、Tobotics

    2024年02月07日
    浏览(46)
  • QXRService:基于高通QXRService获取头显SLAM Pose和IMU Data

    QXRService博文: QVRService:基于SnapdragonXR-SDK 4.0.6进行QVRService的开发 QXRService:高通SnapdragonXR OpenXR SDK v1.x 概略 QXRService:基于高通QXRService获取头显SLAM Pose和IMU Data​​​​​​ QXRService:基于高通QXRService获取SLAM Camera图像 在上一篇博文的最后提到过,基于高通QXRService已经开发出了

    2023年04月20日
    浏览(34)
  • 使用kalibr对相机和IMU标定

    目录 一、IMU标定 二、相机标定 三、联合标定 关于需要下载的环境和具体的包参考【1】 记录标定过程 : ①录制imu的rosbag ②标定 单位问题 :   ①连续时间  ②离散时间 parameter symbol units gyr_n acc_n gyr_w acc_w 对于离散时间的白噪声  = 连续时间的白噪声 * 频率的平方根 对于离

    2024年02月15日
    浏览(42)
  • 如何使用imu和轮速里程计融合定位?代码怎么写

    使用IMU和轮速里程计融合定位的一般步骤如下: 将IMU和轮速里程计的数据预处理成需要的形式。 使用一种预测滤波器(例如卡尔曼滤波器或高斯滤波器)来预测机器人的位置和姿态。 使用轮速里程计的测量来校正预测的位置和姿态。 使用IMU的测量来校正预测的姿态。 下面是使

    2024年02月16日
    浏览(38)
  • d435i 相机和imu标定

    使用 imu_utils 功能包标定 IMU,由于imu_utils功能包的编译依赖于code_utils,需要先编译code_utils,主要参考 相机与IMU联合标定_熊猫飞天的博客-CSDN博客 Ubuntu20.04编译并运行imu_utils,并且标定IMU_学无止境的小龟的博客-CSDN博客 1.1 编译 code_utils 创建工作空间 1.1.1 修改 CMakeLists.txt 文件

    2024年02月09日
    浏览(54)
  • Ubuntu18.04下使用安卓手机Camera和IMU信息运行ORB-SLAM2

    1、下载Android_Camera-IMU,将其中的Camera-Imu.apk文件发送至手机端进行安装。 下载命令: git clone https://github.com/hitcm/Android_Camera-IMU.git  发送至手机的文件在手机端安装以后的软件  在手机端安装好以后的软件如下:  2、安装功能依赖包:sudo apt-get install ros-melodic-imu-tools  # 修改

    2024年02月09日
    浏览(50)
  • ubuntu20.04+kalibr_相机与imu联合标定

    本文使用的相机是 Realsense D435i ,imu是轮趣科技的 N100 。 基于 ubuntu20.04+kalibr+imu_utils 标定相机和imu以及联合标定。 安装依赖 建立工作空间 下载kalibr并编译 或者直接把setup.bash加到 ~/.bashrc –type apriltag 标定板类型 –nx [NUM_COLS] 列个数 6 –ny [NUM_ROWS] 行个数 6 –tsize [TAG_WIDTH_M]

    2024年02月03日
    浏览(41)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包