ygz-stereo-inertial.tar

这是YGZ立体惯性SLAM，一种立体惯性VO代码。它专为立体声和立体声惯性传感器模块而设计，如vi-sensor。它使用LK光流作为前端，并使用滑动窗口束调整作为后端。随意在数据集和您自己的传感器中尝试它。高博写的基于LK光流前端的源码，自行添加了ROS节点

#include “ygz/Feature.h“
#include “ygz/MapPoint.h“
#include “ygz/frame.h“
#include “ygz/BackendSlidingWindowG2O.h“
#include “ygz/Tracker.h“
#include “ygz/ORBMatcher.h“
#include “ygz/Camera.h“

// g2o related
#include “ygz/G2OTypes.h“
#include 
#include 
#include 
#include 
#include 

namespace ygz {

    bool BackendSlidingWindowG2O::IsBusy（） {
        unique_lock lock（mMutexAccept）;
        return !mbAcceptKeyframes;
    }

    void BackendSlidingWindowG2O::MainLoop（） {

        mbFinished = false;
        while （1） {

            // Tracking will see that Local Mapping is busy
            SetAcceptKeyframes（false）;

            // Check if there are keyframes in the queue
            if （CheckNewKeyframes（）） {
                // BoW conversion and insertion in Map
                mbAbortBA = false;
                LOG（INFO） << “Process new KF“ << endl;
                ProcessNewKeyframe（）;
                LOG（INFO） << “Process new KF done.“ << endl;
            } else if （Stop（）） {
                // Safe area to stop
                while （isStopped（） && !CheckFinish（）） {
                    usleep（3000）;
                }
                if （CheckFinish（））
                    break;
            }

            ResetIfRequested（）;

            // Tracking will see that Local Mapping is busy
            SetAcceptKeyframes（true）;

            if （CheckFinish（））
                break;

            usleep（3000）;
        }

        SetFinish（）;
    }

    void BackendSlidingWindowG2O::ProcessNewKeyframe（） {
        {
            unique_lock lock（mMutexNewKFs）;
            mpCurrent = mpNewKFs.front（）;
            mpNewKFs.pop_front（）;
        }

        {
            unique_lock lockKF（mMutexKFs）;
            mpKFs.push_back（mpCurrent）;
        }

        // 将双目匹配出来的，且没有关联地图点的那些点，为它们创建新的地图点
        mpCurrent->AssignFeaturesToGrid（）;

        int cntSetGood = 0 cntImmature = 0;

        {
            unique_lock lockMps（mMutexPoints）;
            unique_lock lock（mpCurrent->mMutexFeature）;
            for （size_t i = 0; i < mpCurrent->mFeaturesLeft.size（）; i++） {
                shared_ptr feat = mpCurrent->mFeaturesLeft[i];
                if （feat == nullptr）
                    continue;
                if （feat->mfInvDepth > 0 && feat->mpPoint == nullptr） {

                    // 距离过近或过远都不可靠
                    if （feat->mfInvDepth < setting::minNewMapPointInvD ||
                        feat->mfInvDepth > setting::maxNewMapPointInvD）
                        continue;

                    // create a new map point
                    shared_ptr pNewMP（new MapPoint（mpCurrent i））;
                    pNewMP->AddObservation（mpCurrent i）;
                    feat->mpPoint = pNewMP;
                    pNewMP->Com