大神一步步教你读懂ORB算法，赞！！

工作就没有在学校时间上有那么自由了，最近出差了快一个月，博客也就落下了。现在开始一点点的来学习orb-slam2，将自己的学习过程写出，望大家指正批评。
至于为什么学习orb-slam2，主要这比较完整的实现了slam的整个过程，论文发表在IEEE Transactions on Robotics有一定的参考价值，关键作者较完整的开源，这样在论文代码都有的情况下学习起来就轻松了许多。
首先我们先看一下算法的整个流程

下面的这个系列也就根据这个流程一步步的实现。

特征检测

首先第一步特征检测，作者采用ORB特征子，这里我不详细叙述orb特征的一个实现过程，主要我对很多的细节也不是很明白，具体可以参考论文,我主要对ORB-SLAM作者对特征进行均分部分进行简单叙述，以及和opencv自带orb进行对比。

说白了特征提取也就是对图像进行一定的操作，也就是对像素点进行一些操作，跟相邻的一些像素点进行比较，通过一些模板进行滤波卷积等操作，再通过阈值进行一些控制，找到了可以代表该图像的某些位置，这也就是特征提取。

那下一步进行特征提取,orb-slam中，作者对opencv中的orb源码进行了修改，将特征进行均匀化。具体调用和opencv一致，也是重载了函数调用操作符operator()。
那下面就构建这个类

123456789101112131415161718192021222324252627282930313233343536373839404142434445

class  ORBextractor{public:ORBextractor(int features_num = 500, float scale_factor = 1.2f, int levels_num = 8,int default_fast_threshold = 20, int min_fast_threshold = 7);~ORBextractor(){}/** \brief 计算图像的orb特征及描述，将orb特征分配到一个四叉树当中*  目前mask参数是被忽略的，没有实现*/void operator()(cv::InputArray image, cv::InputArray mask,std::vector<cv::KeyPoint>& keypoints,cv::OutputArray descriptors);protected:/** \brief 计算图像金字塔*/void computePyramid(cv::Mat image);/** \brief 通过四叉树的方式计算特征点*/void computeKeyPointsQuadTree(std::vector<std::vector<cv::KeyPoint> >& all_keypoints);/** \brief 通过四叉树的方式分配特征点*/std::vector<cv::KeyPoint> distributeQuadTree(const std::vector<cv::KeyPoint>& vec_to_distribute_keys, const int &min_x,const int &max_x, const int &min_y, const int &max_y, const int &feature_num, const int &level);public:std::vector<cv::Mat> vec_image_pyramid_;//!<图像金字塔protected:std::vector<cv::Point> pattern_;//!<用于存放训练的模板int features_num_;//!<最多提取的特征点的数量float scale_factor_;//!<金字塔图像之间的尺度参数int levels_num_;//!<高斯金字塔的层数int default_fast_threshold_;//!<默认设置fast角点阈值20int min_fast_threshold_;//!<设置fast角点阈值为9std::vector<int> feature_num_per_level_;//!<每层特征的个数std::vector<int> umax_;//!< 用于存储计算特征方向时，图像每个v坐标对应最大的u坐标std::vector<float> vec_scale_factor_;//!<用于存储每层的尺度因子};

构造函数

通过构造函数ORBextractor(int features_num = 500, float scale_factor = 1.2f, int levels_num = 8, int default_fast_threshold = 20, int min_fast_threshold = 7);传入features_num最多提取的特征点的数量，scale_factor金字塔图像之间的尺度参数，levels_num金字塔的层数，default_fast_threshold默认fast角点检测的时候的阈值，为了防止用默认阈值fast角点检测检测的特征数过少，添加设置min_fast_threshold最小的fast特征检测阈值，以保证检测的特征数目。

在构造函数中，首先先初始化每层的尺度因子待用！

1234567

// 这边将每层金字塔对应的尺度因子给出vec_scale_factor_.resize(levels_num_);vec_scale_factor_[0] = 1.0f;for (int i = 1; i < levels_num_; i++){vec_scale_factor_[i] = vec_scale_factor_[i - 1] * scale_factor_;}

这边去掉了作者初始化构建尺度因子的倒数，平方，平方的倒数，因为后期调用不多，显得冗余去掉了。

接下来给每层分配待提取的特征数，具体通过等比数列求和的方式，求出每一层应该提取的特征数

123456789101112

feature_num_per_level_.resize(levels_num_);float factor = 1.0f / scale_factor_;float desired_features_per_scale = features_num_*(1 - factor) / (1 - (float)pow((double)factor, (double)levels_num_));int sum_features = 0;for (int level = 0; level < levels_num_ - 1; level++){feature_num_per_level_[level] = cvRound(desired_features_per_scale);sum_features += feature_num_per_level_[level];desired_features_per_scale *= factor;}feature_num_per_level_[levels_num_ - 1] = std::max(features_num_ - sum_features, 0);

接下来做一些初始化，用于计算特征的方向和描述

1234567891011121314151617181920212223

// 复制训练的模板const int npoints = 512;const cv::Point* pattern0 = (const cv::Point*)bit_pattern_31_;std::copy(pattern0, pattern0 + npoints, std::back_inserter(pattern_));//用于计算特征方向时，每个v坐标对应最大的u坐标umax_.resize(HALF_PATCH_SIZE + 1);// 将v坐标划分为两部分进行计算，主要为了确保计算特征主方向的时候，x,y方向对称int v, v0, vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1);int vmin = cvCeil(HALF_PATCH_SIZE * sqrt(2.f) / 2);// 通过勾股定理计算const double hp2 = HALF_PATCH_SIZE*HALF_PATCH_SIZE;for (v = 0; v <= vmax; ++v)umax_[v] = cvRound(sqrt(hp2 - v * v));// 确保对称，即保证是一个圆for (v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v){while (umax_[v0] == umax_[v0 + 1])++v0;umax_[v] = v0;++v0;}

好，上面就是构造函数部分，使用的时候实例化对象就好，那下一步就涉及调用，这重载了函数调用操作符operator()，那就看看函数调用。

函数调用

构建图像金字塔

首先构建图像金字塔，主要就是根据尺度因子对图像进行缩放处理

123456789101112131415161718

void ORBextractor::computePyramid(cv::Mat image){for (int level = 0; level < levels_num_; ++level){float scale = 1.0f / vec_scale_factor_[level];cv::Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale));if (level != 0){resize(vec_image_pyramid_[level - 1], vec_image_pyramid_[level], sz, 0, 0, cv::INTER_LINEAR);}else{vec_image_pyramid_[level] = image;}}}

计算特征点

下一步就是计算特征点，作者通过四叉树的方式对特征划分，以保证特征的均匀。

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091

void ORBextractor::computeKeyPointsQuadTree(std::vector<std::vector<cv::KeyPoint> >& all_keypoints){all_keypoints.resize(levels_num_);// 设置格子大小const float border_width = 30;for (int level = 0; level < levels_num_; ++level){// 得到每一层图像进行特征检测区域上下两个坐标const int min_border_x = EDGE_THRESHOLD - 3;const int min_border_y = min_border_x;const int max_border_x = vec_image_pyramid_[level].cols - EDGE_THRESHOLD + 3;const int max_border_y = vec_image_pyramid_[level].rows - EDGE_THRESHOLD + 3;// 用于分配的关键点std::vector<cv::KeyPoint> vec_to_distribute_keys;vec_to_distribute_keys.reserve(features_num_ * 10);const float width = (max_border_x - min_border_x);const float height = (max_border_y - min_border_y);// 将待检测区域划分为格子的行列数const int cols = width / border_width;const int rows = height / border_width;// 重新计算每个格子的大小const int width_cell = ceil(width / cols);const int height_cell = ceil(height / rows);// 在每个格子内进行fast特征检测for (int i = 0; i < rows; i++){const float ini_y = min_border_y + i*height_cell;float max_y = ini_y + height_cell + 6;if (ini_y >= max_border_y - 3)continue;if (max_y > max_border_y)max_y = max_border_y;for (int j = 0; j < cols; j++){const float ini_x = min_border_x + j*width_cell;float max_x = ini_x + width_cell + 6;if (ini_x >= max_border_x - 6)continue;if (max_x > max_border_x)max_x = max_border_x;std::vector<cv::KeyPoint> vec_keys_cell;cv::FAST(vec_image_pyramid_[level].rowRange(ini_y, max_y).colRange(ini_x, max_x),vec_keys_cell, default_fast_threshold_, true);// 如果检测到的fast特征为空，则降低阈值再进行检测if (vec_keys_cell.empty()){cv::FAST(vec_image_pyramid_[level].rowRange(ini_y, max_y).colRange(ini_x, max_x),vec_keys_cell, min_fast_threshold_, true);}// 计算实际特征点的位置if (!vec_keys_cell.empty()){for (std::vector<cv::KeyPoint>::iterator vit = vec_keys_cell.begin(); vit != vec_keys_cell.end(); vit++){(*vit).pt.x += j*width_cell;(*vit).pt.y += i*height_cell;vec_to_distribute_keys.push_back(*vit);}}}}std::vector<cv::KeyPoint> & keypoints = all_keypoints[level];keypoints.reserve(features_num_);// 将特征点进行四叉树划分keypoints = distributeQuadTree(vec_to_distribute_keys, min_border_x, max_border_x,min_border_y, max_border_y, feature_num_per_level_[level], level);const int scaled_patch_size = PATCH_SIZE*vec_scale_factor_[level];// 换算特征点真实位置（添加边界值），添加特征点的尺度信息const int nkps = keypoints.size();for (int i = 0; i < nkps; i++){keypoints[i].pt.x += min_border_x;keypoints[i].pt.y += min_border_y;keypoints[i].octave = level;keypoints[i].size = scaled_patch_size;}}// 计算特征点的方向for (int level = 0; level < levels_num_; ++level)computeOrientation(vec_image_pyramid_[level], all_keypoints[level], umax_);}

主要就是划分格子，在不同的尺度下，每个格子进行Fast特征检测，接下来就是将图像划分成四叉树形式，根据这一层的特征数确定四叉树的节点，将这一层图像检测到的特征划分到这些节点，保证每个节点里面有一个特征。

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222

std::vector<cv::KeyPoint> ORBextractor::distributeQuadTree(const std::vector<cv::KeyPoint>& vec_to_distribute_keys, const int &min_x,const int &max_x, const int &min_y, const int &max_y, const int &feature_num, const int &level){// 计算初始时有几个节点const int init_node_num = round(static_cast<float>(max_x - min_x) / (max_y - min_y));// 得到节点之间的间隔const float interval_x = static_cast<float>(max_x - min_x) / init_node_num;std::vector<ExtractorNode*> init_nodes;init_nodes.resize(init_node_num);// 划分之后包含的节点std::list<ExtractorNode> list_nodes;for (int i = 0; i < init_node_num; i++){ExtractorNode ni;ni.UL_ = cv::Point2i(interval_x*static_cast<float>(i), 0);ni.UR_ = cv::Point2i(interval_x*static_cast<float>(i + 1), 0);ni.BL_ = cv::Point2i(ni.UL_.x, max_y - min_y);ni.BR_ = cv::Point2i(ni.UR_.x, max_y - min_y);ni.vec_keys_.reserve(vec_to_distribute_keys.size());list_nodes.push_back(ni);init_nodes[i] = &list_nodes.back();}//将点分配给子节点for (size_t i = 0; i < vec_to_distribute_keys.size(); i++){const cv::KeyPoint &kp = vec_to_distribute_keys[i];init_nodes[kp.pt.x / interval_x]->vec_keys_.push_back(kp);}std::list<ExtractorNode>::iterator lit = list_nodes.begin();while (lit != list_nodes.end()){// 如果只含一个特征点的时候，则不再划分if (lit->vec_keys_.size() == 1){lit->is_no_more_ = true;lit++;}else if (lit->vec_keys_.empty())lit = list_nodes.erase(lit);elselit++;}bool is_finish = false;int iteration = 0;std::vector<std::pair<int, ExtractorNode*> > keys_size_and_node;//节点及对应包含的特征数keys_size_and_node.reserve(list_nodes.size() * 4);while (!is_finish){iteration++;// 初始节点个数，用于判断是否节点再一次进行了划分int prev_size = list_nodes.size();lit = list_nodes.begin();// 表示节点分解次数int to_expand_num = 0;keys_size_and_node.clear();while (lit != list_nodes.end()){if (lit->is_no_more_){// 表面只有一个特征点，则不再划分lit++;continue;}else{// 如果超过一个特征点，则继续划分ExtractorNode n1, n2, n3, n4;lit->divideNode(n1, n2, n3, n4);// 对划分之后的节点进行判断，是否含有特征点，含有特征点则添加节点if (n1.vec_keys_.size() > 0){list_nodes.push_front(n1);if (n1.vec_keys_.size() > 1){to_expand_num++;keys_size_and_node.push_back(std::make_pair(n1.vec_keys_.size(), &list_nodes.front()));list_nodes.front().node_iter_ = list_nodes.begin();}}if (n2.vec_keys_.size() > 0){list_nodes.push_front(n2);if (n2.vec_keys_.size() > 1){to_expand_num++;keys_size_and_node.push_back(std::make_pair(n2.vec_keys_.size(), &list_nodes.front()));list_nodes.front().node_iter_ = list_nodes.begin();}}if (n3.vec_keys_.size() > 0){list_nodes.push_front(n3);if (n3.vec_keys_.size() > 1){to_expand_num++;keys_size_and_node.push_back(std::make_pair(n3.vec_keys_.size(), &list_nodes.front()));list_nodes.front().node_iter_ = list_nodes.begin();}}if (n4.vec_keys_.size() > 0){list_nodes.push_front(n4);if (n4.vec_keys_.size() > 1){to_expand_num++;keys_size_and_node.push_back(std::make_pair(n4.vec_keys_.size(), &list_nodes.front()));list_nodes.front().node_iter_ = list_nodes.begin();}}lit = list_nodes.erase(lit);continue;}}// 当节点个数大于需分配的特征数或者所有的节点只有一个特征点（节点不能划分）的时候，则结束。if ((int)list_nodes.size() >= feature_num || (int)list_nodes.size() == prev_size){is_finish = true;}else if (((int)list_nodes.size() + to_expand_num * 3) > feature_num)//节点展开次数乘以3用于表明下一次的节点分解可能超过特征数，即为最后一次分解{while (!is_finish){prev_size = list_nodes.size();std::vector<std::pair<int, ExtractorNode*> > prev_size_and_node = keys_size_and_node;keys_size_and_node.clear();sort(prev_size_and_node.begin(), prev_size_and_node.end());for (int j = prev_size_and_node.size() - 1; j >= 0; j--){ExtractorNode n1, n2, n3, n4;prev_size_and_node[j].second->divideNode(n1, n2, n3, n4);// 划分之后进一步的判断if (n1.vec_keys_.size() > 0){list_nodes.push_front(n1);if (n1.vec_keys_.size() > 1){keys_size_and_node.push_back(std::make_pair(n1.vec_keys_.size(), &list_nodes.front()));list_nodes.front().node_iter_ = list_nodes.begin();}}if (n2.vec_keys_.size() > 0){list_nodes.push_front(n2);if (n2.vec_keys_.size() > 1){keys_size_and_node.push_back(std::make_pair(n2.vec_keys_.size(), &list_nodes.front()));list_nodes.front().node_iter_ = list_nodes.begin();}}if (n3.vec_keys_.size() > 0){list_nodes.push_front(n3);if (n3.vec_keys_.size() > 1){keys_size_and_node.push_back(std::make_pair(n3.vec_keys_.size(), &list_nodes.front()));list_nodes.front().node_iter_ = list_nodes.begin();}}if (n4.vec_keys_.size() > 0){list_nodes.push_front(n4);if (n4.vec_keys_.size() > 1){keys_size_and_node.push_back(std::make_pair(n4.vec_keys_.size(), &list_nodes.front()));list_nodes.front().node_iter_ = list_nodes.begin();}}list_nodes.erase(prev_size_and_node[j].second->node_iter_);if ((int)list_nodes.size() >= feature_num)break;}if ((int)list_nodes.size() >= feature_num || (int)list_nodes.size() == prev_size)is_finish = true;}}}// 保留每个节点下最好的特征点std::vector<cv::KeyPoint> result_keys;result_keys.reserve(features_num_);for (std::list<ExtractorNode>::iterator lit = list_nodes.begin(); lit != list_nodes.end(); lit++){std::vector<cv::KeyPoint> &node_keys = lit->vec_keys_;cv::KeyPoint* keypoint = &node_keys[0];float max_response = keypoint->response;for (size_t k = 1; k < node_keys.size(); k++){if (node_keys[k].response > max_response){keypoint = &node_keys[k];max_response = node_keys[k].response;}}result_keys.push_back(*keypoint);}return result_keys;}

上述通过划分四叉树的形式将图像分割成每个节点，对每个节点里面的特征进行选择最好特征，这样就对检测到的特征进行了均匀化处理。

计算特征的方向

好，特征检测好了之后，计算特征的方向，计算特征方向是为了保证特征具有旋转不变的特性。

12345678

static void computeOrientation(const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, const std::vector<int>& umax){for (std::vector<cv::KeyPoint>::iterator keypoint = keypoints.begin(),keypoint_end = keypoints.end(); keypoint != keypoint_end; ++keypoint){keypoint->angle = IC_Angle(image, keypoint->pt, umax);}}

具体计算

123456789101112131415161718192021222324252627282930

// 灰度质心法计算特征点方向static float IC_Angle(const cv::Mat& image, cv::Point2f pt, const std::vector<int> & u_max){int m_01 = 0, m_10 = 0;// 得到中心位置const uchar* center = &image.at<uchar>(cvRound(pt.y), cvRound(pt.x));// 对 v=0 这一行单独计算for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)m_10 += u * center[u];// 这边要注意图像的step不一定是图像的宽度int step = (int)image.step1();for (int v = 1; v <= HALF_PATCH_SIZE; ++v){// 上下和左右两条线同时计算int v_sum = 0;int d = u_max[v];for (int u = -d; u <= d; ++u){int val_plus = center[u + v*step], val_minus = center[u - v*step];v_sum += (val_plus - val_minus);//计算上下的时候是有符号的，所以这边是减m_10 += u * (val_plus + val_minus);//这边加是由于u已经确定好了符号}m_01 += v * v_sum;}return cv::fastAtan2((float)m_01, (float)m_10);}

灰度质心法假设角点的灰度与质心之间存在一个偏移，这个向量可以用于表示一个方向，具体也就是计算这个区域的所有像素和对应x的坐标的乘积与所有像素与对应y的坐标的乘积的比值,计算反正切。
具体的一些细节也可以参考博客ORB特征点检测.

计算特征描述子

特征计算出来，方向也计算了，那下面是计算特征描述子

12345678910111213141516171819202122232425262728293031323334353637383940

const float factor_pi = (float)(CV_PI / 180.f);static void computeOrbDescriptor(const cv::KeyPoint& kpt,const cv::Mat& img, const cv::Point* pattern,uchar* desc){float angle = (float)kpt.angle*factor_pi;float a = (float)cos(angle), b = (float)sin(angle);const uchar* center = &img.at<uchar>(cvRound(kpt.pt.y), cvRound(kpt.pt.x));const int step = (int)img.step;#define GET_VALUE(idx) \        center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + \               cvRound(pattern[idx].x*a - pattern[idx].y*b)]for (int i = 0; i < 32; ++i, pattern += 16){int t0, t1, val;t0 = GET_VALUE(0); t1 = GET_VALUE(1);val = t0 < t1;t0 = GET_VALUE(2); t1 = GET_VALUE(3);val |= (t0 < t1) << 1;t0 = GET_VALUE(4); t1 = GET_VALUE(5);val |= (t0 < t1) << 2;t0 = GET_VALUE(6); t1 = GET_VALUE(7);val |= (t0 < t1) << 3;t0 = GET_VALUE(8); t1 = GET_VALUE(9);val |= (t0 < t1) << 4;t0 = GET_VALUE(10); t1 = GET_VALUE(11);val |= (t0 < t1) << 5;t0 = GET_VALUE(12); t1 = GET_VALUE(13);val |= (t0 < t1) << 6;t0 = GET_VALUE(14); t1 = GET_VALUE(15);val |= (t0 < t1) << 7;desc[i] = (uchar)val;}#undef GET_VALUE}

这部分的计算的在博客ORB特征点检测.讲述的比较清楚，我这边不在叙述。

实验

好了，到这边orb-slam中的orb特征检测就结束了，下面我们就简单做一些实验，来测试一下当前orb的特征检测与opencv中的orb特征检测的差异。
具体实验结果如下：

我们可以看到orb-slam的特征分布的更加均匀一点，另外也对时间进行了测试，测试计算50次orb特征检测的平均时间，opencv大约24毫秒，而orb-slam中大约11毫秒，时间也有所提高，测试平台i7处理器。

这边代码中有误，实际是OpenCV的时间为11ms，orb-slma的时间是24ms

这一部分完整代码参考：https://github.com/yueying/openslam.git

原文链接：http://www.fengbing.net/2016/04/03/%E4%B8%80%E6%AD%A5%E6%AD%A5%E5%AE%9E%E7%8E%B0slam2-orb%E7%89%B9%E5%BE%81%E6%A3%80%E6%B5%8B/