ORB-SLAM2 地图加载2

　　补充SystemSetting和InitKeyFrame两个类的代码。实际上，由于是通过SystemSetting来读取的相机内参以及ORB特征参数，所以就可以将Tracking.cc中关于读取内参的部分替换掉了。

1. SystemSetting.h

#ifndef SYSTEMSETTING_H

#define SYSTEMSETTING_H

#include <string>

#include "ORBVocabulary.h"

#include<opencv2/core/core.hpp>

namespace ORB_SLAM2 {

    class SystemSetting{

        //Load camera parameters from setting file

    public:

        SystemSetting(ORBVocabulary* pVoc);

        //SystemSetting::SystemSetting(ORBVocabulary* pVoc, KeyFrameDatabase* pKFDB );

        bool LoadSystemSetting(const std::string strSettingPath);

    public:

        //The Vocabulary and KeyFrameDatabase

        ORBVocabulary* pVocabulary;

        //KeyFrameDatabase* pKeyFrameDatabase;

        //Camera parameters

        float width;

        float height;

        float fx;

        float fy;

        float cx;

        float cy;

        float invfx;

        float invfy;

        float bf;

        float b;

        float fps;

        cv::Mat K;

        cv::Mat DistCoef;

        bool initialized;

        //Camera RGB parameters

        int nRGB;

        //ORB feature parameters

        int nFeatures;

        float fScaleFactor;

        int nLevels;

        float fIniThFAST;

        float fMinThFAST;

        //other parameters

        float ThDepth = -;

        float DepthMapFactor = -;

    };

} //namespace ORB_SLAM2

#endif //SystemSetting

2. SystemSetting.cc

#include <iostream>

#include "SystemSetting.h"

using namespace std;

namespace ORB_SLAM2 {

    SystemSetting::SystemSetting(ORBVocabulary* pVoc):

        pVocabulary(pVoc)

        {

        }

    //SystemSetting::SystemSetting(ORBVocabulary* pVoc, KeyFrameDatabase* pKFDB):

    //    pVocabulary(pVoc), pKeyFrameDatabase(pKFDB)

    //    {

    //    }

    bool SystemSetting::LoadSystemSetting(const std::string strSettingPath){

        cout<<endl<<"Loading System Parameters form:"<<strSettingPath<<endl;

        cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);

        width  = fSettings["Camera.width"];

        height = fSettings["Camera.height"];

        fx     = fSettings["Camera.fx"];

        fy     = fSettings["Camera.fy"];

        cx     = fSettings["Camera.cx"];

        cy     = fSettings["Camera.cy"];

        cv::Mat tmpK = cv::Mat::eye(,,CV_32F);

        tmpK.at<float>(,) = fx;

        tmpK.at<float>(,) = fy;

        tmpK.at<float>(,) = cx;

        tmpK.at<float>(,) = cy;

        tmpK.copyTo(K);

        cv::Mat tmpDistCoef(,,CV_32F);

        tmpDistCoef.at<float>() = fSettings["Camera.k1"];

        tmpDistCoef.at<float>() = fSettings["Camera.k2"];

        tmpDistCoef.at<float>() = fSettings["Camera.p1"];

        tmpDistCoef.at<float>() = fSettings["Camera.p2"];

        const float k3 = fSettings["Camera.k3"];

        if( k3!= )

        {

            tmpDistCoef.resize();

            tmpDistCoef.at<float>() = k3;

        }

        tmpDistCoef.copyTo( DistCoef );

        bf = fSettings["Camera.bf"];

        fps= fSettings["Camera.fps"];

        invfx = 1.0f/fx;

        invfy = 1.0f/fy;

        b     = bf  /fx;

        initialized = true;

        cout<<"- size:"<<width<<"x"<<height<<endl;

        cout<<"- fx:"  <<fx<<endl;

        cout << "- fy: " << fy << endl;

        cout << "- cx: " << cx << endl;

        cout << "- cy: " << cy << endl;

        cout << "- k1: " << DistCoef.at<float>() << endl;

        cout << "- k2: " << DistCoef.at<float>() << endl;

        if(DistCoef.rows==)

            cout << "- k3: " << DistCoef.at<float>() << endl;

        cout << "- p1: " << DistCoef.at<float>() << endl;

        cout << "- p2: " << DistCoef.at<float>() << endl;

        cout << "- bf: " << bf << endl;

        //Load RGB parameter

        nRGB = fSettings["Camera.RGB"];

        //Load ORB feature parameters

        nFeatures = fSettings["ORBextractor.nFeatures"];

        fScaleFactor = fSettings["ORBextractor.scaleFactor"];

        nLevels = fSettings["ORBextractor.nLevels"];

        fIniThFAST = fSettings["ORBextractor.iniThFAST"];

        fMinThFAST = fSettings["ORBextractor.minThFAST"];

        cout << endl  << "ORB Extractor Parameters: " << endl;

        cout << "- Number of Features: " << nFeatures << endl;

        cout << "- Scale Levels: " << nLevels << endl;

        cout << "- Scale Factor: " << fScaleFactor << endl;

        cout << "- Initial Fast Threshold: " << fIniThFAST << endl;

        cout << "- Minimum Fast Threshold: " << fMinThFAST << endl;

        //Load others parameters, if the sensor is MONOCULAR, the parameters is zero;

        //ThDepth = fSettings["ThDepth"];

        //DepthMapFactor = fSettings["DepthMapFactor"];

        fSettings.release();

        return true;

    }

}

3. InitKeyFrame.h

#ifndef INITKEYFRAME_H

#define INITKEYFRAME_H

#include "Thirdparty/DBoW2/DBoW2/BowVector.h"

#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h"

#include "SystemSetting.h"

#include <opencv2/opencv.hpp>

#include "ORBVocabulary.h"

#include "KeyFrameDatabase.h"

//#include "MapPoints.h"

namespace ORB_SLAM2

{

#define FRAME_GRID_ROWS 48

#define FRAME_GRID_COLS 64

class SystemSetting;

class KeyFrameDatabase;

//class ORBVocabulary;

class InitKeyFrame

{

public:

    InitKeyFrame(SystemSetting &SS);

    void UndistortKeyPoints();

    bool PosInGrid(const cv::KeyPoint& kp, int &posX, int &posY);

    void AssignFeaturesToGrid();

public:

    ORBVocabulary* pVocabulary;

    //KeyFrameDatabase* pKeyFrameDatabase;

    long unsigned int nId;

    double TimeStamp;

    float fGridElementWidthInv;

    float fGridElementHeightInv;

    std::vector<std::size_t> vGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS];

    float fx;

    float fy;

    float cx;

    float cy;

    float invfx;

    float invfy;

    float bf;

    float b;

    float ThDepth;

    int N;

    std::vector<cv::KeyPoint> vKps;

    std::vector<cv::KeyPoint> vKpsUn;

    cv::Mat Descriptors;

    //it's zero for mono

    std::vector<float> vRight;

    std::vector<float> vDepth;

    DBoW2::BowVector BowVec;

    DBoW2::FeatureVector FeatVec;

    int nScaleLevels;

    float fScaleFactor;

    float fLogScaleFactor;

    std::vector<float> vScaleFactors;

    std::vector<float> vLevelSigma2;

    std::vector<float> vInvLevelSigma2;

    std::vector<float> vInvScaleFactors;

    int nMinX;

    int nMinY;

    int nMaxX;

    int nMaxY;

    cv::Mat K;

    cv::Mat DistCoef;    

};

} //namespace ORB_SLAM2

#endif //INITKEYFRAME_H

4. InitKeyFrame.cc

#include "InitKeyFrame.h"

#include <opencv2/opencv.hpp>

#include "SystemSetting.h"

namespace ORB_SLAM2

{

InitKeyFrame::InitKeyFrame(SystemSetting &SS):pVocabulary(SS.pVocabulary)//, pKeyFrameDatabase(SS.pKeyFrameDatabase)

{

    fx = SS.fx;

    fy = SS.fy;

    cx = SS.cx;

    cy = SS.cy;

    invfx = SS.invfx;

    invfy = SS.invfy;

    bf = SS.bf;

    b  = SS.b;

    ThDepth = SS.ThDepth;

    nScaleLevels = SS.nLevels;

    fScaleFactor = SS.fScaleFactor;

    fLogScaleFactor = log(SS.fScaleFactor);

    vScaleFactors.resize(nScaleLevels);

    vLevelSigma2.resize(nScaleLevels);

    vScaleFactors[] = 1.0f;

    vLevelSigma2[]  = 1.0f;

    for ( int i = ; i < nScaleLevels; i ++ )

    {

        vScaleFactors[i] = vScaleFactors[i-]*fScaleFactor;

        vLevelSigma2[i]  = vScaleFactors[i]*vScaleFactors[i];

    }

    vInvScaleFactors.resize(nScaleLevels);

    vInvLevelSigma2.resize(nScaleLevels);

    for ( int i = ; i < nScaleLevels; i ++ )

    {

        vInvScaleFactors[i] = 1.0f/vScaleFactors[i];

        vInvLevelSigma2[i]  = 1.0f/vLevelSigma2[i];

    }

    K = SS.K;

    DistCoef = SS.DistCoef;

    if( SS.DistCoef.at<float>()!=0.0)

    {

        cv::Mat mat(,,CV_32F);

        mat.at<float>(,) = 0.0;

        mat.at<float>(,) = 0.0;

        mat.at<float>(,) = SS.width;

        mat.at<float>(,) = 0.0;

        mat.at<float>(,) = 0.0;

        mat.at<float>(,) = SS.height;

        mat.at<float>(,) = SS.width;

        mat.at<float>(,) = SS.height;

        mat = mat.reshape();

        cv::undistortPoints(mat, mat, SS.K, SS.DistCoef, cv::Mat(), SS.K);

        mat = mat.reshape();

        nMinX = min(mat.at<float>(,), mat.at<float>(,));

        nMaxX = max(mat.at<float>(,), mat.at<float>(,));

        nMinY = min(mat.at<float>(,), mat.at<float>(,));

        nMaxY = max(mat.at<float>(,), mat.at<float>(,));

    }

    else

    {

        nMinX = 0.0f;

        nMaxX = SS.width;

        nMinY = 0.0f;

        nMaxY = SS.height;

    }

    fGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/(nMaxX-nMinX);

    fGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/(nMaxY-nMinY);

}

void InitKeyFrame::UndistortKeyPoints()

{

    if( DistCoef.at<float>() == 0.0)

    {

        vKpsUn = vKps;

        return;

    }

    cv::Mat mat(N,,CV_32F);

    for ( int i = ; i < N; i ++ )

    {

        mat.at<float>(i,) = vKps[i].pt.x;

        mat.at<float>(i,) = vKps[i].pt.y;

    }

    mat = mat.reshape();

    cv::undistortPoints(mat, mat, K, DistCoef, cv::Mat(), K );

    mat = mat.reshape();

    vKpsUn.resize(N);

    for( int i = ; i < N; i ++ )

    {

        cv::KeyPoint kp = vKps[i];

        kp.pt.x = mat.at<float>(i,);

        kp.pt.y = mat.at<float>(i,);

        vKpsUn[i] = kp;

    }

}

void InitKeyFrame::AssignFeaturesToGrid()

{

    int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS);

    for ( unsigned int i = ; i < FRAME_GRID_COLS; i ++ )

    {

        for ( unsigned int j = ; j < FRAME_GRID_ROWS; j ++)

            vGrid[i][j].reserve(nReserve);

    }

    for ( int i = ; i < N; i ++ )

    {

        const cv::KeyPoint& kp = vKpsUn[i];

        int nGridPosX, nGridPosY;

    if( PosInGrid(kp, nGridPosX, nGridPosY))

        vGrid[nGridPosX][nGridPosY].push_back(i);

    }

}

bool InitKeyFrame::PosInGrid(const cv::KeyPoint &kp, int &posX,  int &posY)

{

    posX = round((kp.pt.x-nMinX)*fGridElementWidthInv);

    posY = round((kp.pt.y-nMinY)*fGridElementHeightInv);

    if(posX< || posX>=FRAME_GRID_COLS ||posY< || posY>=FRAME_GRID_ROWS)

        return false;

    return true;

}

}

巴特西

ORB-SLAM2 地图加载2

最新文章

热门文章