ORB-SLAM2代码详解04: 帧Frame

最新推荐文章于 2024-04-27 21:35:36 发布
最新推荐文章于 2024-04-27 21:35:36 发布
阅读量5.1k 收藏 30
点赞数 29
分类专栏： SLAM 文章标签： ORB-SLAM2
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/ncepu_chen/article/details/116784701
版权

pdf版本笔记的下载地址: ORB-SLAM2代码详解04_帧Frame,排版更美观一点,这个网站的默认排版太丑了（访问密码：3834）

ORB-SLAM2代码详解04: 帧Frame

可以看看我录制的视频5小时让你假装大概看懂ORB-SLAM2源码

请添加图片描述

各成员函数/变量

相机相关信息

Frame类与相机相关的参数大部分设为static类型,整个系统内的所有Frame对象共享同一份相机参数.

成员函数/变量访问控制意义
mbInitialComputationspublic static是否需要为Frame类的相机参数赋值
初始化为false,第一次为相机参数赋值后变为false
float fx, float fy
float cx, float cy
float invfx, float invfy		public static相机内参
cv::Mat mKpublic相机内参矩阵
设为static是否更好?
float mbpublic相机基线,相机双目间的距离
float mbfpublic相机基线与焦距的乘积

这些参数首先由Tracking对象从配置文件TUM1.yaml内读入,再传给Frame类的构造函数,第一次调用Frame的构造函数时为这些成员变量赋值.

Tracking::Tracking(const string &strSettingPath, ...) {

    // 从配置文件中读取相机参数并构造内参矩阵
    cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);
    float fx = fSettings["Camera.fx"];
    float fy = fSettings["Camera.fy"];
    float cx = fSettings["Camera.cx"];
    float cy = fSettings["Camera.cy"];

    cv::Mat K = cv::Mat::eye(3, 3, CV_32F);
    K.at<float>(0, 0) = fx;
    K.at<float>(1, 1) = fy;
    K.at<float>(0, 2) = cx;
    K.at<float>(1, 2) = cy;
    K.copyTo(mK);

    // ...
}

// 每传来一帧图像,就调用一次该函数
cv::Mat Tracking::GrabImageStereo(..., const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double &timestamp) {
	mCurrentFrame = Frame(mImGray, mK, mDistCoef, mbf, mThDepth);

    Track();

    // ...
}

// Frame构造函数
Frame::Frame(cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
	: mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth) {
    
	// ...
        
	// 第一次调用Frame()构造函数时为所有static变量赋值
    if (mbInitialComputations) {
        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;		// 赋值完毕后将mbInitialComputations复位
    }

    mb = mbf / fx;
}

特征点提取

在Frame类构造函数中调用成员变量mpORBextractorLeft和mpORBextractorRight的()运算符进行特征点提取.

成员函数/变量访问控制意义
ORBextractor* mpORBextractorLeft
ORBextractor* mpORBextractorRight		public左右目图像的特征点提取器
cv::Mat mDescriptors
cv::Mat mDescriptorsRight		public左右目图像特征点描述子
std::vector<cv::KeyPoint> mvKeys
std::vector<cv::KeyPoint> mvKeysRight		public畸变矫正前的左/右目特征点
std::vector<cv::KeyPoint> mvKeysUnpublic畸变矫正后的左目特征点
std::vector<float> mvuRightpublic左目特征点在右目中匹配特征点的横坐标
(左右目匹配特征点的纵坐标相同)
std::vector<float> mvDepthpublic特征点深度
float mThDepthpublic判断单目特征点和双目特征点的阈值
深度低于该值得特征点被认为是双目特征点
深度低于该值得特征点被认为是单目特征点

mvKeys、 mvKeysUn、 mvuRight、 mvDepth的坐标索引是对应的,也就是说对于第i个图像特征点:

  • 其畸变矫正前的左目特征点是mvKeys[i].
  • 其畸变矫正后的左目特征点是mvKeysUn[i].
  • 其在右目图片中对应特征点的横坐标为mvuRight[i],纵坐标与mvKeys[i]的纵坐标相同.
  • 特征点的深度是mvDepth[i].

对于单目特征点(单目相机输入的特征点或没有找到右目匹配的左目图像特征点),其mvuRight和mvDepth均为-1.

特征点提取: ExtractORB()

成员函数/变量访问控制意义
void ExtractORB(int flag, const cv::Mat &im)public进行ORB特征提取
void Frame::ExtractORB(int flag, const cv::Mat &im) {
    if (flag == 0)		// flag==0, 表示对左图提取ORB特征点
        (*mpORBextractorLeft)(im, cv::Mat(), mvKeys, mDescriptors);
    else				// flag==1, 表示对右图提取ORB特征点
        (*mpORBextractorRight)(im, cv::Mat(), mvKeysRight, mDescriptorsRight);
}

ORB-SLAM2对双目/RGBD特征点的预处理

双目/RGBD相机中可以得到特征点的立体信息,包括右目特征点信息(mvuRight)、特征点深度信息(mvDepth)

  • 对于双目相机,通过双目特征点匹配关系计算特征点的深度值.
  • 对于RGBD相机,根据特征点深度构造虚拟的右目图像特征点.

请添加图片描述

成员函数/变量访问控制意义
void ComputeStereoMatches()public双目图像特征点匹配,用于双目相机输入图像预处理
void ComputeStereoFromRGBD(const cv::Mat &imDepth)public根据深度信息构造虚拟右目图像,用于RGBD相机输入图像预处理
cv::Mat UnprojectStereo(const int &i)public根据深度信息将第i个特征点反投影成MapPoint

通过这种预处理,在后面SLAM系统的其他部分中不再区分双目特征点和RGBD特征点,它们以双目特征点的形式被处理.(仅通过判断mvuRight[idx]判断某特征点是否有深度).

int ORBmatcher::SearchByProjection(Frame &F, const vector<MapPoint *> &vpMapPoints, const float th) {
	// ...
    for (size_t idx : vIndices) {
        if (F.mvuRight[idx] > 0) {		// 通过判断 mvuRight[idx] 判断该特征点是否有深度
            // 针对有深度的特征点特殊处理
        } else {
            // 针对单目特征点的特殊处理
        }
    }
    // ...
}

双目视差公式

请添加图片描述

观测距离 z z z,基线 b b b,焦距 f f f,视差 d d d,根据三角形相似性:
z b = z − f b − d \frac{z}{b} = \frac{z-f}{b-d} bz​=b−dz−f​
得到:
d = b ⋅ f z d = \frac{b \cdot f}{z} d=zb⋅f​

双目特征点的处理:双目图像特征点匹配: ComputeStereoMatches()

请添加图片描述

双目相机分别提取到左右目特征点后对特征点进行双目匹配,并通过双目视差估计特征点深度.双目特征点匹配步骤:

  1. 粗匹配: 根据特征点描述子距离和金字塔层级判断匹配.粗匹配关系是按行寻找的,对于左目图像中每个特征点,在右目图像对应行上寻找匹配特征点.
  2. 精匹配: 根据特征点周围窗口内容相似度判断匹配.
  3. 亚像素插值: 将特征点相似度与匹配坐标之间拟合成二次曲线,寻找最佳匹配位置(得到的是一个小数).
  4. 记录右目匹配mvuRight和深度mvDepth信息.
  5. 离群点筛选: 以平均相似度的2.1倍为标准,筛选离群点.

请添加图片描述

void Frame::ComputeStereoMatches() {

    mvuRight = vector<float>(N, -1.0f);
    mvDepth = vector<float>(N, -1.0f);
    
    // step0. 右目图像特征点逐行统计: 将右目图像中每个特征点注册到附近几行上
    vector<vector<size_t> > vRowIndices(nRows, vector<size_t>());	// 图像每行的1右目特征点索引
    for (int iR = 0; iR < mvKeysRight.size(); iR++) {
        const cv::KeyPoint &kp = mvKeysRight[iR];
        const float &kpY = kp.pt.y;
        const int maxr = ceil(kpY + 2.0f * mvScaleFactors[mvKeysRight[iR].octave]);
        const int minr = floor(kpY - 2.0f * mvScaleFactors[mvKeysRight[iR].octave]);
        for (int yi = minr; yi <= maxr; yi++)
            vRowIndices[yi].push_back(iR);
    }

    // step1. + 2. 粗匹配+精匹配
    const float minZ = mb, minD = 0, maxD = mbf / minZ;		// 根据视差公式计算两个特征点匹配搜索的范围
	const int thOrbDist = (ORBmatcher::TH_HIGH + ORBmatcher::TH_LOW) / 2;

    vector<pair<int, int> > vDistIdx;		// 保存特征点匹配

    for (int iL = 0; iL < N; iL++) {

        const cv::KeyPoint &kpL = mvKeys[iL];
        const int &levelL = kpL.octave;
        const float &vL = kpL.pt.y, &uL = kpL.pt.x;

        const vector<size_t> &vCandidates = vRowIndices[vL];
        if (vCandidates.empty()) continue;
		
        // step1. 粗匹配,根据特征点描述子和金字塔层级进行粗匹配
        int bestDist = ORBmatcher::TH_HIGH;
        size_t bestIdxR = 0;
        const cv::Mat &dL = mDescriptors.row(iL);
        for (size_t iC = 0; iC < vCandidates.size(); iC++) {

            const size_t iR = vCandidates[iC];
            const cv::KeyPoint &kpR = mvKeysRight[iR];
            if (kpR.octave < levelL - 1 || kpR.octave > levelL + 1)
                continue;
            const float &uR = kpR.pt.x;

            if (uR >= minU && uR <= maxU) {
                const cv::Mat &dR = mDescriptorsRight.row(iR);
                const int dist = ORBmatcher::DescriptorDistance(dL, dR);
                if (dist < bestDist) {
                    bestDist = dist;
                    bestIdxR = iR;
                }
            }
        }

        // step2. 精匹配: 滑动窗口匹配,根据匹配点周围5✖5窗口寻找精确匹配
        if (bestDist < thOrbDist) {
            const float uR0 = mvKeysRight[bestIdxR].pt.x;
            const float scaleFactor = mvInvScaleFactors[kpL.octave];
            const float scaleduL = round(kpL.pt.x * scaleFactor);
            const float scaledvL = round(kpL.pt.y * scaleFactor);
            const float scaleduR0 = round(uR0 * scaleFactor);
            const int w = 5;
            cv::Mat IL = mpORBextractorLeft->mvImagePyramid[kpL.octave].rowRange(scaledvL - w, scaledvL + w + 1).colRange(scaleduL - w, scaleduL + w + 1);
            IL.convertTo(IL, CV_32F);
            IL = IL - IL.at<float>(w, w) * cv::Mat::ones(IL.rows, IL.cols, CV_32F);
            int bestDist = INT_MAX;
            int bestincR = 0;
            const int L = 5;
            vector<float> vDists;
            vDists.resize(2 * L + 1);
            const float iniu = scaleduR0 + L - w;
            const float endu = scaleduR0 + L + w + 1;
            for (int incR = -L; incR <= +L; incR++) {
                cv::Mat IR = mpORBextractorRight->mvImagePyramid[kpL.octave].rowRange(scaledvL - w, scaledvL + w + 1).colRange(scaleduR0 + incR - w, scaleduR0 + incR + w + 1);
                IR.convertTo(IR, CV_32F);
                IR = IR - IR.at<float>(w, w) * cv::Mat::ones(IR.rows, IR.cols, CV_32F);
                float dist = cv::norm(IL, IR, cv::NORM_L1);
                if (dist < bestDist) {
                    bestDist = dist;
                    bestincR = incR;
                }
                vDists[L + incR] = dist;
            }
            
            // step3. 亚像素插值: 将特征点匹配距离拟合成二次曲线,寻找二次曲线最低点(是一个小数)作为最优匹配点坐标
            const float dist1 = vDists[L + bestincR - 1];
            const float dist2 = vDists[L + bestincR];
            const float dist3 = vDists[L + bestincR + 1];
            const float deltaR = (dist1 - dist3) / (2.0f * (dist1 + dist3 - 2.0f * dist2));

            // step4. 记录特征点的右目和深度信息
            float bestuR = mvScaleFactors[kpL.octave] * ((float) scaleduR0 + (float) bestincR + deltaR);
            float disparity = (uL - bestuR);
            if (disparity >= minD && disparity < maxD) {
                mvDepth[iL] = mbf / disparity;
                mvuRight[iL] = bestuR;
                vDistIdx.push_back(pair<int, int>(bestDist, iL));
            }
        }
    }
    
    // step5. 删除离群点: 匹配距离大于平均匹配距离2.1倍的视为误匹配
    sort(vDistIdx.begin(), vDistIdx.end());
    const float median = vDistIdx[vDistIdx.size() / 2].first;
    const float thDist = 1.5f * 1.4f * median;
    for (int i = vDistIdx.size() - 1; i >= 0; i--) {
        if (vDistIdx[i].first < thDist)
            break;
        else {
            mvuRight[vDistIdx[i].second] = -1;
            mvDepth[vDistIdx[i].second] = -1;
        }
    }
}

RBGD特征点的处理: 根据深度信息构造虚拟右目图像: ComputeStereoFromRGBD()

请添加图片描述

对于RGB特征点,根据深度信息构造虚拟右目图像

void Frame::ComputeStereoFromRGBD(const cv::Mat &imDepth) {
    // 初始化 右目 和 深度 信息
    mvuRight = vector<float>(N, -1);
    mvDepth = vector<float>(N, -1);

    for (int i = 0; i < N; i++) {
        const cv::KeyPoint &kp = mvKeys[i];
        const cv::KeyPoint &kpU = mvKeysUn[i];
 
        // 从未畸变矫正的深度图中获取深度信息,从校正过后的左图中获取特征点位置信息,构造虚拟右目
        const float d = imDepth.at<float>(kp.pt.y, kp.pt.x);
        if (d > 0) {
            mvDepth[i] = d;
            mvuRight[i] = kpU.pt.x - mbf / d;
        }
    }
}

畸变矫正: UndistortKeyPoints()

成员函数/变量访问控制意义
cv::Mat mDistCoefpublic相机的畸变矫正参数
std::vector<cv::KeyPoint> mvKeys
std::vector<cv::KeyPoint> mvKeysRight		public畸变矫正前的左/右目特征点
std::vector<cv::KeyPoint> mvKeysUnpublic畸变矫正后的左目特征点
void UndistortKeyPoints()private对所有特征点进行畸变矫正
float mnMinX
float mnMaxX
float mnMinY
float mnMaxY		public畸变矫正后的图像边界
void ComputeImageBounds(const cv::Mat &imLeft)private计算畸变矫正后的图像边界

实际上,畸变矫正只对单目和RGBD相机输入图像有效,双目相机的畸变矫正参数均为0,因为双目相机数据集在发布之前预先做了双目矫正.

  • RGBD相机输入配置文件TUM1.yaml

    Camera.k1: 0.262383
Camera.k2: -0.953104
Camera.p1: -0.005358
Camera.p2: 0.002628
Camera.k3: 1.163314

#....

  • 双目相机输入配置文件EuRoC.yaml

    Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0

# ...

双目矫正效果如下,双目矫正将两个相机的成像平面矫正到同一平面上.双目矫正之后两个相机的极线相互平行,极点在无穷远处,这也是我们在函数ComputeStereoMatches()中做极线搜索的理论基础.

请添加图片描述

UndistortKeyPoints()函数和ComputeImageBounds()内调用了cv::undistortPoints()函数对特征点进行畸变矫正

void Frame::UndistortKeyPoints() {
    // step1. 若输入图像是双目图像,则已做好了双目矫正,其畸变参数为0
    if (mDistCoef.at<float>(0) == 0.0) {
        mvKeysUn = mvKeys;
        return;
    }

	// 将特征点坐标转为undistortPoints()函数要求的格式
    cv::Mat mat(N, 2, CV_32F);
    for (int i = 0; i < N; i++) {
        mat.at<float>(i, 0) = mvKeys[i].pt.x;
        mat.at<float>(i, 1) = mvKeys[i].pt.y;
    }
	mat = mat.reshape(2);
    // 进行畸变矫正
    cv::undistortPoints(mat, mat, mK, mDistCoef, cv::Mat(), mK);

    // 记录校正后的特征点
    mat = mat.reshape(1);
    mvKeysUn.resize(N);
    for (int i = 0; i < N; i++) {
        cv::KeyPoint kp = mvKeys[i];
        mvKeysUn[i].pt.x = mat.at<float>(i, 0);
        mvKeysUn[i].pt.y = mat.at<float>(i, 1);
    }
}

// 通过计算图片顶点畸变矫正后的坐标来计算畸变矫正后的图片有效范围
void Frame::ComputeImageBounds(const cv::Mat &imLeft) {
    if (mDistCoef.at<float>(0) != 0.0) {
        // 4个顶点坐标
        cv::Mat mat(4, 2, CV_32F);
        mat.at<float>(0, 0) = 0.0;         //左上
        mat.at<float>(0, 1) = 0.0;
        mat.at<float>(1, 0) = imLeft.cols; //右上
        mat.at<float>(1, 1) = 0.0;
        mat.at<float>(2, 0) = 0.0;         //左下
        mat.at<float>(2, 1) = imLeft.rows;
        mat.at<float>(3, 0) = imLeft.cols; //右下
        mat.at<float>(3, 1) = imLeft.rows;
		// 畸变矫正
        mat = mat.reshape(2);
        cv::undistortPoints(mat, mat, mK, mDistCoef, cv::Mat(), mK);
        mat = mat.reshape(1);
        // 记录图片有效范围
        mnMinX = min(mat.at<float>(0, 0), mat.at<float>(2, 0));		//左上和左下横坐标最小的
        mnMaxX = max(mat.at<float>(1, 0), mat.at<float>(3, 0));		//右上和右下横坐标最大的
        mnMinY = min(mat.at<float>(0, 1), mat.at<float>(1, 1));		//左上和右上纵坐标最小的
        mnMaxY = max(mat.at<float>(2, 1), mat.at<float>(3, 1));		//左下和右下纵坐标最小的
    } else {
        mnMinX = 0.0f;
        mnMaxX = imLeft.cols;
        mnMinY = 0.0f;
        mnMaxY = imLeft.rows;
    }
}

特征点分配: AssignFeaturesToGrid()

在对特征点进行预处理后,将特征点分配到48行64列的网格中以加速匹配

成员函数/变量访问控制意义
FRAME_GRID_ROWS=48
FRAME_GRID_COLS=64		#DEFINE宏网格行数/列数
float mfGridElementWidthInv
float mfGridElementHeightInv		public static
public static		每个网格的宽度/高度
std::vector<std::size_t> mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS]public每个网格内特征点编号列表
void AssignFeaturesToGrid()private将特征点分配到网格中
vector<size_t> GetFeaturesInArea(float &x, float &y, float &r, int minLevel, int maxLevel)public获取半径为r的圆域内的特征点编号列表

成员变量std::vector<std::size_t> mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS]是一个二维数组,数组中每个元素是对应网格的所有特征点索引列表.

static成员变量mfGridElementWidthInv、mfGridElementHeightInv表示网格宽度/高度,它们在第一次调用Frame构造函数时被计算赋值.

// Frame构造函数
Frame::Frame(cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
	: mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth) {
    
	// ...
    
	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);
        
	// 第一次调用Frame()构造函数时为所有static变量赋值
    if (mbInitialComputations) {
        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;		// 赋值完毕后将mbInitialComputations复位
    }

    mb = mbf / fx;
}

函数AssignFeaturesToGrid()将特征点分配到网格中

void Frame::AssignFeaturesToGrid() {
    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);
    }
}

函数vector<size_t> GetFeaturesInArea(float &x, float &y, float &r, int minLevel, int maxLevel)获取点(y,x)周围半径为r的圆域内所有特征点编号.

请添加图片描述

构造函数: Frame()

Frame()构造函数依次进行上面介绍的步骤:

// 双目相机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)
    : mpORBvocabulary(voc), mpORBextractorLeft(extractorLeft), mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), mpReferenceKF(static_cast<KeyFrame *>(NULL)) {
    
        
	// step0. 帧ID自增
    mnId = nNextId++;

    // step1. 计算金字塔参数
    mnScaleLevels = mpORBextractorLeft->GetLevels();
    mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
    mfLogScaleFactor = log(mfScaleFactor);
    mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
    mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
    mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
    mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();

    // step2. 提取双目图像特征点
    thread threadLeft(&Frame::ExtractORB, this, 0, imLeft);
    thread threadRight(&Frame::ExtractORB, this, 1, imRight);
    threadLeft.join();
    threadRight.join();

    N = mvKeys.size();
    if (mvKeys.empty())
        return;

    // step3. 畸变矫正,实际上UndistortKeyPoints()不对双目图像进行矫正
    UndistortKeyPoints();

    // step4. 双目图像特征点匹配
    ComputeStereoMatches();
        
	// step5. 第一次调用构造函数时计算static变量
    if (mbInitialComputations) {
        ComputeImageBounds(imLeft);
        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;
    }
	
	mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(NULL));	// 初始化本帧的地图点
    mvbOutlier = vector<bool>(N, false);	// 标记当前帧的地图点不是外点
    mb = mbf / fx;		// 计算双目基线长度

    // step6. 将特征点分配到网格中
    AssignFeaturesToGrid();
}

// RGBD相机Frame构造函数
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) {
    // step0. 帧ID自增
    mnId = nNextId++;

    // step1. 计算金字塔参数
    mnScaleLevels = mpORBextractorLeft->GetLevels();
    mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
    mfLogScaleFactor = log(mfScaleFactor);
    mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
    mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
    mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
    mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();

    // step2. 提取左目图像特征点
    ExtractORB(0, imGray);

    N = mvKeys.size();
    if (mvKeys.empty())
        return;

    // step3. 畸变矫正
    UndistortKeyPoints();
    
    // step4. 根据深度信息构造虚拟右目图像
    ComputeStereoFromRGBD(imDepth);

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

    // step5. 第一次调用构造函数时计算static变量
    if (mbInitialComputations) {
        ComputeImageBounds(imLeft);
        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;
    }
        
	mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(NULL));	// 初始化本帧的地图点
    mvbOutlier = vector<bool>(N, false);	// 标记当前帧的地图点不是外点
    mb = mbf / fx;		// 计算双目基线长度


    // step6. 将特征点分配到网格中
    AssignFeaturesToGrid();
}

Frame类的用途

Tracking类有两个Frame类型的成员变量

成员函数/变量访问控制意义
Frame mCurrentFramepublic当前正在处理的帧
Frame mLastFrameprotected上一帧

Tracking线程每收到一帧图像,就调用函数Tracking::GrabImageMonocular()、Tracking::GrabImageStereo()或Tracking::GrabImageRGBD()创建一个Frame对象,赋值给mCurrentFrame.

// 每传来一帧图像,就调用一次这个函数
cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp) {
    mImGray = im;
	// 图像通道转换
    if (mImGray.channels() == 3) {
        if (mbRGB)
            cvtColor(mImGray, mImGray, CV_RGB2GRAY);
        else
            cvtColor(mImGray, mImGray, CV_BGR2GRAY);
    } else if (mImGray.channels() == 4) {
        if (mbRGB)
            cvtColor(mImGray, mImGray, CV_RGBA2GRAY);
        else
            cvtColor(mImGray, mImGray, CV_BGRA2GRAY);
    }

    // 构造Frame
    if (mState == NOT_INITIALIZED || mState == NO_IMAGES_YET) //没有成功初始化的前一个状态就是NO_IMAGES_YET
        mCurrentFrame = Frame(mImGray, timestamp, mpIniORBextractor, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth);
    else
        mCurrentFrame = Frame(mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth);

    // 跟踪
    Track();

    // 返回当前帧的位姿
    return mCurrentFrame.mTcw.clone();
}

Track()函数跟踪结束后,会将mCurrentFrame赋值给mLastFrame

void Tracking::Track() {
	
    // 进行跟踪
    // ...
    
	// 将当前帧记录为上一帧
	mLastFrame = Frame(mCurrentFrame);
    
    // ...
}

请添加图片描述

除了少数被选为KeyFrame的帧以外,大部分Frame对象的作用仅在于Tracking线程内追踪当前帧位姿,不会对LocalMapping线程和LoopClosing线程产生任何影响,在mLastFrame和mCurrentFrame更新之后就被系统销毁了.

pdf版本笔记的下载地址: ORB-SLAM2代码详解04_帧Frame,排版更美观一点,这个网站的默认排版太丑了（访问密码：3834）

ncepu_Chen
关注
专栏目录
ORB-SLAM2项目实战(3) — ORB-SLAM2稠密地图重建
几度春风里的博客
06-04 8118
文章参考部分开源代码和报错文章。
ORB-SLAM2源码解读（2.1）：Tracking
try_again_later的博客
11-14 2163
Tracking是SLAM的灵魂，更像是前端里程计VO，这里Tracking的主要任务两方面：（1）完成相机位姿估计（2）跟踪局部地图 思路：TrackLocalMap（）在当前帧和局部地图之间找到尽可能多的对应关系，优化当前帧的位姿。对每一帧都进行跟踪 第一次接触这么大的工程，发现之前接触的真的好弱鸡，总结下来是：先看基本流程图+从头到尾啃一遍代码+总结 这里要感谢吴博和吃水的鱼和蚁族的坚...
orb-slam2源码解读（一）——Frame类
福尔摩睿的工作站
01-18 2477
Orb-slam2是slam界中一个非常优秀且著名的项目，关于该项目的解读网上资料较少，大都是对算法的整体构架和算法思路进行解读，本系列博客将会带你一点一滴地解读orb-slam2的源代码，在兼顾算法讲解的同时，更着重与代码的结构与实现细节。 这次就先从Frame类讲起。 Frame类： Frame类的主要作用是实时记录和处理每一帧的信息，给视频中的每一帧进行一个信息的总结处理。
ORB-SLAM2源码详解
07-18
详细介绍ORB-SLAM2源码内容，是初学者学习源码的好文档，转载自【泡泡机器人SLAM】微信公众号
ORB-SLAM2代码
may0522的博客
10-09 137
摄像头学习：https://blog.csdn.net/learning_tortosie/article/details/79879575 参考博客：https://blog.csdn.net/learning_tortosie/article/list/4 安装教程：https://blog.csdn.net/sanyiji/article/details/105591536 （换为中科大的源：https://blog.csdn.net/sumyer/article/details/8810.
ORB-SLAM2代码详解
热门推荐
格物致知
05-01 1万+
ORB-SLAM2代码详解 文章目录ORB-SLAM2代码详解1. ORB-SLAM2代码详解01_ORB-SLAM2代码运行流程1 运行官方Demo1.2. 阅读代码之前你应该知道的事情1.2.1 变量命名规则1.3 理解多线程1.3.1 为什么要使用多线程?1.3.2 多线程中的锁1.4 SLAM主类`System`1.4.1 System`类是ORB-SLAM2系统的主类,先分析其主要的成员函数和成员变量:1.4.2 构造函数1.4.3 跟踪函数2. ORB-SLAM2代码详解02_特征点提取器ORB
ORB SLAM2 代码分析(1)
huangkangying的专栏
03-05 496
简介 ORB-SLAM只支持单目相机，ORB-SLAM2在ORB-SLAM的基础上加入了RGBD和双目相机的支持。相对于ORB-SLAM, ORB-SLAM2的主要贡献如下： 这是一个同时支持单目，双目，RGBD的开源SLAM系统 通过BA对RGBD进行优化，效果优于ICP 通过使用近/远距离的双目观测及单目观测，使得双目slam效果优于目前的state-of-the-art 支持localiz...
SLAM知识点——ORB-SLAM2跑通代码
qq_46515446的博客
01-24 2218
TUM数据集：https://vision.in.tum.de/data/datasets/rgbd-dataset/download# associate.py文件：点击useful tools跳转到该文件（只能使用python2） Ubuntu：which python; sudo apt install pip pangolin安装：https://blog.csdn.net/u012986684/article/details/52860849 sudo apt install git cmake升
ORB-SLAM2源码详解.pdf
04-01
ORB-SLAM2源码详解.pdf文件中详细解析了ORB-SLAM2的关键源代码，这是由信息科学与工程学院人工智能与机器人研究所的吴博撰写。在本文中，将介绍ORB-SLAM2的代码架构、变量命名规则、系统入口以及三个主要线程的工作...
ORB-SLAM2源码详解-补充H矩阵分解1
08-04
总结来说，ORB-SLAM2的源码详解涉及到图像输入、跟踪、局部映射和闭环检测等多个核心模块，每个模块都有其特定的职责和算法。其中，H矩阵的分解是解决定位和闭环检测问题的关键技术之一。理解这些概念和代码细节对于...
orbslam2 代码详细解读
04-26
参考这篇文章做的pdf 文件https://blog.csdn.net/snnily/article/details/79560722 具体视频讲解可以参考 https://v.qq.com/x/page/p0526c7c1cm.html
ORB-SLAM2源码中文详解.pdf
08-17
东北大学吴博讲解ORB_SLAM的文档，仅有文档。
ORB_SLAM2代码注释原理解析
05-08
ORB_SLAM2 特征法SLAM 单目 双目 RGBD * ORB主要借鉴了PTAM的思想，借鉴的工作主要有 * Rubble的ORB特征点； * DBow2的place recognition用于闭环检测； * Strasdat的闭环矫正和covisibility graph思想； * 以及Kuemmerle和Grisetti的g2o用于优化。
ORB-SLAM2源码详解1
08-03
在这个源码详解的第一部分，我们将深入理解ORB-SLAM2的基本架构和核心模块，以及代码中的变量命名规则。 1. **代码结构**： - **Tracking.cpp**: 负责实时的相机姿态估计和特征匹配。 - **LocalMapping.cpp**: ...
ORB-SLAM2代码详解08: 局部建图线程LocalMapping
ncepu_Chen的博客
05-14 4517
ORB-SLAM2代码详解08: 局部建图线程LocalMapping各成员函数/变量局部建图主函数: `Run()`处理队列中第一个关键帧: `ProcessNewKeyFrame()`剔除冗余地图点: `MapPointCulling()`创建新地图点: `CreateNewMapPoints()`融合当前关键帧和其共视帧的地图点: `SearchInNeighbors()`局部BA优化: `Optimizer::LocalBundleAdjustment()`剔除冗余关键帧: `KeyFrameCul
【代码阅读】ORBSLAM2
博客标题不能为空我也没办法
07-30 1239
ORBSLAM2源代码阅读记录，根据程序运行顺序整理了代码的实现细节
ORB-SLAM2
最新发布
qq_65913626的博客
04-27 1038
从github.com下载ORB-SLAM2的代码。地址是：ORB-SLAM2可以在github上直接download ZIP压缩文件。也可在安装git之后，用以下命令在终端上下载。
ORB-SLAM2系统代码流程粗讲解（一）
weixin_51547017的博客
12-18 612
注：本文讲解的是整个系统的运行过程，不涉及具体代码的讲解。均假设系统运行没有重复构建现象 需要说明该系统分为三个线程并行进行：追踪线程、局部建图线程、回环检测线程 1.从main函数讲起，进行整个系统初始化，包括判断输入传感器的类型，加载词袋模型，初始化三个线程并运行，设置进程间的指针。 2.依次遍历每张图像，将图像传入SLAM系统，先判断系统的类型（纯追踪模式还是追踪＋建图），接下来根据传入的图像判断相机的位姿。 3.每一帧图像进入均需要提取特征点，唯一的区别是当第一帧图像图像进入时，要求提取2倍的特征点
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值