ORB与CCM-SLAM

入口函数

mono_euroc.cc

int main(int argc, char **argv)
    
// 图像序列的文件名字符串序列
    vector<string> vstrImageFilenames;
    // 时间戳
    vector<double> vTimestamps;

    LoadImages(string(argv[3]),             // path_to_image_folder
               string(argv[4]),             // path_to_times_file
               vstrImageFilenames,          // 读取到的图像名称数组
               vTimestamps);   

    ORB_SLAM2::System SLAM(
        argv[1],                            // path_to_vocabulary
        argv[2],                            // path_to_settings
        ORB_SLAM2::System::MONOCULAR,       // 单目模式
        true);   

system.cc

  

System::System(......)

    mpVocabulary = new ORBVocabulary();

    mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);

    mpMap = new Map();

    mpFrameDrawer = new FrameDrawer(mpMap);

    mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);

 mpTracker = new Tracking(this,						//现在还不是很明白为什么这里还需要一个this指针  
    						 mpVocabulary,				//字典
    						 mpFrameDrawer, 			//帧绘制器
    						 mpMapDrawer,				//地图绘制器
                             mpMap, 					//地图
                             mpKeyFrameDatabase, 		//关键帧地图
                             strSettingsFile, 			//设置文件路径
                             mSensor);					//传感器类型iomanip

Tracking.cc


  

Tracking::Tracking(.....)

//读入配置文件参数
。。。。。。。。。。。。。

    mpORBextractorLeft = new ORBextractor(
        nFeatures,      //参数的含义还是看上面的注释吧
        fScaleFactor,
        nLevels,
        fIniThFAST,
        fMinThFAST);

ORBextractor.cc
  

首先声明一些容器的size，

ORBextractor::ORBextractor(。。。。。)

    mvScaleFactor.resize(nlevels);  
	//存储这个sigma^2，其实就是每层图像相对初始图像缩放因子的平方
    mvLevelSigma2.resize(nlevels);
	//对于初始图像，这两个参数都是1
    mvScaleFactor[0]=1.0f;
    mvLevelSigma2[0]=1.0f;


	//然后逐层计算图像金字塔中图像相当于初始图像的缩放系数 
    for(int i=1; i<nlevels; i++)  
    {
		//其实就是这样累乘计算得出来的
        mvScaleFactor[i]=mvScaleFactor[i-1]*scaleFactor;
		//原来这里的sigma^2就是每层图像相对于初始图像缩放因子的平方
        mvLevelSigma2[i]=mvScaleFactor[i]*mvScaleFactor[i];
    }


    //接下来的两个向量保存上面的参数的倒数
    mvInvScaleFactor.resize(nlevels);
    mvInvLevelSigma2.resize(nlevels);
    for(int i=0; i<nlevels; i++)
    {
        mvInvScaleFactor[i]=1.0f/mvScaleFactor[i];
        mvInvLevelSigma2[i]=1.0f/mvLevelSigma2[i];
    }


    //调整图像金字塔vector以使得其符合设定的图像层数
    mvImagePyramid.resize(nlevels);

	//每层需要提取出来的特征点个数，这个向量也要根据图像金字塔设定的层数进行调整
    mnFeaturesPerLevel.resize(nlevels);

//

构建图像金字塔，计算每一层图像需要分配的特征点个数

//

之后：

//这里的512表示512个点（上面的数组中是存储的坐标所以是256*2*2）
    const int npoints = 512;
注意到pattern0数据类型为Points*,bit_pattern_31_是int[]型，所以这里需要进行强制类型转换
    const Point* pattern0 = (const Point*)bit_pattern_31_;	
	//使用std::back_inserter的目的是可以快覆盖掉这个容器pattern之前的数据
	//其实这里的操作就是，将在全局变量区域的、int格式的随机采样点以cv::point格式复制到当前类对象中的成员变量中
    std::copy(pattern0, pattern0 + npoints, std::back_inserter(pattern));

之后，画一个园，以对称的方式计算园边界每一个点的（x，y）坐标，用于后面计算方向

std::vector<int> umax;	                 ///<计算特征点方向的时候，有个圆形的图像区域，这个vector中存储了每行u轴的边界（四分之一，其他部分通过对称获得）
umax.resize(HALF_PATCH_SIZE + 1);

至此

ORBextractor::ORBextractor(。。。。。)

结束

返回Tracking.cc

 // 在单目初始化的时候，会用mpIniORBextractor来作为特征点提取器
if(sensor==System::MONOCULAR)
        mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);

至此

Tracking::Tracking(.....)

结束

回到

mono_euroc.cc

 统计追踪一帧耗时 (仅Tracker线程)  

    vector<float> vTimesTrack;
    vTimesTrack.resize(nImages);

// Main loop
// step 4 依次追踪序列中的每一张图像  

    cv::Mat im;
    for(int ni=0; ni<nImages; ni++)
    {
        // Read image from file
        // step 4.1 读根据前面获得的图像文件名读取图像,读取过程中不改变图像的格式 
        im = cv::imread(vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);
        double tframe = vTimestamps[ni];

        // Pass the image to the SLAM system
        // step 4.4 追踪当前图像
        SLAM.TrackMonocular(im,tframe);

System.cc

cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp)

选择模式。是否关闭建图线程，仅追踪

 cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);

// Step 1 ：将彩色图像转为灰度图像  

// Step 2 ：构造Frame
//判断该帧是不是初始化  

    if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET) //没有成功初始化的前一个状态就是NO_IMAGES_YET
        mCurrentFrame = Frame(
            mImGray,
            timestamp,
            mpIniORBextractor,      //初始化ORB特征点提取器会提取2倍的指定特征点数目
            mpORBVocabulary,
            mK,
            mDistCoef,
            mbf,
            mThDepth);
    else
        mCurrentFrame = Frame(
            mImGray,
            timestamp,
            mpORBextractorLeft,     //正常运行的时的ORB特征点提取器，提取指定数目特征点
            mpORBVocabulary,
            mK,
            mDistCoef,
            mbf,
            mThDepth);

Frame.cc

Frame::Frame(.....)  

// Step 1 帧的ID 自增  

// Step 2 计算图像金字塔的参数   

// Step 3 对这个单目图像进行提取特征点, 第一个参数0-左图， 1-右图  

    ExtractORB(0,imGray);

void Frame::ExtractORB(int flag, const cv::Mat &im)
{
    // 判断是左图还是右图
    if(flag==0)
        // 左图的话就套使用左图指定的特征点提取器，并将提取结果保存到对应的变量中 
        // 这里使用了仿函数来完成，重载了括号运算符 ORBextractor::operator() 
        (*mpORBextractorLeft)(im,				//待提取特征点的图像
							  cv::Mat(),		//掩摸图像, 实际没有用到
							  mvKeys,			//输出变量，用于保存提取后的特征点
							  mDescriptors);	//输出变量，用于保存特征点的描述子
    else
        // 右图的话就需要使用右图指定的特征点提取器，并将提取结果保存到对应的变量中 
        (*mpORBextractorRight)(im,cv::Mat(),mvKeysRight,mDescriptorsRight);
}

ORBextractor.cc

仿函数

void ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints,
                      OutputArray _descriptors)

    Mat image = _image.getMat();

OutputArray是opencv源码中常用到的类型，类型大概是用来表示为Mat和vector<>的。

而getMat（）函数即是将OutputArray数据转换成Mat类型。

//判断图像的格式是否正确，要求是单通道灰度值
   assert(image.type() == CV_8UC1 );  

assert宏的原型定义在<assert.h>中，其作用是如果它的条件返回错误，则终止程序执行，原型定义：

  

// Step 2 构建图像金字塔 

   ComputePyramid(image);

void ORBextractor::ComputePyramid(cv::Mat image)

   // Step 3 计算图像的特征点，并且将特征点进行均匀化。均匀的特征点可以提高位姿计算精度
// 存储所有的特征点，注意此处为二维的vector，第一维存储的是金字塔的层数，第二维存储的是那一层金字塔图像里提取的所有特征点  

    vector < vector<KeyPoint> > allKeypoints; 
    //使用四叉树的方式计算每层图像的特征点并进行分配
    ComputeKeyPointsOctTree(allKeypoints);

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

每一层划分网格，提取每一个网格的特征点，把当前图层绝对坐标存在

vToDistributeKeys中，  

        keypoints = DistributeOctTree(vToDistributeKeys, 			//当前图层提取出来的特征点，也即是等待剔除的特征点
																	//NOTICE 注意此时特征点所使用的坐标都是在“半径扩充图像”下的
									  minBorderX, maxBorderX,		//当前图层图像的边界，而这里的坐标却都是在“边缘扩充图像”下的
                                      minBorderY, maxBorderY,
									  mnFeaturesPerLevel[level], 	//希望保留下来的当前层图像的特征点个数
									  level);						//当前层图像所在的图层

函数 DistributeOctTree() 进行八叉树筛选(非极大值抑制),不断将存在特征点的图像区域进行4等分,直到分出了足够多的分区,每个分区内只保留响应值最大的特征点.
其代码实现比较琐碎,程序里还定义了一个 ExtractorNode 类用于进行八叉树分配

// compute orientations
//然后计算这些特征点的方向信息，注意这里还是分层计算的  

    for (int level = 0; level < nlevels; ++level)
        computeOrientation(mvImagePyramid[level],	//对应的图层的图像
						   allKeypoints[level], 	//这个图层中提取并保留下来的特征点容器
						   umax);		

// Step 4 拷贝图像描述子到新的矩阵descriptors  

	//统计整个图像金字塔中的特征点
    int nkeypoints = 0;
	//开始遍历每层图像金字塔，并且累加每层的特征点个数
    for (int level = 0; level < nlevels; ++level)
        nkeypoints += (int)allKeypoints[level].size();

        _descriptors.create(nkeypoints,		//矩阵的行数，对应为特征点的总个数
							32, 			//矩阵的列数，对应为使用32*8=256位描述子
							CV_8U);			//矩阵元素的格式

//  Step 5 对图像进行高斯模糊  

        GaussianBlur(workingMat, 		//源图像
					 workingMat, 		//输出图像
					 Size(7, 7), 		//高斯滤波器kernel大小，必须为正的奇数
					 2, 				//高斯滤波在x方向的标准差
					 2, 				//高斯滤波在y方向的标准差
					 BORDER_REFLECT_101);//边缘拓展点插值类型

// Step 6 计算高斯模糊后图像的描述子  

        computeDescriptors(workingMat, 	//高斯模糊之后的图层图像
						   keypoints, 	//当前图层中的特征点集合
						   desc, 		//存储计算之后的描述子
						   pattern);	//随机采样模板

 退出至System.cc

进入frame

// Step 4 用OpenCV的矫正函数、内参对提取到的特征点进行矫正   

UndistortKeyPoints();

    // 初始化本帧的地图点
    mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
	// 记录地图点是否为外点，初始化均为外点false
    mvbOutlier = vector<bool>(N,false);

//  Step 5 计算去畸变后图像边界，将特征点分配到网格中。这个过程一般是在第一帧或者是相机标定参数发生变化之后进行  

        ComputeImageBounds(imGray);