ORB-SLAM3 双目、RGBD、单目融合LK光流法进行双追踪方式纯自己怕自己忘了大家可以忽略

APS2023

于 2024-04-18 21:49:34 发布

阅读量492

点赞数 3

文章标签： opencv 人工智能计算机视觉

本文链接：https://blog.csdn.net/qq_41694024/article/details/137936496

版权

1 将LK.h与LK.cpp放入文件中

2 跟踪线程初始化LK指针

3 追踪策略

4 因为光流法不需要提取ORB特征使算法运行速度快一倍因此需要更改帧的属性

5 效果

1 将LK.h与LK.cpp放入文件中

对于ORBSLAM3 1.0版本，需要做如下修改：

void LK::getRefKepPoints3D2D()
{
    int obsPlus = 0;
    if(mpRefKeyFrame->mnId>3)
        obsPlus = 3;//who can observate the MapPoints(number)

    if(mpRefKeyFrame==NULL)
    {
        cout<<"no ref keyframe"<<endl;
        return;
    }

    //clear 
    mvRefKeyPoints2D.clear();
    mvRefKeyPoints3D.clear();
    vector<MapPoint*> tMapPoints;
    //get map points associate with Reference KeyFrame
    tMapPoints = mpRefKeyFrame->GetMapPointMatches();

    for(unsigned int i=0;i<tMapPoints.size();i++)
    {
        if(tMapPoints[i]==NULL)
        {
            continue;
        }

        if(tMapPoints[i]->Observations()>obsPlus)
        {
            //valid MapPoint
            //copy MapPoint to LK(include ref 2d , ref 3d in  ref KeyFrame)
            mvRefKeyPoints2D.push_back(mpRefKeyFrame->mvKeysUn[i].pt);
            cv::Mat tWorldPos = tMapPoints[i]->GetWorldPos();
            cv::Point3f tPoints3D;
            tPoints3D.x=tWorldPos.at<float>(0);
            tPoints3D.y=tWorldPos.at<float>(1);
            tPoints3D.z=tWorldPos.at<float>(2);
            mvRefKeyPoints3D.push_back(tPoints3D);
        }
    }
}

对于ORBSLAM3 0.4版本，需要改成如下格式：

void LK::getRefKepPoints3D2D()
{
    int obsPlus = 0;
    if(mpRefKeyFrame->mnId>3)
        obsPlus = 3;//who can observate the MapPoints(number)

    if(mpRefKeyFrame==NULL)
    {
        cout<<"no ref keyframe"<<endl;
        return;
    }

    //clear
    mvRefKeyPoints2D.clear();
    mvRefKeyPoints3D.clear();
    vector<MapPoint*> tMapPoints;
    //get map points associate with Reference KeyFrame
    tMapPoints = mpRefKeyFrame->GetMapPointMatches();

    for(unsigned int i=0;i<tMapPoints.size();i++)
    {
        if(tMapPoints[i]==NULL)
        {
            continue;
        }

        if(tMapPoints[i]->Observations()>obsPlus)
        {
            //valid MapPoint
            //copy MapPoint to LK(include ref 2d , ref 3d in  ref KeyFrame)
            mvRefKeyPoints2D.push_back(mpRefKeyFrame->mvKeysUn[i].pt);
            cv::Mat tWorldPos = tMapPoints[i]->GetWorldPos();
            cv::Point3f tPoints3D;
            tPoints3D.x=tWorldPos.at<float>(0);
            tPoints3D.y=tWorldPos.at<float>(1);
            tPoints3D.z=tWorldPos.at<float>(2);
            mvRefKeyPoints3D.push_back(tPoints3D);


        }
    }
}

这是因为1.0版本位姿是用sophus库表示，0.4版本用cv::Mat表示。

2 跟踪线程初始化LK指针

在Tracking.h中，引入LK.h头文件，并建立LK指针。

#include"Viewer.h"
#include"FrameDrawer.h"
#include"Atlas.h"
#include"LocalMapping.h"
#include"LoopClosing.h"
#include"Frame.h"
#include "ORBVocabulary.h"
#include"KeyFrameDatabase.h"
#include"ORBextractor.h"
#include "Initializer.h"
#include "MapDrawer.h"
#include "System.h"
#include "ImuTypes.h"
#include "LK.h"
#include "GeometricCamera.h"

public:
    cv::Mat mImRight;
    LK* mpLK;
    int LK_track_num;

在Tracking.cc 中，初始化指针：

    Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Atlas *pAtlas, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor, Settings* settings, const string &_nameSeq):
            mState(NO_IMAGES_YET), mSensor(sensor), mTrackedFr(0), mbStep(false),
            mbOnlyTracking(false), mbMapUpdated(false), mbVO(false), mpORBVocabulary(pVoc), mpKeyFrameDB(pKFDB),
            mbReadyToInitializate(false), mpSystem(pSys), mpViewer(NULL), bStepByStep(false),
            mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpAtlas(pAtlas), mnLastRelocFrameId(0), time_recently_lost(5.0),
            mnInitialFrameId(0), mbCreatedMap(false), mnFirstFrameId(0), mpCamera2(nullptr), mpLastKeyFrame(static_cast<KeyFrame*>(NULL))
    {
        mpLK = new LK(20);
        LK_track_num = 0;

3 追踪策略

一个新的选择关键帧的策略：

    bool Tracking::NeedNewKeyFrameMod()
    {

        if(mpAtlas->GetCurrentMap()->isImuInitialized() == false || mpAtlas->GetCurrentMap()->GetIniertialBA2() == false)
        {
            return true;
        }
        static int frameCount = 0;
        if(frameCount<3)
        {
            frameCount++;
            return true;
        }

        if((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && !mpAtlas->GetCurrentMap()->isImuInitialized())
        {
            if (mSensor == System::IMU_MONOCULAR && (mCurrentFrame.mTimeStamp-mpLastKeyFrame->mTimeStamp)>=0.25)
                return true;
            else if ((mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && (mCurrentFrame.mTimeStamp-mpLastKeyFrame->mTimeStamp)>=0.25)
                return true;
            else
                return false;
        }

        if(mbOnlyTracking)
            return false;

        if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())
        {
            return false;
        }

        const int nKFs = mpAtlas->KeyFramesInMap();

        // Do not insert keyframes if not enough frames have passed from last relocalisation
        if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && nKFs>mMaxFrames)
        {
            return false;
        }

        int nMinObs = 3;
        if(nKFs<=2)
            nMinObs=2;
        int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);

        // Local Mapping accept keyframes?
        bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();

        // Check how many "close" points are being tracked and how many could be potentially created.
        int nNonTrackedClose = 0;
        int nTrackedClose= 0;

        if(mSensor!=System::MONOCULAR && mSensor!=System::IMU_MONOCULAR)
        {
            int N = (mCurrentFrame.Nleft == -1) ? mCurrentFrame.N : mCurrentFrame.Nleft;
            for(int i =0; i<N; i++)
            {
                if(mCurrentFrame.mvDepth[i]>0 && mCurrentFrame.mvDepth[i]<mThDepth)
                {
                    if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
                        nTrackedClose++;
                    else
                        nNonTrackedClose++;

                }
            }
        }

        bool bNeedToInsertClose;
        bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);

        // Thresholds
        float thRefRatio = 0.75f;
        if(nKFs<2)
            thRefRatio = 0.4f;


        if(mSensor==System::MONOCULAR)
            thRefRatio = 0.9f;

        if(mpCamera2) thRefRatio = 0.75f;

        if(mSensor==System::IMU_MONOCULAR)
        {
            if(mnMatchesInliers>350) // Points tracked from the local map
                thRefRatio = 0.75f;
            else
                thRefRatio = 0.90f;
        }

        // c1a --> 每10帧一定要ORB跟踪一次
        const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+10;
        // Condition 1b: More than "MinFrames" have passed and Local Mapping is idle
        const bool c1b = ((mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames) && bLocalMappingIdle); //mpLocalMapper->KeyframesInQueue() < 2);
        //Condition 1c: tracking is weak
        const bool c1c = mSensor!=System::MONOCULAR && mSensor!=System::IMU_MONOCULAR && mSensor!=System::IMU_STEREO && mSensor!=System::IMU_RGBD && (mnMatchesInliers<nRefMatches*0.25 || bNeedToInsertClose) ;
        // Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.
        const bool c2 = (((mnMatchesInliers<nRefMatches*thRefRatio || bNeedToInsertClose)) && mnMatchesInliers>15);

        //std::cout << "NeedNewKF: c1a=" << c1a << "; c1b=" << c1b << "; c1c=" << c1c << "; c2=" << c2 << std::endl;
        // Temporal condition for Inertial cases
        bool c3 = false;
        if(mpLastKeyFrame)
        {
            if (mSensor==System::IMU_MONOCULAR)
            {
                if ((mCurrentFrame.mTimeStamp-mpLastKeyFrame->mTimeStamp)>=0.5)
                    c3 = true;
            }
            else if (mSensor==System::IMU_STEREO || mSensor == System::IMU_RGBD)
            {
                if ((mCurrentFrame.mTimeStamp-mpLastKeyFrame->mTimeStamp)>=0.5)
                    c3 = true;
            }
        }

        bool c4 = false;
        if ( mnMatchesInliers<200 ) // MODIFICATION_2, originally ((((mnMatchesInliers<75) && (mnMatchesInliers>15)) || mState==RECENTLY_LOST) && ((mSensor == System::IMU_MONOCULAR)))
            c4=true;
        else
            c4=false;

        if(c1a || ((c1b||c1c) && c2)||c3 ||c4)
        {
            // If the mapping accepts keyframes, insert keyframe.
            // Otherwise send a signal to interrupt BA
            if(bLocalMappingIdle || mpLocalMapper->IsInitializing())
            {
                return true;
            }
            else
            {
                mpLocalMapper->InterruptBA();
                if(mSensor!=System::MONOCULAR  && mSensor!=System::IMU_MONOCULAR)
                {
                    if(mpLocalMapper->KeyframesInQueue()<3)
                        return true;
                    else
                        return false;
                }
                else
                {
                    //std::cout << "NeedNewKeyFrame: localmap is busy" << std::endl;
                    return false;
                }
            }
        }
        else
            return false;
    }

在0.4版本中，需要如此修改：

  bool Tracking::NeedNewKeyFrameMod()
    {

        if(mpAtlas->GetCurrentMap()->isImuInitialized() == false || mpAtlas->GetCurrentMap()->GetIniertialBA2() == false)
        {
            return true;
        }
        static int frameCount = 0;
        if(frameCount<3)
        {
            frameCount++;
            return true;
        }

        if((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO ) && !mpAtlas->GetCurrentMap()->isImuInitialized())
        {
            if (mSensor == System::IMU_MONOCULAR && (mCurrentFrame.mTimeStamp-mpLastKeyFrame->mTimeStamp)>=0.25)
                return true;
            else if ((mSensor == System::IMU_STEREO ) && (mCurrentFrame.mTimeStamp-mpLastKeyFrame->mTimeStamp)>=0.25)
                return true;
            else
                return false;
        }

        if(mbOnlyTracking)
            return false;

        if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())
        {
            return false;
        }

        const int nKFs = mpAtlas->KeyFramesInMap();

        // Do not insert keyframes if not enough frames have passed from last relocalisation
        if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && nKFs>mMaxFrames)
        {
            return false;
        }

        int nMinObs = 3;
        if(nKFs<=2)
            nMinObs=2;
        int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);

        // Local Mapping accept keyframes?
        bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();

        // Check how many "close" points are being tracked and how many could be potentially created.
        int nNonTrackedClose = 0;
        int nTrackedClose= 0;

        if(mSensor!=System::MONOCULAR && mSensor!=System::IMU_MONOCULAR)
        {
            int N = (mCurrentFrame.Nleft == -1) ? mCurrentFrame.N : mCurrentFrame.Nleft;
            for(int i =0; i<N; i++)
            {
                if(mCurrentFrame.mvDepth[i]>0 && mCurrentFrame.mvDepth[i]<mThDepth)
                {
                    if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
                        nTrackedClose++;
                    else
                        nNonTrackedClose++;

                }
            }
        }

        bool bNeedToInsertClose;
        bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);

        // Thresholds
        float thRefRatio = 0.75f;
        if(nKFs<2)
            thRefRatio = 0.4f;


        if(mSensor==System::MONOCULAR)
            thRefRatio = 0.9f;

        if(mpCamera2) thRefRatio = 0.75f;

        if(mSensor==System::IMU_MONOCULAR)
        {
            if(mnMatchesInliers>350) // Points tracked from the local map
                thRefRatio = 0.75f;
            else
                thRefRatio = 0.90f;
        }

        // c1a --> 每10帧一定要ORB跟踪一次
        const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+10;
        // Condition 1b: More than "MinFrames" have passed and Local Mapping is idle
        const bool c1b = ((mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames) && bLocalMappingIdle); //mpLocalMapper->KeyframesInQueue() < 2);
        //Condition 1c: tracking is weak
        const bool c1c = mSensor!=System::MONOCULAR && mSensor!=System::IMU_MONOCULAR && mSensor!=System::IMU_STEREO && (mnMatchesInliers<nRefMatches*0.25 || bNeedToInsertClose) ;
        // Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.
        const bool c2 = (((mnMatchesInliers<nRefMatches*thRefRatio || bNeedToInsertClose)) && mnMatchesInliers>15);

        //std::cout << "NeedNewKF: c1a=" << c1a << "; c1b=" << c1b << "; c1c=" << c1c << "; c2=" << c2 << std::endl;
        // Temporal condition for Inertial cases
        bool c3 = false;
        if(mpLastKeyFrame)
        {
            if (mSensor==System::IMU_MONOCULAR)
            {
                if ((mCurrentFrame.mTimeStamp-mpLastKeyFrame->mTimeStamp)>=0.5)
                    c3 = true;
            }
            else if (mSensor==System::IMU_STEREO )
            {
                if ((mCurrentFrame.mTimeStamp-mpLastKeyFrame->mTimeStamp)>=0.5)
                    c3 = true;
            }
        }

        bool c4 = false;
        if ( mnMatchesInliers<200 ) // MODIFICATION_2, originally ((((mnMatchesInliers<75) && (mnMatchesInliers>15)) || mState==RECENTLY_LOST) && ((mSensor == System::IMU_MONOCULAR)))
            c4=true;
        else
            c4=false;

        if(c1a || ((c1b||c1c) && c2)||c3 ||c4)
        {
            // If the mapping accepts keyframes, insert keyframe.
            // Otherwise send a signal to interrupt BA
            if(bLocalMappingIdle || mpLocalMapper->IsInitializing())
            {
                return true;
            }
            else
            {
                mpLocalMapper->InterruptBA();
                if(mSensor!=System::MONOCULAR  && mSensor!=System::IMU_MONOCULAR)
                {
                    if(mpLocalMapper->KeyframesInQueue()<3)
                        return true;
                    else
                        return false;
                }
                else
                {
                    //std::cout << "NeedNewKeyFrame: localmap is busy" << std::endl;
                    return false;
                }
            }
        }
        else
            return false;
    }

OK！

4 因为光流法不需要提取ORB特征使算法运行速度快一倍因此需要更改帧的属性

    Frame::Frame(bool idPlus,double timeStamp)
    {
        if(idPlus)
            mnId=nNextId++;
        mTimeStamp=timeStamp;
    }

    Frame(bool idPlus,double timeStamp);

5 效果

核心代码不做公布，这里仅放效果图供客户参阅！

ORB+LK双追踪语义建图 + 剔除动态特征点。

APS2023

关注

3
点赞
踩
6

收藏

觉得还不错? 一键收藏
打赏
0
评论
ORB-SLAM3 双目、RGBD、单目融合LK光流法进行双追踪方式纯自己怕自己忘了大家可以忽略

这是因为1.0版本位姿是用sophus库表示，0.4版本用cv::Mat表示。在Tracking.h中，引入LK.h头文件，并建立LK指针。ORB+LK双追踪语义建图 + 剔除动态特征点。核心代码不做公布，这里仅放效果图供客户参阅！
复制链接

扫一扫