ORB-SLAM2修改：前后帧特征匹配情况可视化

前言

orb-slam2作为经典的视觉SLAM 算法，网上资料非常多，但最近需要修改orb-slam2实现一个简单的功能，要求能够实现显示前后帧特征点匹配情况的功能，我在中文互联网上并没有找到有帮助的博客，这里将我的修改思路分享给大家供参考。下面链接是我改过的orb-slam2和关键代码文件。

资料分享：https://pan.baidu.com/s/1_bpu-QTOhmJhu3VzldfmUA?pwd=2022 提取码：2022

修改目标与背景知识

修改目标：
修改后能够实现以下功能:

  1. 显示前后两帧两张图片，标记特征点并连线可视化匹配情况
  2. 打印前一帧的特征点数量

背景知识：
需要掌握orb-slam2编译与运行，安装好对应的依赖库，这方面CSDN中有很多相关文章，这里不再赘述。

orb-slam2安装参考博客：
http://t.csdnimg.cn/k6Wgr
http://t.csdnimg.cn/lXDAG
等等等等*

我的实验环境是ubuntu18.04,ros-melodic，跑的TUM数据集。
成功运行orb-slam2后可以得到Map Viewer和Current Frame两个画面，如下图所示，我们的目标就是将Current Frame画面改成能够显示特征匹配情况的matches画面。
orb-slam2运行显示情况

源代码修改

orb-slam2是利用Pangolin库进行可视化显示的，主要是需要修改Viewer.cc和FrameDrawer.cc这两个文件。

Viewer修改

Viewer.cc对Map Viewer和Current Frame进行可视化显示，功能实现主要在void Viewer::Run()这个函数中，其中

cv::Mat im = mpFrameDrawer->DrawFrame();
cv::imshow("ORB-SLAM2: Current Frame",im);

通过这两句代码可知对图片的处理主要在DrawFrame()函数中，因为需要将一张图片改为两张图片，然后显示特征匹配情况，因此笔者在DrawFrame函数中进行两张图片的数据准备，将前一帧图片、当前帧图片、前一帧特征点、当前帧特征点、匹配情况作为DrawFrame函数的返回值，然后利用cv::drawMatches函数进行前后两帧特征点匹配情况的显示。关于利用cv::drawMatches函数进行匹配情况显示这一部分参考高博SLAM十四讲中第七讲。
所以，将上述两句代码改为：

auto results = mpFrameDrawer->DrawFrame();
std::vector<cv::KeyPoint> keypoints_1, keypoints_2;
std::vector<cv::DMatch> matches;
cv::Mat img_1 =  std::get < 0 > (results);
cv::Mat img_2 = std::get < 1 > (results);
keypoints_1 = std::get <2 > (results);
keypoints_2 = std::get < 3 > (results);
matches =  std::get < 4 > (results);
cv::Mat img_match;
cv::drawMatches(img_1, keypoints_1, img_2, keypoints_2, matches, img_match);
cv::imshow("ORB-SLAM2: matches",img_match);

同时，这个文件窗口创建部分代码也需要修改：

//cv::namedWindow("ORB-SLAM2: Current Frame");//修改前
cv::namedWindow("ORB-SLAM2: matches");//修改后

FrameDrawer修改

下一步定位到FrameDrawer.cc文件中，进行DrawFrame()函数的修改。这里主要需要修改两部分，一个是修改DrawFrame()函数使之返回前后两帧图片的图像、特征点以及它们的匹配情况，一个是修改Update(Tracking *pTracker)函数获得前一帧数据与两帧匹配情况。

  1. DrawFrame()函数修改
    此处主要是需要参考当前帧处理方式对前一帧进行处理得到地图点，在图像中绘制好地图点，然后组织好数据进行返回。需要注意的是，前后两帧匹配情况要求的返回值是cv::DMatch格式,而orb-slam2内部匹配函数返回的是数组，需要进行转换。
std::tuple<cv::Mat, cv::Mat, std::vector<cv::KeyPoint>, std::vector<cv::KeyPoint>,  vector<cv::DMatch> > FrameDrawer::DrawFrame()
{
    cv::Mat im,im1,im2;
    vector<cv::KeyPoint> vIniKeys,lastKeys; // Initialization: KeyPoints in reference frame
    vector<int> vMatches; // Initialization: correspondeces with reference keypoints
    vector<cv::KeyPoint> vCurrentKeys; // KeyPoints in current frame
    vector<bool> vbVO, vbMap; // Tracked MapPoints in current frame
    vector<bool> vbVO1, vbMap1; // Tracked MapPoints in reference frame
    int state; // Tracking state
    vector<cv::DMatch>  matchess;
    vector<cv::KeyPoint> nkeys1;
    vector<cv::KeyPoint> nkeys2;
    //Copy variables within scoped mutex
    {
        unique_lock<mutex> lock(mMutex);
        state=mState;
        if(mState==Tracking::SYSTEM_NOT_READY)
            mState=Tracking::NO_IMAGES_YET;
        mIm.copyTo(im);
        mIm.copyTo(im2);
        mIm_r.copyTo(im1);
        if(mState==Tracking::NOT_INITIALIZED)
        {
            vCurrentKeys = mvCurrentKeys;
            vIniKeys = mvIniKeys;
            vMatches = mvIniMatches;
             lastKeys=mvlastkeys;
        }
        else if(mState==Tracking::OK)
        {
            vCurrentKeys = mvCurrentKeys;
            vbVO = mvbVO;
            vbMap = mvbMap;
            vbVO1 = mvbVO1;
            vbMap1 = mvbMap1;
            lastKeys=mvlastkeys;
        }
        else if(mState==Tracking::LOST)
        {
            vCurrentKeys = mvCurrentKeys;
             lastKeys=mvlastkeys;
        }
    } // destroy scoped mutex -> release mutex

    if(im.channels()<3) //this should be always true
       { cvtColor(im,im,CV_GRAY2BGR);
        cvtColor(im1,im1,CV_GRAY2BGR);
        cvtColor(im2,im2,CV_GRAY2BGR);
        }

    //Draw
    if(state==Tracking::NOT_INITIALIZED) //INITIALIZING
    {
        for(unsigned int i=0; i<vMatches.size(); i++)
        {
            if(vMatches[i]>=0)
            {
                cv::line(im,vIniKeys[i].pt,vCurrentKeys[vMatches[i]].pt,
                       cv::Scalar(0,255,0));
            }
        }        
    }
    else if(state==Tracking::OK) //TRACKING
    {
        mnTracked=0;
        mnTrackedVO=0;
        const float r = 5;
        const int n = vCurrentKeys.size();
        
        for(int i=0;i<n;i++)
        {
            if(vbVO[i] || vbMap[i])
            {
                cv::Point2f pt1,pt2;
                pt1.x=vCurrentKeys[i].pt.x-r;
                pt1.y=vCurrentKeys[i].pt.y-r;
                pt2.x=vCurrentKeys[i].pt.x+r;
                pt2.y=vCurrentKeys[i].pt.y+r;

                // This is a match to a MapPoint in the map
                if(vbMap[i])
                {
                    cv::rectangle(im2,pt1,pt2,cv::Scalar(0,255,0));
                   	cv::circle(im2,vCurrentKeys[i].pt,2,cv::Scalar(0,255,0),-1);
                    mnTracked++;
                }
                else // This is match to a "visual odometry" MapPoint created in the last frame
                {
                    cv::rectangle(im2,pt1,pt2,cv::Scalar(255,0,0));
                   	cv::circle(im2,vCurrentKeys[i].pt,2,cv::Scalar(255,0,0),-1);
                    mnTrackedVO++;
                }
            }
        }
        const int n1 =  lastKeys.size();
        for(int i=0;i<n1;i++)
        {
             if(vbVO1[i] || vbMap1[i])
            {
                cv::Point2f pt1,pt2;
                pt1.x=lastKeys[i].pt.x-r;
                pt1.y=lastKeys[i].pt.y-r;
                pt2.x=lastKeys[i].pt.x+r;
                pt2.y=lastKeys[i].pt.y+r;
                
             if(vbMap1[i])
                {
                    cv::rectangle(im1,pt1,pt2,cv::Scalar(0,255,0));
                   	cv::circle(im1,lastKeys[i].pt,2,cv::Scalar(0,255,0),-1);
                }
                else // This is match to a "visual odometry" MapPoint created in the last frame
                {
                   cv::rectangle(im1,pt1,pt2,cv::Scalar(255,0,0));
                   cv::circle(im1,lastKeys[i].pt,2,cv::Scalar(255,0,0),-1);
                }
            }
        }
    }
    //匹配情况格式转换
    for(size_t i1=0, iend=vMatches12.size(); i1<iend;i1++)
    {
            int i2 = vMatches12[i1];
            if (i2<0)
            {
                nkeys1.push_back(lastKeys[i1]);
                continue;
            }
            cv::DMatch mm;
            mm.queryIdx = nkeys1.size();
            mm.trainIdx = i2;
            matchess.push_back(mm);
            nkeys1.push_back(lastKeys[i1]);
            nkeys2.push_back(vCurrentKeys[i2]);
    }
    return std::make_tuple(im1,im2,lastKeys,vCurrentKeys,matchess);
}
  1. Update(Tracking *pTracker)函数修改
    利用orb-slam2内部的匹配函数**matcher.SearchForInitialization()**进行前后帧匹配，并且最后不断将当前帧数据保存成前一帧以便下次更新数据。这里有个问题就是又一次的特征匹配浪费了时间，因此整个修改后的代码用于检查不同场景特征点匹配情况还可以但是实际运行还是改回原来的好一些。
void FrameDrawer::Update(Tracking *pTracker)
{
    unique_lock<mutex> lock(mMutex);
    mIm.copyTo(mIm_r);
    pTracker->mImGray.copyTo(mIm);

    mvCurrentKeys=pTracker->mCurrentFrame.mvKeys;
    mvlastkeys=store_lastframe.mvKeys;
    N = mvCurrentKeys.size();
    N1 = mvlastkeys.size();
    mvbVO = vector<bool>(N,false);
    mvbMap = vector<bool>(N,false);
    mvbVO1 = vector<bool>(N1,false);
    mvbMap1 = vector<bool>(N1,false);
    mbOnlyTracking = pTracker->mbOnlyTracking;


    if(pTracker->mLastProcessedState==Tracking::NOT_INITIALIZED)
    {
        mvIniKeys=pTracker->mInitialFrame.mvKeys;
        mvIniMatches=pTracker->mvIniMatches;
    }
    else if(pTracker->mLastProcessedState==Tracking::OK)
    {
        for(int i=0;i<N1;i++)
        {
            MapPoint* pMP1 = store_lastframe.mvpMapPoints[i];
            if(pMP1)
            {
                if(!store_lastframe.mvbOutlier[i])
                {
                    if(pMP1->Observations()>0)
                        mvbMap1[i]=true;
                    else
                        mvbVO1[i]=true;
                }
            }
        }

        for(int i=0;i<N;i++)
        {
            MapPoint* pMP = pTracker->mCurrentFrame.mvpMapPoints[i];
            if(pMP)
            {
                if(!pTracker->mCurrentFrame.mvbOutlier[i])
                {
                    if(pMP->Observations()>0)
                        mvbMap[i]=true;
                    else
                        mvbVO[i]=true;
                }
            }
        }    
    }
    // Find correspondences
    ORBmatcher matcher(0.6,false);
    vPrevMatched.resize(store_lastframe.mvKeysUn.size());
    for(size_t i=0; i<store_lastframe.mvKeysUn.size(); i++)
        vPrevMatched[i]=store_lastframe.mvKeysUn[i].pt;
    int nmatches = matcher.SearchForInitialization(store_lastframe,pTracker->mCurrentFrame,vPrevMatched,vMatches12,100);
    std::cout <<"points: "<<nmatches << std::endl;
    mState=static_cast<int>(pTracker->mLastProcessedState);
    store_lastframe=pTracker->mCurrentFrame;
    
}

最后FrameDrawer.h也需要修改并添加一些变量：

class FrameDrawer
{
public:
    FrameDrawer(Map* pMap);

    // Update info from the last processed frame.
    void Update(Tracking *pTracker);

    // Draw last processed frame.
    std::tuple<cv::Mat, cv::Mat, std::vector<cv::KeyPoint>, std::vector<cv::KeyPoint>,   vector<cv::DMatch> >  DrawFrame();
protected:
    void DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText);
    
    // Info of the frame to be drawn
    cv::Mat mIm;
    cv::Mat mIm_r;
    int N,N1;
    vector<cv::KeyPoint> mvCurrentKeys,mvlastkeys;
    vector<bool> mvbMap, mvbVO;
     vector<bool> mvbMap1, mvbVO1;
    Frame store_lastframe;
    bool mbOnlyTracking;
    int mnTracked, mnTrackedVO;
    vector<cv::KeyPoint> mvIniKeys;
    vector<int> mvIniMatches;
    int mState;
    std::vector<cv::Point2f> vPrevMatched;
    std::vector<int> vMatches12;

    Map* mpMap;

    std::mutex mMutex;
};

运行结果

左图为前一帧，右图为当前帧。
运行结果
可能还有些小BUG，欢迎大家批评指正。

# c++ # 人工智能 # 计算机视觉
Logo
永洪数据分析社区

永洪科技，致力于打造全球领先的数据技术厂商，具备从数据应用方案咨询、BI、AIGC智能分析、数字孪生、数据资产、数据治理、数据实施的端到端大数据价值服务能力。

更多推荐

cover

Archery 系统配置 SQL优化审核平台配置【详解】

avatar 永洪数据分析社区
cover

举个栗子！Tableau 技巧（249）：为文本表中的不同度量设置不同颜色

avatar 永洪数据分析社区
cover

毕业设计：基于python渔业数据可视化分析系统 Flask框架 海洋捕捞品类 全国渔业 渔业加工（源码+文档）✅

avatar 永洪数据分析社区