SLAM学习----特征点与特征匹配

slam 代码 学习（一）

---

int main()
{
    ParameterReader parameterReader;
    Tracker::Ptr    tracker( new Tracker(parameterReader) );
    FrameReader     frameReader( parameterReader );
    PoseGraph       poseGraph( parameterReader, tracker );
    Mapper          mapper( parameterReader, poseGraph );//这里是初始化构图，分配线程
}

---

class Mapper
{
public:
    Mapper( const ParameterReader& para, PoseGraph& graph )
        : parameterReader( para ), poseGraph( graph )
//C++构造函数初始化列表，以一个冒号开始，接着是以逗号分隔的数据成员列表，每个数据成员后面跟一个放在括号中的初始化式
    {
        resolution = para.getData<double>("mapper_resolution");//para是parameterReader类
        max_distance = para.getData<double>("mapper_max_distance");//这里是获得参数
        viewerThread = make_shared<thread> ( bind( &Mapper::viewer, this ));//这里是线程
    }
    void shutdown()
    {
        shutdownFlag = true;
        viewerThread->join();
    }
    // viewer线程
    void viewer();
protected:
    // viewer thread
    shared_ptr<thread>      viewerThread = nullptr;
    const ParameterReader& parameterReader;  //初始化列表的数据类型定义
    PoseGraph&  poseGraph;
};

/*shared_ptr可以指向特定类型的对象，用于自动释放所指的对象.eg: */  
    shared_ptr<CDlgPointCloud> PointCloudDlgPointer;
//指向类型为CDlgPointCloud的对象PointCloudDlgPointer;
/* 还有一个最安全的分配和使用动态内存的方法就是调用一个名为make_shared的标准库函数；
make_shared 在动态内存中分配一个对象并初始化，返回指向此对象的shared_ptr，make_shared与智能指针定义在头文件memory中；当要用make_shared时，必须指定想要创建的对象类型，定义方式与模板类相同，在函数名之后跟一个尖括号，在其中给出类型；
如 make_shared<int> p3 = make_shared<int>(42)；*/

pcl::visualization::CloudViewer Class Reference
Simple point cloud visualization class. 点云可视化类
#include <pcl/visualization/cloud_viewer.h>

pcl::visualization::CloudViewer::CloudViewer    ( const std::string & window_name)  
//Construct a cloud viewer, with a window name. 构建一个点云窗口
Parameters //参数是窗口的名字
    window_name This is displayed at the top of the window 

void Mapper::viewer() //多线程 --- 建图函数
{
    pcl::visualization::CloudViewer viewer("viewer");//构建点云可视化窗口
    PointCloud::Ptr globalMap (new PointCloud);

    pcl::VoxelGrid<PointT>  voxel;
    voxel.setLeafSize( resolution, resolution, resolution );

    while (shutdownFlag == false)
    {
        static int cntGlobalUpdate = 0;
        if ( poseGraph.keyframes.size() <= this->keyframe_size )
        {
            usleep(1000);
            continue;
        }
        // keyframe is updated
        PointCloud::Ptr tmp(new PointCloud());
        if (cntGlobalUpdate % 15 == 0)
        {
            // update all frames
            cout<<"redrawing frames"<<endl;
            globalMap->clear();
            for ( int i=0; i<poseGraph.keyframes.size(); i+=2 )
            {
                PointCloud::Ptr cloud = this->generatePointCloud(poseGraph.keyframes[i]);
                *globalMap += *cloud;
            }
        }
        else
        {
            for ( int i=poseGraph.keyframes.size()-1; i>=0 && i>poseGraph.keyframes.size()-6; i-- )
            {
                PointCloud::Ptr cloud = this->generatePointCloud(poseGraph.keyframes[i]);
                *globalMap += *cloud;
            }
        }

        cntGlobalUpdate ++ ;
        //voxel
        voxel.setInputCloud( globalMap );
        voxel.filter( *tmp );

        keyframe_size = poseGraph.keyframes.size();
        globalMap->swap( *tmp );
        viewer.showCloud( globalMap );

        cout<<"points in global map: "<<globalMap->points.size()<<endl;
    }
}

特征提取与特征匹配

using namespace std;
int main()
{
    cout<<"running test pnp"<<endl;
    ParameterReader para;
    FrameReader frameReader( para );
    OrbFeature  orb(para);
    PnPSolver   pnp(para, orb);

    RGBDFrame::Ptr refFrame = frameReader.next();
    //RGBD::Ptr frame； // 帧变量，使用一个智能指针创建一个空的帧，这种指针用完会自动释放其内存空间
    orb.detectFeatures( refFrame );
}

orb.detectFeatures( refFrame );

// 提取特征，存放到frame的成员变量中
    // 这里每次定义一个新的帧--基本单位，都是通过RGBDFrame::Ptr 定义一个智能指针

    void detectFeatures( rgbd_tutor::RGBDFrame::Ptr& frame ) const
    { //此处将frame 基本单位帧  传递过来了
        cv::Mat gray = frame->rgb;//RGB图像

        if (frame->rgb.channels() == 3)//是三通道-->转换为灰度图
        {
            // The BGR image
            //C++: void cvtColor(InputArray src, OutputArray dst, int code, int dstCn=0 )
            cv::cvtColor( frame->rgb, gray, cv::COLOR_BGR2GRAY );
            //灰度图是只含有黑白颜色，和0~255亮度等级的图片
            /*
             * 灰度是指只含亮度信息，不含色彩信息的图像。黑白照片就是灰度图，特点是亮度由暗到明，变化是连续的。
             * 要表示灰度图，就需要把亮度值进行量化;;;使用灰度图的好处：
             * RGB的值都一样。。。。图像数据即调色板索引值，就是实际的RGB值，也就是亮度值。
             * 因为是256色调色板，所以图像数据中一个字节代表一个像素，很整齐。所以，做图像处理时都采用灰度图。
             * */
        }
/*
  KeyPoint 类
  KeyPoint类主要是存储图像中检测到的关键点(特征点)，特征点主要是由特征检测器在图像中进行检测得到的，常见的
  特征检测器有：Harris corner detector, cv::FAST, cv::StarDetector, cv::SURF, cv::SIFT, cv::LDetector等。
  关键点主要是由二维特征，尺度(与需要考虑的邻域直径成正比)，方向和一些其他的参数。关键点邻域是由生成
  描述子(通常被表示成一个特征向量)的算法分析得到的。在不同图像中表示相同对象的关键点能通过cv::KDTree
  或其他方法匹配到。
*/
        vector<cv::KeyPoint>    keypoint1;
        /*声明一个cv::KeyPoint类型的向量-->动态的序列容器-->"size可变的数组"*/

// kps[0].size size是关键点的邻域直径 ;
// kps[0].class_id class_id 若关键点被聚类的话,class_id是关键点所在聚类的id
// kps[0].angle ;angle是关键点的方向，在[0,360]度之间，方向与图像坐标系有关，例如顺时针
// kps[0].pt ;pt是关键点的坐标
// kps[0].octave ;octave是关键点提取所在的金子塔层数
// kps[0].response ;response是最强特征点被选择的响应，可用来进行更近一步的排序或子采样
        cv::Mat     descriptors1;

        (*extractor) ( gray, cv::Mat(), keypoint1, descriptors1);//这里执行重载函数
        //Compute the ORB features and descriptors on an image.
        //ORB are dispersed on the image using an octree.
        //Mask is ignored in the current implementation.
        //重载了()运算符，作为提取器的对外接口

        // 这里经过计算后;在线GDB调试第一张图:得到302个关键点+302个描述子;
        // 在ORB的代码里每个描述子又分别:descriptors = Mat::zeros((int)keypoints.size(), 32, CV_8UC1);
        // 所以描述子的矩阵大小为 302(rows)*32(cols)
        /**void operator()( cv::InputArray image, cv::InputArray mask,
             std::vector<cv::KeyPoint>& keypoints,
             cv::OutputArray descriptors);**/

        //这里的关键点的size()--->得到容器keypoint1的大小
        /*声明一个cv::KeyPoint类型的向量-->动态的序列容器-->"size可变的数组"*/
        for ( size_t i=0; i<keypoint1.size(); i++ )
        {
            rgbd_tutor::Feature feature;
            feature.keypoint = keypoint1[i];
            feature.descriptor  = descriptors1.row(i).clone();
            /* 实际上特征点的每个描述子只是矩阵302*32中的一行数据--1*32 */
            feature.position = frame->project2dTo3d( keypoint1[i].pt.x, keypoint1[i].pt.y );
            //根据2D的像素坐标计算得到三维坐标  其中三维坐标z由尺度和深度图得到
            frame->features.push_back( feature );
        }
    }

ORBextractor函数 特征提取

ORBextractor类中主要调用OpenCV中的函数，提取图像中特征点（关键点及其描述，描述子，以及图像金字塔）

参考TUM1.yaml文件中的参数，每一帧图像共提取1000个特征点，分布在金字塔8层中，层间尺度比例1.2，计算下来金字塔0层大约有217个特征点，7层大约有50个特征点。这样有一个比较直观的概念。

提取特征点使用FAST，但是ORB中的FAST加入了旋转信息，也就是去计算特征点的角度，同时加入了尺度信息，也就是计算在多层金字塔中去提取。

描述子使用的是BRIEF，通过二进制BRIEF描述子之间的汉明距离来考察两个特征点之间的相似度。

整个提取过程使用重载了的()操作符来完成。

shared_ptr<ORB_SLAM2::ORBextractor> extractor;

//上面应用了此处的重载运算符
(*extractor) ( gray, cv::Mat(), keypoint1, descriptors1);//这里执行重载函数

最后为了提取出的特征点在图像中分布比较均匀（实际情况中，特征点通常分布得比较集中，这样不利于进行匹配，也不利于精确地求解相机间的位姿从而得到精确的VO轨迹），使用了八叉树（其实是平面上的四叉树）的数据结构来存储提取出的特征点：

void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoints);

金字塔中每一层提取出的特征点放在不同的vector<KeyPoint>中；OctTree主要的实现：

vector<cv::KeyPoint> ORBextractor::DistributeOctTree
(const vector<cv::KeyPoint>& vToDistributeKeys, const int &minX,
 const int &maxX, const int &minY, const int &maxY, const int &N, const int &level);

该树结构除了根节点其实只实现了3层，最顶层的node数量由图像的横纵比决定;下面两层最多产生64个叶子。因此，对于前面提到的特征点数，平均每个分割节点中分布一两个特征点，如果该叶子中含有较多特征点，则选取其中Harris响应值（是由OpenCV的KeyPoint.response属性计算的）最大的，其他的抛弃！