一起读orb slam2源码

最新推荐文章于 2024-01-25 19:35:36 发布

原创最新推荐文章于 2024-01-25 19:35:36 发布 · 318 阅读

0 ·

CC 4.0 BY-SA版权

slam 专栏收录该内容

5 篇文章

订阅专栏

本文详细剖析ORB SLAM2中的`mono_tum.cc`代码流程，同时涵盖System头文件及实现，以及Frame结构的定义与实现。对于不熟悉DBow字典的读者，推荐查阅相关链接以增进理解。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

一起读orb slam2源码

mono_tum.cc代码流程
System.h和Syetem.cc
Frame.h和Frame.cc

mono_tum.cc代码流程

1. main(int argc, char **argv)   //主函数入口，传入参数
   ./mono_tum path_to_vocabulary path_to_settings path_to_sequence  //  ./mono_tum   字典路径  yaml路径 数据集路径
   
2. LoadImages(strFile, vstrImageFilenames, vTimestamps);    //从txt文件中解析出图片名和时间戳

3.ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true); //初始化一个ORB_SLAM类

4.for(int ni=0; ni<nImages; ni++)  //遍历image
{
    SLAM.TrackMonocular(im,tframe);//im为当前图片 tframe为当前时间戳
}

5.SLAM.Shutdown(); //结束线程
   SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");  //储存轨迹

如果对DBow字典不熟悉可以参考网址https://www.jianshu.com/p/cfcdf12a3bb6

System.h和Syetem.cc

##################################################
                   System.h
##################################################
namespace ORB_SLAM2
{
//声明类
class Viewer;
class FrameDrawer;
class Map;
class Tracking;
class LocalMapping;
class LoopClosing;

class System
{
public:
enum eSensor{
        MONOCULAR=0,
        STEREO=1,
        RGBD=2
    };
public:
System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true); //构造函数,启动本地地图,回环检测和视觉线程
cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp);  //处理双目帧,图像必须是同步的 返回相机位姿
cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp);//处理RGBD图象 返回相机位姿
cv::Mat TrackMonocular(const cv::Mat &im, const double &timestamp); //处理单目 返回相机位姿

void ActivateLocalizationMode();  //这会停止本地地图的构建,只执行相机跟踪
void DeactivateLocalizationMode(); //这会恢复本地地图的构建,并再次做SLAM

bool MapChanged();   //如果发生了回环和全局BA,返回true,从上次执行此函数开始算起
void Reset();      //Reset系统,清除map
void Shutdown();   //所有线程都会被关闭 这个函数必须在储存轨迹前执行

void SaveTrajectoryTUM(const string &filename); //储存轨迹 只能对stereo和RGBD
void SaveKeyFrameTrajectoryTUM(const string &filename);  //储存关键帧轨迹 对所有传感器适用
void SaveTrajectoryKITTI(const string &filename);  //储存轨迹 只能对stereo和RGBD

   // TODO: Save/Load functions
    // SaveMap(const string &filename);
    // LoadMap(const string &filename);

    // You can call this right after TrackMonocular (or stereo or RGBD)
    int GetTrackingState();    //获得最近处理的帧信息
    std::vector<MapPoint*> GetTrackedMapPoints();
    std::vector<cv::KeyPoint> GetTrackedKeyPointsUn();   //KeyPoint是啥？？？？

private:
    eSensor mSensor;   //枚举类型
    
    ORBVocabulary* mpVovabulaty;

	KeyFrameDatabase* mpKeyFrameDatabase;  //这个数据库是用来做回环检测和重定位的

	Map* mpMap;  //用来储存所有的关键帧和地图点？？？？

	Tracking* mpTracker; //跟踪器，它收到一帧然后计算它的位姿
    //它还有其他功能：决定什么时候插入关键帧，创建新的地图点，并在跟踪失败时完成重定位

	LocalMapping* mpLocalMapper;  //本地地图，它用来管理本地地图并完成局部BA

	LoopClosing* mpLoopCloser;    //对于每一个新的关键帧，它会检测回环，如果发生回环，它会完成图优化和全局BA

	Viewer* mpViewer;  //这个viewer用来画地图和当前的相机位姿 用Pangolin

	FrameDrawer* mpFrameDrawer;
	MapDrawer* mpMapDrawer;

//系统线程：局部建图，回环检测，可视化
//Tracking thread live在主函数中
//那Map线程呢？？？
std::thread* mptLocalMapping;
std::thread* mptLoopClosing;
std::thread* mptViewer;

//Reset flag
std:::mutex mMutexReset; //互斥量
bool mbReset;
 
 //改变mode flag
std::mutex mMutexMode;   //std::mutex只有lock和unlock之分
bool mbActivateLocalizationMode;// ？？？
bool mbDeactivateLocalizationMode; // ？？？

//跟踪状态
int mTrackingState;
std::vector<MapPoint*> mTrackedMapPoints;
std::vector<cv::KeyPoint> mTrackedKeyPointsUn;   //是什么？？？
std::mutex mMutexState;
};
}//namespace ORB_SLAM

**********************************************************************
                              system.cc
**********************************************************************

Frame.h和Frame.cc

*********************************************************************
                              Frame.h
*********************************************************************
namespace ORB_SLAM2
{
class MapPoint;
class KeyFrame;

class Frame
{
public:
    Frame();
    Frame(const Frame &frame);  //拷贝构造函数 

   //双目构造函数
    Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth);
	//RGBD构造函数
	Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth);
	//单目构造函数
	Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth);
	
	//提取orb特征点 0 for left image and 1 for right image.
    void ExtractORB(int flag, const cv::Mat &im);  //？？？ 只传入了一张图
    
    //计算词袋
	void ComputeBoW();

	//设定相机位姿
	void SetPose(cv::Mat Tcw);

   //根据相机位姿算出旋转，平移和相机光心矩阵
   void UpdatePoseMatrices();  //光心矩阵？？

   //返回相机光心
   inline cv::Mat GetCameraCenter(){
        return mOw.clone();
    }
    
  //返回旋转的逆
  inline cv::Mat GetRotationInverse(){
        return mRwc.clone();
    }

  //确认MapPoint是否在相机的视野中，并传入用来跟踪的MapPoint
  bool isInFrustum(MapPoint* pMP, float viewingCosLimit);

 //计算关键点的单元格(如果不在网格中，返回false)
 bool PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY);     //？？？
 
 vector<size_t> GetFeaturesInArea(const float &x, const float  &y, const float  &r, const int minLevel=-1, const int maxLevel=-1) const;    //？？？
 
 //匹配左右相机的特征点 如果匹配成功，可以计算出深度并存储与左关键点关联的右坐标。
 void ComputeStereoMatches();
  
  //关联右坐标系到关键点，如果有深度图
  void ComputeStereoFromRGBD(const cv::Mat &imDepth);

  //把keypoint投影到世界坐标系（如果有双目或者深度信息）
  cv::Mat UnprojectStereo(const int &i);
 
 public:
   ORBVocabulary* mpORBvocabulary;
   ORBextractor* mpORBextractorLeft, *mpORBextractorRight;  //只有在双目情况下才用mpORBextractorRight
   double mTimeStamp;
  
  //相机内参 
   cv::Mat mK;
    static float fx;
    static float fy;
    static float cx;
    static float cy;
    static float invfx;
    static float invfy;
    cv::Mat mDistCoef;   //这是什么？？

    float mbf;   //Stereo baseline multiplied by fx
    float mb;   //Stereo baseline in meters

	float mThDepth;  //近点和远点的阈值 Close points are inserted from 1 view. Far points are inserted as in the monocular case from 2 views.
     
     int N;  //keypoint数目

     //RGBD中，RGB图像会有畸变
	 std::vector<cv::KeyPoint> mvKeys, mvKeysRight; //原始图像（没有去畸变）的keypoint向量
     std::vector<cv::KeyPoint> mvKeysUn;   //去畸变的keypoint vector 
     //In the stereo case, mvKeysUn is redundant as images must be rectified

  //Corresponding stereo coordinate and depth for each keypoint.
   // "Monocular" keypoints have a negative value.
   std::vector<float> mvuRight;
    std::vector<float> mvDepth;

   // Bag of Words Vector structures.
    DBoW2::BowVector mBowVec;
    DBoW2::FeatureVector mFeatVec;

  //ORB描述子 ，每列关联一个特征点
   cv::Mat mDescriptors, mDescriptorsRight;
  
  //与keypoint的地图点，没有返回空指针  
  std::vector<MapPoint*> mvpMapPoints;
     
     //标记以识别异常值关联
     std::vector<bool> mvbOutlier;
    
    //keypoint被发放到网格的cell里，目的是为了减少匹配复杂性
    static float mfGridElementWidthInv;  //网格的宽度的倒数
    static float mfGridElementHeightInv; //网格的高度的倒数
    //mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/(mnMaxX-mnMinX);
     //mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/(mnMaxY-mnMinY);
    std::vector<std::size_t> mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS];

   cv::Mat mTcw; //相机位姿
    static long unsigned int nNextId;  //下一帧的id
    long unsigned int mnId;    //当前帧位姿
    
    //参考帧
    KeyFrame* mpReferenceKF;
 
   //尺度金字塔信息  ？？？
   int mnScaleLevels;
   float mfScaleFactor;
   float mfLogScaleFactor;
   vector<float> mvScaleFactors;
   vector<float> mvInvScaleFactors;
   vector<float> mvLevelSigma2;
   vector<float> mvInvLevelSigma2;
 
   //去畸变图像边界（只计算一次）  ？？？是截取图片？？
    static float mnMinX;
    static float mnMaxX;
    static float mnMinY;
    static float mnMaxY;
    static bool mbInitialComputations;  //是否初始化网格大小的flag

private:
  //给keypoint去畸变，只有在RGBD情况下才用，双目已经纠正过了
    void UndistortKeyPoints();

  //对于去畸变的图像计算图像畸变
  void ComputeImageBounds(const cv::Mat &imLeft);

  //把keypoints发布到网格中，加速特征匹配
  void AssignFeaturesToGrid();

    cv::Mat mRcw;
    cv::Mat mtcw;
    cv::Mat mRwc;
    cv::Mat mOw; // ==mtwc 
 };
 } //namespace ORB_SLAM

**************************************************************
                                 Frame.cc
**************************************************************
构造函数
//拷贝构造函数
Frame::Frame(const Frame &frame)
    :mpORBvocabulary(frame.mpORBvocabulary), mpORBextractorLeft(frame.mpORBextractorLeft), mpORBextractorRight(frame.mpORBextractorRight),
     mTimeStamp(frame.mTimeStamp), mK(frame.mK.clone()), mDistCoef(frame.mDistCoef.clone()),
     mbf(frame.mbf), mb(frame.mb), mThDepth(frame.mThDepth), N(frame.N), mvKeys(frame.mvKeys),
     mvKeysRight(frame.mvKeysRight), mvKeysUn(frame.mvKeysUn),  mvuRight(frame.mvuRight),
     mvDepth(frame.mvDepth), mBowVec(frame.mBowVec), mFeatVec(frame.mFeatVec),
     mDescriptors(frame.mDescriptors.clone()), mDescriptorsRight(frame.mDescriptorsRight.clone()),
     mvpMapPoints(frame.mvpMapPoints), mvbOutlier(frame.mvbOutlier), mnId(frame.mnId),
     mpReferenceKF(frame.mpReferenceKF), mnScaleLevels(frame.mnScaleLevels),
     mfScaleFactor(frame.mfScaleFactor), mfLogScaleFactor(frame.mfLogScaleFactor),
     mvScaleFactors(frame.mvScaleFactors), mvInvScaleFactors(frame.mvInvScaleFactors),
     mvLevelSigma2(frame.mvLevelSigma2), mvInvLevelSigma2(frame.mvInvLevelSigma2)
{
    for(int i=0;i<FRAME_GRID_COLS;i++)
        for(int j=0; j<FRAME_GRID_ROWS; j++)
            mGrid[i][j]=frame.mGrid[i][j];

    if(!frame.mTcw.empty())
        SetPose(frame.mTcw);
}

//初始化构造函数（双目）
Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
    :mpORBvocabulary(voc),mpORBextractorLeft(extractorLeft),mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth),
     mpReferenceKF(static_cast<KeyFrame*>(NULL))

//初始化构造函数（RGBD）
Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
    :mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast<ORBextractor*>(NULL)),
     mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth)

//初始化构造函数（单目）
//参数cv::Mat &distCoef, const float &bf，const float &thDepth是什么？？
Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
    :mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast<ORBextractor*>(NULL)),
     mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth)
{
// Frame ID
    mnId=nNextId++;

    // Scale Level Info
    mnScaleLevels = mpORBextractorLeft->GetLevels();
    mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
    mfLogScaleFactor = log(mfScaleFactor);
    mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
    mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
    mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
    mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();

    // ORB extraction
    ExtractORB(0,imGray);    //ExtractORB是哪里定义的？？

    N = mvKeys.size();   //mvKeys的值从哪里来？？

    if(mvKeys.empty())
        return;

    UndistortKeyPoints();

    // 没有双目信息
    mvuRight = vector<float>(N,-1);
    mvDepth = vector<float>(N,-1);

    mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
    mvbOutlier = vector<bool>(N,false);

    // 只有在第一帧的时候才做
    if(mbInitialComputations)
    {
        ComputeImageBounds(imGray);
        
        //计算每个网格的宽和高的倒数
        mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);
        mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);

        fx = K.at<float>(0,0);
        fy = K.at<float>(1,1);
        cx = K.at<float>(0,2);
        cy = K.at<float>(1,2);
        invfx = 1.0f/fx;
        invfy = 1.0f/fy;

        mbInitialComputations=false;
    }

    mb = mbf/fx;  //计算双目基线

    AssignFeaturesToGrid();
}

//std::vector<std::size_t> mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS];
//每个格子是一个向量
void Frame::AssignFeaturesToGrid()
{
     int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS);//  平均每个格子里有多少个特征点*0.5???
     for(unsigned int i=0; i<FRAME_GRID_COLS;i++)
        for (unsigned int j=0; j<FRAME_GRID_ROWS;j++)
            mGrid[i][j].reserve(nReserve);  //每个网格向量为平均特征点数的一半 为什么要取一半？？
      for(int i=0;i<N;i++)
    {
        const cv::KeyPoint &kp = mvKeysUn[i];

        int nGridPosX, nGridPosY;
        if(PosInGrid(kp,nGridPosX,nGridPosY)) //把特征点放到对应的网格向量中
            mGrid[nGridPosX][nGridPosY].push_back(i);
    }
 }