orbslam2学习记录

1.orb slam2的总体框架

Tracking -跟踪提取关键点 帧间匹配 初选关键帧
Local Mapping- 关键帧筛选 地图点剔除 局部优化
Loop closing 回环检测

• 输入图像
• 预处理图像
• 位姿检测（运动模型）
• 初始化【2】
• 投影跟踪
• 重定位
• 跟踪局部地图
• 关键帧的选择
• 去除无效地图点
• 局部聚类
• 无用关键帧删除
• 词袋模型
• se3计算位姿变换
• 回环矫正
• 更新地图
  

2.orb slam2如何完成初始化

• 跟踪前需要进行初始化
• 初始化包括相机初始帧位姿，新建地图，新建关键帧
• 单目初始化：
• 寻找匹配点：对前两帧图像提取ORB特征之后进行特征点匹配，匹配数小于100 则失败
• 由匹配点恢复位姿：利用八点法同时计算单应矩阵和基础矩阵，并通过计算评分比较合适的结果作为初始位姿。
  • 八点法https://blog.csdn.net/lzydelyc/article/details/105653896?biz_id=102&utm_term=八点法&utm_medium=distribute.pc_search_result.none-task-blog-2_allsobaiduweb~default-1-105653896&spm=1018.2118.3001.4449

3. 如何从单幅图提取特征点

什么是特征点

• 相机位置少量变化后会有一些保持不变的点
• 角点就是特征（旋转 走近可能就不是了）
• 由关键点和描述子组成
• 关键点：特征点在图像中的位置+朝向大小等
• 描述子：向量 描述了关键点周围的像素关系 两个特征点距离近 描述子同
• 特征=提取关键点+计算描述子

ORB特征

关键点

• 关键点：“Oriented FAST” ,是一种改进的 FAST 角点
• 主要检测局部像素灰度变化明显的地方
• FAST角点检测法速度快
• 尺度不变性/尺度描述-图像金字塔
• 旋转不变性/旋转描述-灰度质心法
• 步骤
  • 在图像中选中像素p 亮度为Ip
  • 设置阈值T 比如Ip的20%
  • 以像素 p 为中心, 选取半径为 3 的圆上的 16 个像素点假如选取的圆上,有连续的 N 个点的亮度大于 Ip+ T 或小于 Ip− T,那么像素 p 可以被认为是特征点 (N 通常取 12,即为 FAST-12。其它常用的 N 取值为 9 和 11, 他们分别被称为 FAST-9,FAST-11)。/对于每个像素,直接检测邻域圆上的第 1,5,9,13 个像素的亮度。只有当这四个像素中有三个同时大于 Ip+ T 或小于 Ip− T 时,当前像素才有可能是一个角点,否则应该直接排除。
  • 遍历每个像素
  • 非极大抑制：在一定区域内只保留响应极大值得点，避免角点集中

图像金字塔

• 为了实现尺度不变性：为了实现尺度不变性，需要给特征加上尺度因子。在进行特征描述的时候，将尺度统一就可以实现尺度不变性了。
• 缩放因子为1.2
• 原始图像为第零层依次进行1/1.2缩放，共八张七层
• 分别对得到的图形进行特征提取
• 记录特征所在金字塔的第几层
• 
• 第m层对应成像视野I0
• dm第m层相机距图像的距离
• dm/d0=1.2^m
• Fm第m层的特征点
• 要想第m层特征点和第零层一样大 该层空间点与相机的距离，即尺度不变性的最大物距dmax=dm*1.2^m
• 要想第m层特征点和第7层一样大 该层空间点与相机的距离，即尺度不变性的最小物距dmin=dm*1.2^(m-7)
• 注意：上面变量与代码中变量的对应关系：
  7 <–> nLevels-1
  m <–> level
  1.2m <–> levelScaleFactor
  dmax <–> mfMaxDistance
  dmin <–> mfMinDistance

void MapPoint::UpdateNormalAndDepth()
{
    map<KeyFrame*,size_t> observations;
    KeyFrame* pRefKF;
    cv::Mat Pos;
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        if(mbBad)
            return;

        observations=mObservations; // 获得观测到该3d点的所有关键帧
        pRefKF=mpRefKF;             // 观测到该点的参考关键帧
        Pos = mWorldPos.clone();    // 3d点在世界坐标系中的位置
    }

    if(observations.empty())
        return;

    cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);
    int n=0;
    for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first;
        cv::Mat Owi = pKF->GetCameraCenter();
        cv::Mat normali = mWorldPos - Owi;
        normal = normal + normali/cv::norm(normali); // 对所有关键帧对该点的观测方向归一化为单位向量进行求和
        n++;
    } 

    cv::Mat PC = Pos - pRefKF->GetCameraCenter(); // 参考关键帧相机指向3D点的向量（在世界坐标系下的表示）
    const float dist = cv::norm(PC); // 该点到参考关键帧相机的距离
    const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;
    const float levelScaleFactor =  pRefKF->mvScaleFactors[level];
    const int nLevels = pRefKF->mnScaleLevels; // 金字塔层数

    {
        unique_lock<mutex> lock3(mMutexPos);
        // 另见PredictScale函数前的注释
        mfMaxDistance = dist*levelScaleFactor;                           // 观测到该点的距离下限
        mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1]; // 观测到该点的距离上限
        mNormalVector = normal/n;                                        // 获得平均的观测方向
    }
}

灰度质心法

• 指向亮区
• 以几何中心和灰度质心连线作为改特征点方向
• 图像梯度指向的角度
• 旋转不变性：给予一个定义于内积空间的函数，假若对于任意旋转，函数的参数值可能会改变，但是函数的数值仍旧保持不变，则称此性质为旋转不变性
• 一阶矩m10和m01用来确定图像的灰度中心
• 零阶矩m00表示图像灰度的总和
• 已知像素坐标 和像素灰度值函数可以得到定义特征点的方向角
• 

static float IC_Angle(const Mat& image, Point2f pt,  const vector<int> & u_max)
{
	//图像的矩，前者是按照图像块的y坐标加权，后者是按照图像块的x坐标加权
    int m_01 = 0, m_10 = 0;
 
	//获得这个特征点所在的图像块的中心点坐标灰度值的指针center
    const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x));
 
    // Treat the center line differently, v=0
	//这条v=0中心线的计算需要特殊对待
    //由于是中心行+若干行对，所以PATCH_SIZE应该是个奇数
    for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
		//注意这里的center下标u可以是负的！中心水平线上的像素按x坐标（也就是u坐标）加权
        m_10 += u * center[u];
 
    // Go line by line in the circular patch  
	//这里的step1表示这个图像一行包含的字节总数。参考[https://blog.csdn.net/qianqing13579/article/details/45318279]
    int step = (int)image.step1();
	//注意这里是以v=0中心线为对称轴，然后对称地每成对的两行之间进行遍历，这样处理加快了计算速度
    for (int v = 1; v <= HALF_PATCH_SIZE; ++v)
    {
        // Proceed over the two lines
		//本来m_01应该是一列一列地计算的，但是由于对称以及坐标x,y正负的原因，可以一次计算两行
        int v_sum = 0;
		// 获取某行像素横坐标的最大范围，注意这里的图像块是圆形的！
        int d = u_max[v];
		//在坐标范围内挨个像素遍历，实际是一次遍历2个
        // 假设每次处理的两个点坐标，中心线上方为(x,y),中心线下方为(x,-y) 
        // 对于某次待处理的两个点：m_10 = Σ x*I(x,y) =  x*I(x,y) + x*I(x,-y) = x*(I(x,y) + I(x,-y))
        // 对于某次待处理的两个点：m_01 = Σ y*I(x,y) =  y*I(x,y) - y*I(x,-y) = y*(I(x,y) - I(x,-y))
        for (int u = -d; u <= d; ++u)
        {
			//得到需要进行加运算和减运算的像素灰度值
			//val_plus：在中心线上方x=u时的的像素灰度值
            //val_minus：在中心线下方x=u时的像素灰度值
            int val_plus = center[u + v*step], val_minus = center[u - v*step];
			//在v（y轴）上，2行所有像素灰度值之差
            v_sum += (val_plus - val_minus);
			//u轴（也就是x轴）方向上用u坐标加权和（u坐标也有正负符号），相当于同时计算两行
            m_10 += u * (val_plus + val_minus);
        }
        //将这一行上的和按照y坐标加权
        m_01 += v * v_sum;
    }
 
    //为了加快速度还使用了fastAtan2()函数，输出为[0,360)角度，精度为0.3°
    return fastAtan2((float)m_01, (float)m_10);
}

非极大抑制

• 对于一个33（或55,7*7等奇数窗口）的窗口，如果存在多个特征点，则删除响应值较小的特征点，只保留响应值最大的特征点。

#include <opencv2\opencv.hpp>

using namespace cv;
using namespace std;

//局部极大值抑制，这里利用fast特征点的响应值做比较
void selectMax(int window, cv::Mat gray, std::vector<KeyPoint> & kp){

    //window是局部极大值抑制的窗口大小，r为半径
    int r = window / 2;
    if (window != 0){
        //对kp中的点进行局部极大值筛选
        for (int i = 0; i < kp.size(); i++){
            for (int j = i + 1; j < kp.size(); j++){
                //如果两个点的距离小于半径r，则删除其中响应值较小的点
                if (abs(kp[i].pt.x - kp[j].pt.x) + abs(kp[i].pt.y - kp[j].pt.y) <= 2 * r){
                    if (kp[i].response < kp[j].response){
                        std::vector<KeyPoint>::iterator it = kp.begin() + i;
                        kp.erase(it);
                        selectMax(window, gray, kp);
                    }
                    else{
                        std::vector<KeyPoint>::iterator it = kp.begin() + j;
                        kp.erase(it);
                        selectMax(window, gray, kp);
                    }
                }
            }
        }
    }

}

//
void fastpoint(cv::Mat gray, int threshold, int window, int pointNum, std::vector<KeyPoint> & kp){

    std::vector<KeyPoint> keypoint;

    cv::FastFeatureDetector fast(threshold, true);  //threshold 为阈值，越大，特征点越少
    fast.detect(gray, keypoint);  //fast特征检测
    if (keypoint.size() > pointNum){
        threshold = threshold + 5;
        fastpoint(gray, threshold, window, pointNum, keypoint);
    }
    selectMax(window, gray, keypoint);
    kp.assign(keypoint.begin(), keypoint.end());    //复制可以point到kp

}


int main(){
    cv::Mat img = cv::imread("D:/photo/06.jpg");
    cv::Mat gray;
    cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
    std::vector<KeyPoint> kp;
    int threshold = 30;  //fast阈值
    int window1 = 7;  //局部非极大值抑制窗口
    int pointMaxNum1 = 400; //特征点最大个数
    fastpoint(gray, threshold, window1, pointMaxNum1, kp);

    cv::Mat img2, gray2;
    std::vector<KeyPoint> kp2;
    img.copyTo(img2);
    gray.copyTo(gray2);
    int window2 = 15;  //局部非极大值抑制窗口
    int pointMaxNum2 = 400; //特征点最大个数
    fastpoint(gray2, threshold, window2, pointMaxNum2, kp2);

    cv::drawKeypoints(img, kp, img, Scalar(0, 0, 255));
    cv::drawKeypoints(img2, kp2, img2, Scalar(255, 0, 0));

    cv::imwrite("D:/photo/06_1.jpg", img);
    cv::namedWindow("img", cv::WINDOW_NORMAL);
    cv::imshow("img", img);

    cv::imwrite("D:/photo/06_2.jpg", img2);
    cv::namedWindow("img2", cv::WINDOW_NORMAL);
    cv::imshow("img2", img2);

    cv::waitKey(0);

    system("pause");
    return 0;
}

描述子：BRIEF

• 二进制描述子 n 维的向量
• 关键点附近随机两个像素pq，如果p比q大，取1，反之，取0
• 取128维的向量
• 已知关键点的方向信息

如何提取特征点

ORB特征提取

• FAST 角点提取：找出图像中的角点 orb中计算了特征的主方向 增加了旋转不变特性，有利于后续的描述子计算
• BRIEF 描述子:对前一步提取出特征点的周围图像区域进行描述

4.如何进行特征匹配/数据关联

-通过对描述子进行匹配，为后续的姿态估计，优化等减轻负担。

暴力匹配

• 使用与特征点数量少-快速近似最近邻(FLANN)算法-多
• 两个不同时刻特征点
• 图像 It中提取到特征点 xm t,m = 1, 2, …, M
• 图像 It+1中提取到特征点 xn t+1,n = 1, 2, …, N,
• 两组测量描述子的距离（两个二进制字符串的明汉距离）-表示了两个特征的相似程度

• 明汉距离

• -两个二进制串之间的汉明距离,指的是它们不同位数的个数。
• 然后排序
• 取最近的作为匹配点

快速近似最近邻(FLANN)算法

• 适合于匹配点数量极多的情况
• 加速

4.如何匹配多图特征点（实践）

• 特征点匹配代码

#include <iostream>
//引入opencv核心块
#include <opencv2/core/core.hpp>
//下面两个是显示模块
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace std;
using namespace cv;

int main ( int argc, char** argv )
{
    if ( argc != 3 )
    {
        cout<<"usage: feature_extraction img1 img2"<<endl;
        return 1;
    }
    //-- 读取两张图像 进行特征匹配
    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

    //-- 初始化
    std::vector<KeyPoint> keypoints_1, keypoints_2;
    Mat descriptors_1, descriptors_2;
    //申请关键点
    Ptr<FeatureDetector> detector = ORB::create();
    //申请描述子
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // Ptr<FeatureDetector> detector = FeatureDetector::create(detector_name);
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create(descriptor_name);
    //申请匹配DescriptorMatcher 距离度量范数create 使用汉明距离"BruteForce-Hamming"
    Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );

    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect ( img_1,keypoints_1 );
    detector->detect ( img_2,keypoints_2 );

    //-- 第二步:根据角点位置计算 BRIEF 描述子
    descriptor->compute ( img_1, keypoints_1, descriptors_1 );
    descriptor->compute ( img_2, keypoints_2, descriptors_2 );

    Mat outimg1;
    drawKeypoints( img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT );
    imshow("ORB特征点",outimg1);

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> matches;
    //BFMatcher matcher ( NORM_HAMMING );
    matcher->match ( descriptors_1, descriptors_2, matches );

    //-- 第四步:匹配点对筛选
    double min_dist=10000, max_dist=0;

    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        double dist = matches[i].distance;
        if ( dist < min_dist ) min_dist = dist;
        if ( dist > max_dist ) max_dist = dist;
    }
    
    // 仅供娱乐的写法
    min_dist = min_element( matches.begin(), matches.end(), [](const DMatch& m1, const DMatch& m2) {return m1.distance<m2.distance;} )->distance;
    max_dist = max_element( matches.begin(), matches.end(), [](const DMatch& m1, const DMatch& m2) {return m1.distance<m2.distance;} )->distance;

    printf ( "-- Max dist : %f \n", max_dist );
    printf ( "-- Min dist : %f \n", min_dist );

    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    std::vector< DMatch > good_matches;
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        if ( matches[i].distance <= max ( 2*min_dist, 30.0 ) )
        {
            good_matches.push_back ( matches[i] );
        }
    }

    //-- 第五步:绘制匹配结果
    Mat img_match;
    Mat img_goodmatch;
    drawMatches ( img_1, keypoints_1, img_2, keypoints_2, matches, img_match );
    drawMatches ( img_1, keypoints_1, img_2, keypoints_2, good_matches, img_goodmatch );
    imshow ( "所有匹配点对", img_match );
    imshow ( "优化后匹配点对", img_goodmatch );
    waitKey(0);

    return 0;
}

• build/feature_extraction 1.png 2.pngbuild/特征点匹配执行程序 图片1 图片2
• waitkey(0)无效问题 在这里插入代码片system("pause");重新make

5.如何恢复图像之间摄像机的三维运动-对极几何原理（单目）

解决问题-利用对极几何约束 恢复图像之间摄像机的三维运动

6.如何求解摄像机的三维运动-pnp问题

解决-利用已知三维结构与图像的关系 求解摄像机的三维运动

7.如何求解摄像机三维运动-icp问题（双目，RGB-D，已知距离信息）

解决-利用点云匹配关系 求解摄像机三维运动

8.获得二维图像上对应的三维结构-三角化

解决-可以获得二维图像上对应的三维结构