开发: C++知识库 Java知识库 JavaScript Python PHP知识库人工智能区块链大数据移动开发嵌入式开发工具数据结构与算法开发测试游戏开发网络协议系统运维
教程: HTML教程 CSS教程 JavaScript教程 Go语言教程 JQuery教程 VUE教程 VUE3教程 Bootstrap教程 SQL数据库教程 C语言教程 C++教程 Java教程 Python教程 Python3教程 C#教程
数码: 电脑笔记本显卡显示器固态硬盘硬盘耳机手机 iphone vivo oppo 小米华为单反装机图拉丁

-> C++知识库 -> Learn_VIORB与ORB_SALM2的更改部分 -> 正文阅读

[C++知识库]Learn_VIORB与ORB_SALM2的更改部分

本帖根据大佬的文章加入自己的部分笔记和理解！！
大佬的原文链接：https://blog.csdn.net/qq_24509229/article/details/83038651。

限制于时间和精力，本部分不涉及头文件的更改，但是注意，但凡在.cc文件中新增加函数了的，就要记得在相应的.h文件中增加该函数的声明。

一.Converter.cc

后者在前者的基础上添加了

void Converter::updateNS(NavState& ns, const IMUPreintegrator& imupreint, const Vector3d& gw)
{}
 
cv::Mat Converter::toCvMatInverse(const cv::Mat &Tcw)
{}

二.Frame.cc

后者在前者的第一个Frame::Frame(const Frame &frame) 析构函数中添加了

    mvIMUDataSinceLastFrame = frame.mvIMUDataSinceLastFrame;
    mNavState = frame.GetNavState();
    mMargCovInv = frame.mMargCovInv;
    mNavStatePrior = frame.mNavStatePrior;

除此之外，还在开头添加了下列函数

void Frame::ComputeIMUPreIntSinceLastFrame(const Frame* pLastF, IMUPreintegrator& IMUPreInt) const
{}
void Frame::UpdatePoseFromNS(const cv::Mat &Tbc)
{}
void Frame::UpdateNavState(const IMUPreintegrator& imupreint, const Vector3d& gw)
{}
const NavState& Frame::GetNavState(void) const
{}
void Frame::SetInitialNavStateAndBias(const NavState& ns)
{}
void Frame::SetNavStateBiasGyr(const Vector3d &bg)
{}
void Frame::SetNavStateBiasAcc(const Vector3d &ba)
{}
void Frame::SetNavState(const NavState& ns)
{}
Frame::Frame(const cv::Mat &imGray, const double &timeStamp, const std::vector<IMUData> &vimu, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, KeyFrame*pLastKF):mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast<ORBextractor*>(NULL)),mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth)
{}

三.System.cc中

添加了以下函数

    bool bLocalMapAcceptKF(void);
    void SaveKeyFrameTrajectoryNavState(const string& filename);
     cv::Mat TrackMonoVI(const cv::Mat &im, const std::vector<IMUData> &vimu, const double &timestamp);

四.KeyFrame.cc中

后者修改了

**void KeyFrame::SetBadFlag() **


在SetBadFlag() 函数中添加了俩处：
  1.开头部分
     // Test log
    if(mbBad)
    {
        vector<KeyFrame*> vKFinMap =mpMap->GetAllKeyFrames();
        std::set<KeyFrame*> KFinMap(vKFinMap.begin(),vKFinMap.end());
        if(KFinMap.count(this))
        {
            cerr<<"this bad KF is still in map?"<<endl;
            mpMap->EraseKeyFrame(this);
        }
        mpKeyFrameDB->erase(this);
        cerr<<"KeyFrame "<<mnId<<" is already bad. Set bad return"<<endl;
        return;
    }
     
 2.结尾部分
     // Update Prev/Next KeyFrame for prev/next
    KeyFrame* pPrevKF = GetPrevKeyFrame();
    KeyFrame* pNextKF = GetNextKeyFrame();
    if(pPrevKF)
        pPrevKF->SetNextKeyFrame(pNextKF);
    if(pNextKF)
        pNextKF->SetPrevKeyFrame(pPrevKF);
    SetPrevKeyFrame(NULL);
    SetNextKeyFrame(NULL);
    // TODO: this happend once. Log: Current id = 1.
    // Test log.
    if(!pPrevKF) cerr<<"It's culling the first KF? pPrevKF=NULL. Current id: "<<mnId<<endl;
    if(!pNextKF) cerr<<"It's culling the latest KF? pNextKF=NULL. Current id: "<<mnId<<endl;
    // TODO
    if(pPrevKF && pNextKF)
    {
        if(pPrevKF->isBad()) cerr<<"Prev KF isbad in setbad. previd: "<<pPrevKF->mnId<<", current id"<<mnId<<endl;
        if(pNextKF->isBad()) cerr<<"Next KF isbad in setbad. previd: "<<pNextKF->mnId<<", current id"<<mnId<<endl;

        //Debug log, compare the bias of culled KF and the replaced one
        //cout<<"culled KF bg/ba: "<<mNavState.Get_BiasGyr().transpose()<<", "<<mNavState.Get_BiasAcc().transpose()<<endl;
        //cout<<"next KF bg/ba: "<<pNextKF->GetNavState().Get_BiasGyr().transpose()<<", "<<pNextKF->GetNavState().Get_BiasAcc().transpose()<<endl;

        // Update IMUData for NextKF
        pNextKF->AppendIMUDataToFront(this);
        // Re-compute pre-integrator
        pNextKF->ComputePreInt();
    }

2 将 map<KeyFrame,size_t>* 换成mapMapPointObs 类型,

因此需要在ORB工程中所有涉及到map<KeyFrame*,size_t>变量类型的代码都变成 mapMapPointObs类型的（好麻烦，吐了）。

例如： void KeyFrame::UpdateConnections() 函数中：

  VIO：
  mapMapPointObs/*map<KeyFrame*,size_t>*/ observations = pMP->GetObservations();
for(mapMapPointObs/*map<KeyFrame*,size_t>*/::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
原ORB：
        mapMapPointObs/*map<KeyFrame*,size_t>*/ observations = pMP->GetObservations();
        map<KeyFrame*,size_t> observations = pMP->GetObservations();

（构造函数？？）**KeyFrame::KeyFrame(Frame &F, Map pMap, KeyFrameDatabase pKFDB) 在函数头中增加了关于IMU的变量，新的函数头是

KeyFrame::KeyFrame(Frame &F, Map pMap, KeyFrameDatabase pKFDB, std::vector vIMUData, KeyFrame* pPrevKF)

函数内部添加了

     mvIMUData = vIMUData;

    //SetNavState(F.GetNavState());

    if(pPrevKF)
    {
        pPrevKF->SetNextKeyFrame(this);
    }
    mpPrevKeyFrame = pPrevKF;
    mpNextKeyFrame = NULL;

作者保留了原来的构造函数KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB) 内部添加了提醒输出的标志shouldn’t call this KeyFrame()

    cerr<<"shouldn't call this KeyFrame()"<<endl;
 
    mpPrevKeyFrame = NULL;
    mpNextKeyFrame = NULL;

还在.cc文件中添加了以下函数

void KeyFrame::UpdateNavStatePVRFromTcw(const cv::Mat &Tcw,const cv::Mat &Tbc)
{}
void KeyFrame::SetInitialNavStateAndBias(const NavState& ns)
{}
KeyFrame* KeyFrame::GetPrevKeyFrame(void)
{}
KeyFrame* KeyFrame::GetNextKeyFrame(void)
{}
void KeyFrame::SetPrevKeyFrame(KeyFrame* pKF)
{}
void KeyFrame::SetNextKeyFrame(KeyFrame* pKF)
{}
std::vector<IMUData> KeyFrame::GetVectorIMUData(void)
{}
void KeyFrame::AppendIMUDataToFront(KeyFrame* pPrevKF)
{}
void KeyFrame::UpdatePoseFromNS(const cv::Mat &Tbc)
{}
void KeyFrame::UpdateNavState(const IMUPreintegrator& imupreint, const Vector3d& gw)
{}
void KeyFrame::SetNavState(const NavState& ns)
{}
const NavState& KeyFrame::GetNavState(void)
{}
void KeyFrame::SetNavStateBiasGyr(const Vector3d &bg)
{}
void KeyFrame::SetNavStateBiasAcc(const Vector3d &ba)
{}
void KeyFrame::SetNavStateVel(const Vector3d &vel)
{}
void KeyFrame::SetNavStatePos(const Vector3d &pos)
{}
void KeyFrame::SetNavStateRot(const Matrix3d &rot)
{}
void KeyFrame::SetNavStateRot(const Sophus::SO3 &rot)
{}
void KeyFrame::SetNavStateDeltaBg(const Vector3d &dbg)
{}
void KeyFrame::SetNavStateDeltaBa(const Vector3d &dba)
{}
const IMUPreintegrator & KeyFrame::GetIMUPreInt(void)
{}
void KeyFrame::ComputePreInt(void)
{}

五.LocalMapping.cc

后者修改了

void LocalMapping::ResetIfRequested()

       添加了：
        mlLocalKeyFrames.clear();
 
        // Add resetting init flags
        mbVINSInited = false;
        mbFirstTry = true;

void LocalMapping::KeyFrameCulling() 不同位置中添加了

1.        if(ConfigParam::GetRealTimeFlag())
    {
        if(GetFlagCopyInitKFs())
            return;
    }
    SetFlagCopyInitKFs(true);
    2.
  KeyFrame* pOldestLocalKF = mlLocalKeyFrames.front();
    KeyFrame* pPrevLocalKF = pOldestLocalKF->GetPrevKeyFrame();
    KeyFrame* pNewestLocalKF = mlLocalKeyFrames.back();
    // Test log
    if(pOldestLocalKF->isBad()) cerr<<"pOldestLocalKF is bad, check 1. id: "<<pOldestLocalKF->mnId<<endl;
    if(pPrevLocalKF) if(pPrevLocalKF->isBad()) cerr<<"pPrevLocalKF is bad, check 1. id: "<<pPrevLocalKF->mnId<<endl;
    if(pNewestLocalKF->isBad()) cerr<<"pNewestLocalKF is bad, check 1. id: "<<pNewestLocalKF->mnId<<endl;
3.
// Don't cull the oldest KF in LocalWindow,
        // And the KF before this KF
        if(pKF == pOldestLocalKF || pKF == pPrevLocalKF)
            continue;
 
        // Check time between Prev/Next Keyframe, if larger than 0.5s(for local)/3s(others), don't cull
        // Note, the KF just out of Local is similarly considered as Local
        KeyFrame* pPrevKF = pKF->GetPrevKeyFrame();
        KeyFrame* pNextKF = pKF->GetNextKeyFrame();
        if(pPrevKF && pNextKF && !GetVINSInited())
        {
            if(fabs(pNextKF->mTimeStamp - pPrevKF->mTimeStamp) > /*0.2*/0.5)
                continue;
        }
        // Don't drop the KF before current KF
        if(pKF->GetNextKeyFrame() == mpCurrentKeyFrame)
            continue;
        if(pKF->mTimeStamp >= mpCurrentKeyFrame->mTimeStamp - 0.11)
            continue;
 
        if(pPrevKF && pNextKF)
        {
            double timegap=0.51;
            if(GetVINSInited() && pKF->mTimeStamp < mpCurrentKeyFrame->mTimeStamp - 4.0)
                timegap = 3.01;
 
            if(fabs(pNextKF->mTimeStamp - pPrevKF->mTimeStamp) > timegap)
                continue;
        }

void LocalMapping::ProcessNewKeyFrame() 中末尾添加了

  // Delete bad KF in LocalWindow
    DeleteBadInLocalWindow();
    // Add Keyframe to LocalWindow
    AddToLocalWindow(mpCurrentKeyFrame);

在namespace ORB_SLAM2空间添加了

class KeyFrameInit
{};
bool LocalMapping::GetUpdatingInitPoses(void)
{}
void LocalMapping::SetUpdatingInitPoses(bool flag)
{}
KeyFrame* LocalMapping::GetMapUpdateKF()
{}
bool LocalMapping::GetMapUpdateFlagForTracking()
{}
void LocalMapping::SetMapUpdateFlagInTracking(bool bflag)
{}
bool LocalMapping::GetVINSInited(void)
{}
void LocalMapping::SetVINSInited(bool flag)
{}
bool LocalMapping::GetFirstVINSInited(void)
{}
void LocalMapping::SetFirstVINSInited(bool flag)
{}
cv::Mat LocalMapping::GetGravityVec()
{}
cv::Mat LocalMapping::GetRwiInit()
{}
void LocalMapping::VINSInitThread()
{}
bool LocalMapping::TryInitVIO(void)
{}
void LocalMapping::AddToLocalWindow(KeyFrame* pKF)
{}
void LocalMapping::DeleteBadInLocalWindow(void)
{}

在LocalMapping::LocalMapping析构函数里添加了一些变量

 mpParams = pParams;
    mnLocalWindowSize = ConfigParam::GetLocalWindowSize();
    cout<<"mnLocalWindowSize:"<<mnLocalWindowSize<<endl;
 
    mbVINSInited = false;
    mbFirstTry = true;
    mbFirstVINSInited = false;
 
    mbUpdatingInitPoses = false;
    mbCopyInitKFs = false;
    mbInitGBAFinish = false;

六.Map.cc

VIORB删除了

void Map::InformNewBigChange()
{
    unique_lock<mutex> lock(mMutexMap);
    mnBigChangeIdx++;
}
 
int Map::GetLastBigChangeIdx()
{
    unique_lock<mutex> lock(mMutexMap);
    return mnBigChangeIdx;
}

增加了

bool KFIdComapre::operator ()(const KeyFrame* kfleft,const KeyFrame* kfright) const
{}
void Map::UpdateScale(const double &scale)
{}

七.MapPoint.cc

后者添加了

bool cmpKeyFrameId::operator() (const KeyFrame* a, const KeyFrame* b) const {
}
void MapPoint::UpdateScale(float scale){
}

并对void MapPoint::EraseObservation(KeyFrame* pKF) 添加了

       KeyFrame* pKFrefnew = NULL;
            for(auto ob : mObservations)
            {
                KeyFrame* pkfi = ob.first;
                if(!pkfi->isBad())
                {
                    pKFrefnew = pkfi;
                    break;
                }
            }

八.Optimizer.cc

后者很明显的添加了以下的头文件，说明IMU信息就是用来做优化

#include "IMU/configparam.h"
#include "IMU/g2otypes.h"
#include "Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h"
#include "Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_cholmod.h"

.cc文件中新增加了很多函数


    void static LocalBAPRVIDP(KeyFrame *pKF, const std::list<KeyFrame*> &lLocalKeyFrames, bool* pbStopFlag, Map* pMap, cv::Mat& gw, LocalMapping* pLM=NULL);

    void static GlobalBundleAdjustmentNavStatePRV(Map* pMap, const cv::Mat& gw, int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust);
    void static LocalBundleAdjustmentNavStatePRV(KeyFrame *pKF, const std::list<KeyFrame*> &lLocalKeyFrames, bool* pbStopFlag, Map* pMap, cv::Mat& gw, LocalMapping* pLM=NULL);

    void static GlobalBundleAdjustmentNavState(Map* pMap, const cv::Mat& gw, int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust);

    int static PoseOptimization(Frame *pFrame, KeyFrame* pLastKF, const IMUPreintegrator& imupreint, const cv::Mat& gw, const bool& bComputeMarg=false);
    int static PoseOptimization(Frame *pFrame, Frame* pLastFrame, const IMUPreintegrator& imupreint, const cv::Mat& gw, const bool& bComputeMarg=false);

    void static LocalBundleAdjustmentNavState(KeyFrame *pKF, const std::list<KeyFrame*> &lLocalKeyFrames, bool* pbStopFlag, Map* pMap, cv::Mat& gw, LocalMapping* pLM=NULL);

    Vector3d static OptimizeInitialGyroBias(const std::list<KeyFrame*> &lLocalKeyFrames);
    Vector3d static OptimizeInitialGyroBias(const std::vector<KeyFrame*> &vLocalKeyFrames);
    Vector3d static OptimizeInitialGyroBias(const std::vector<Frame> &vFrames);
    Vector3d static OptimizeInitialGyroBias(const vector<cv::Mat>& vTwc, const vector<IMUPreintegrator>& vImuPreInt);

    void static LocalBundleAdjustment(KeyFrame *pKF, const std::list<KeyFrame*> &lLocalKeyFrames, bool* pbStopFlag, Map* pMap, LocalMapping* pLM=NULL);

九.未修改部分

FrameDrawer.cc中，总体改动不大，后者只添加了几个时间值 mStartTime，mCurTime

Initializer.cc没做修改
KeyFrameDatabase.cc没做修改

MapDrawer.cc没做修改

ORBextractor.cc没做修改

ORBmatcher.cc没做修改

PnPsolver.cc没做修改

Sim3Solver.cc没做修改

十.Tracking.cc

在void Tracking::CreateInitialMapMonocular()
函数中添加了

    // The first imu package include 2 parts for KF1 and KF2
    vector<IMUData> vimu1,vimu2;
    for(size_t i=0; i<mvIMUSinceLastKF.size(); i++)
    {
        IMUData imu = mvIMUSinceLastKF[i];
        if(imu._t < mInitialFrame.mTimeStamp)
            vimu1.push_back(imu);
        else
            vimu2.push_back(imu);
    }

在cv::Mat Tracking::GrabImageMonocular添加

      mvIMUSinceLastKF.insert(mvIMUSinceLastKF.end(), vimu.begin(),vimu.end());

新增函数有


void Tracking::RecomputeIMUBiasAndCurrentNavstate(NavState& nscur)
{}

bool Tracking::TrackLocalMapWithIMU(bool bMapUpdated)
{}

void Tracking::PredictNavStateByIMU(bool bMapUpdated)
{}

bool Tracking::TrackWithIMU(bool bMapUpdated)
{}


IMUPreintegrator Tracking::GetIMUPreIntSinceLastKF(Frame* pCurF, KeyFrame* pLastKF, const std::vector<IMUData>& vIMUSInceLastKF)
{}

IMUPreintegrator Tracking::GetIMUPreIntSinceLastFrame(Frame* pCurF, Frame* pLastF)
{}


cv::Mat Tracking::GrabImageMonoVI(const cv::Mat &im, const std::vector<IMUData> &vimu, const double &timestamp)
{}

//---------------------------------

在Tracking()的构造函数中添加了变量,ConfigParam* pParams

十一。Loopclosing.cc

namespace中添加

bool LoopClosing::GetMapUpdateFlagForTracking()
{
    unique_lock<mutex> lock(mMutexMapUpdateFlag);
    return mbMapUpdateFlagForTracking;
}

void LoopClosing::SetMapUpdateFlagInTracking(bool bflag)
{
    unique_lock<mutex> lock(mMutexMapUpdateFlag);
    mbMapUpdateFlagForTracking = bflag;
}