ORBSLAMとLearnVIORBのコードの違い

8175 ワード
slam
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 &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(NULL)),mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth)
{}

FrameDrawer.ccでは、全体的な変更は大きくなく、後者はいくつかの時間値mStartTime,mCurTimeしか追加されていません.
Initializer.ccは修正していません
KeyFlame.ccでは,後者はvoid KeyFrame::SetBadFlag(),void KeyFrame::UpdateConnections()(
        mapMapPointObs/*map*/ observations = pMP->GetObservations();

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

        for(map::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)

mapをmapMapPointObsタイプに変更)KeyFrame::KeyFrame(Frame&F,Map*pMap,KeyFrameDatabase*pKFDB)(追加
    cerr<

)には、次の関数も追加されています.
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 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()に追加しました
    KeyFrame* pOldestLocalKF = mlLocalKeyFrames.front();
    KeyFrame* pPrevLocalKF = pOldestLocalKF->GetPrevKeyFrame();
    KeyFrame* pNewestLocalKF = mlLocalKeyFrames.back();
    // Test log
    if(pOldestLocalKF->isBad()) cerr<mnId<isBad()) cerr<mnId<isBad()) cerr<mnId<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);
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<

Map.cc、後者は削除しました
void Map::InformNewBigChange()
{
    unique_lock lock(mMutexMap);
    mnBigChangeIdx++;
}

int Map::GetLastBigChangeIdx()
{
    unique_lock 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"

KeyFrameDatabase.ccは修正していません
MapDrawer.ccは修正していません
ORBextractor.ccは修正していません
ORBmatcher.ccは修正していません
PnPsolver.ccは修正していません
Sim3Solver.ccは修正していません
第五週プロジェクト二
毎日持ち歩いているもの