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

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

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

特征检测

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

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

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

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45

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& keypoints, cv::OutputArray descriptors); protected: /** \brief 计算图像金字塔 */ void computePyramid(cv::Mat image); /** \brief 通过四叉树的方式计算特征点 */ void computeKeyPointsQuadTree(std::vector<std::vector >& all_keypoints); /** \brief 通过四叉树的方式分配特征点 */ std::vector distributeQuadTree(const std::vector& 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 vec_image_pyramid_;//!<图像金字塔 protected: std::vector pattern_;//!<用于存放训练的模板 int features_num_;//!<最多提取的特征点的数量 float scale_factor_;//!<金字塔图像之间的尺度参数 int levels_num_;//!<高斯金字塔的层数 int default_fast_threshold_;//!<默认设置fast角点阈值20 int min_fast_threshold_;//!<设置fast角点阈值为9 std::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特征检测阈值，以保证检测的特征数目。

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

1 2 3 4 5 6 7

// 这边将每层金字塔对应的尺度因子给出 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_; }

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

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

1 2 3 4 5 6 7 8 9 10 11 12

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);

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

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23

// 复制训练的模板 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()，那就看看函数调用。

函数调用

构建图像金字塔

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

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18

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; } } }

计算特征点

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

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91

void ORBextractor::computeKeyPointsQuadTree(std::vector<std::vector >& 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 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 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::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 & 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特征检测，接下来就是将图像划分成四叉树形式，根据这一层的特征数确定四叉树的节点，将这一层图像检测到的特征划分到这些节点，保证每个节点里面有一个特征。

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222

std::vector ORBextractor::distributeQuadTree(const std::vector& 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(max_x - min_x) / (max_y - min_y)); // 得到节点之间的间隔 const float interval_x = static_cast(max_x - min_x) / init_node_num; std::vector*> init_nodes; init_nodes.resize(init_node_num); // 划分之后包含的节点 std::list list_nodes; for (int i = 0; i < init_node_num; i++) { ExtractorNode ni; ni.UL_ = cv::Point2i(interval_x*static_cast(i), 0); ni.UR_ = cv::Point2i(interval_x*static_cast(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::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); else lit++; } bool is_finish = false; int iteration = 0; std::vector*> > 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*> > 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 result_keys; result_keys.reserve(features_num_); for (std::list::iterator lit = list_nodes.begin(); lit != list_nodes.end(); lit++) { std::vector &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; }

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

计算特征的方向

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

1 2 3 4 5 6 7 8

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

具体计算

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30

// 灰度质心法计算特征点方向 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(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特征点检测.

计算特征描述子

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

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40

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(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/