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

最新推荐文章于 2024-03-25 09:55:40 发布

清风飞扬go

最新推荐文章于 2024-03-25 09:55:40 发布

阅读量2.1w

点赞数 7

文章标签： ORB SLAM FAST

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

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

特征检测

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

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

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

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角点阈值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特征检测阈值，以保证检测的特征数目。

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

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

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

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

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

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

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

函数调用

构建图像金字塔

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

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

}

计算特征点

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

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

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);
		else
			lit++;
	}

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

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

计算特征的方向

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

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

具体计算

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

计算特征描述子

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

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/

清风飞扬go

关注

7
点赞
踩
84

收藏

觉得还不错? 一键收藏
2
评论
大神一步步教你读懂ORB算法，赞！！

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

扫一扫