文章目录
  1. 1. 目标
  2. 2. 基础数据结构确定
  3. 3. orb-slam中的初始位姿估计实现

根据整个算法流程,下一步进行初始位姿估计。

目标

上一篇博客讲述了orb-slam中的orb特征提取及与opencv中的orb特征提取进行了简单对比,这一篇博客主要讲述初始姿态估计,姿态的估计要进行特征的匹配,svo里面采用了光流进行特征的匹配,具体见一步步实现单目视觉里程计3——初始位置确定,orb这边是将图像进行划分格子,约束匹配特征在对应格子内寻找。这里我会对这两种方法分别实现,进行对比。

另外本篇还会对特征提取与畸变矫正的顺序做简单分析,是先进行图像畸变矫正后特征提取,还是先特征提取之后再对特征进行畸变矫正。

基础数据结构确定

对于orb-slam中Frame类,显得有点臃肿,对其进行简单的重写。
首先将相机参数提出,构建相机模型,具体可以参考一步步实现单目视觉里程计2——FAST特征检测,这边也构建这样的相机模型。
接下来构建图像帧Frame类,简单构建如下:

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
class  Frame
{
public:
/** 帧的构造,给出对应相机参数及对应原始帧及对应的时间码
*/

Frame(PinholeCamera* cam, const cv::Mat& img, double timestamp, ORBextractor* extractor);
~Frame();

private:
/** \brief 对图像进行ORB特征检测
*/

void extractORB(const cv::Mat &image);

public:
static long unsigned int frame_counter_;//!< 创建帧的计数器,用于设置帧的唯一id
long unsigned int id_; //!< 帧的id
double timestamp_; //!< 帧的时间戳
PinholeCamera* cam_; //!< 相机模型
cv::Mat img_; //!< 帧对应的原始图像
float scale_factor_; //!< 对应金字塔图像的尺度因子
int levels_num_; //!< 对应金字塔的层数
Features features_; //!< 帧对应的特征
int keypoints_num_;//!< 特征点的个数
ORBextractor* extractor_; //!< 把特征提取放到帧中
};

接下来做一个实验,因为很多时候进行特征检测的时候,会遇到这么一个问题,是先对图像进行特征提取之后再对特征点进行畸变矫正,还是先对图像进行畸变矫正后再进行特征提取。
对于这部分代码不做详细说明,具体简单代码实现可见https://github.com/yueying/openslam.git
我这边对TUM的数据集中rgbd_dataset_freiburg1_desk进行简单测试,具体效果如下:

左边的图绿色是原始特征,红色是畸变矫正之后的特征,右边的图是对图像进行畸变矫正之后得到的特征。
对于检测的特征点数,两者之间差别不大,对于耗费的时间时间反而是先进行图像畸变矫正耗时少。按照常规想法,对所有图像点进行remap和只对特征点进行remap来说,应该是后者的时间短,主要这边先对相机初始化的时候就计算好了表,后续进行图像畸变矫正只要查表就行,也就是进行了LUT(Look-Up-Table),而不是每次计算,因此耗时较短。

目前两者差别其实并不大,后续考虑鱼眼镜头也就是畸变较大的时候,这两种方式就要进行考虑,后续验证这种方式是否有影响!

orb-slam中的初始位姿估计实现

上一步通过四叉树的方式把检测到的特征尽可能的均匀化,下一步将特征划分到图像对应的格子中,这样也便于下一步的特征匹配。
因为图像失真,考虑了畸变,转换后图像的边界信息就发生了变化,这边对帧先计算出边界信息

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
void Frame::computeImageBounds(const cv::Mat &image)
{
// 如果是畸变图像
if (cam_->distCoef().at<float>(0) != 0.0)
{
cv::Mat mat = (cv::Mat_<float>(4, 2) << 0.0f, 0.0f,
image.cols, 0.0f,
0.0f, image.rows,
image.cols, image.rows);

// 通过mat类型,转成4个点对即图像的4个边角点,进行畸变计算
mat = mat.reshape(2);
cv::undistortPoints(mat, mat, cam_->cvK(), cam_->distCoef(), cv::Mat(), cam_->cvK());
mat = mat.reshape(1);
// 对矫正之后的点选出最大最小边界值,也就是左上与左下进行比较获取x的最小值
min_bound_x_ = std::min(mat.at<float>(0, 0), mat.at<float>(2, 0));
max_bound_x_ = std::max(mat.at<float>(1, 0), mat.at<float>(3, 0));
min_bound_y_ = std::min(mat.at<float>(0, 1), mat.at<float>(1, 1));
max_bound_y_ = std::max(mat.at<float>(2, 1), mat.at<float>(3, 1));

}
else
{
min_bound_x_ = 0.0f;
max_bound_x_ = image.cols;
min_bound_y_ = 0.0f;
max_bound_y_ = image.rows;
}
}

前面将图像分格子,对每个格子进行fast特征检测,保证每个格子里面尽量有特征,然后将特征进行四叉树划分,将特征分配到每个四叉树节点,然后对每个节点选取最好的特征。

这里还需要将图像划分格子,划分格子的目的是为了更好的进行特征匹配。具体划分格子过程如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
void Frame::assignFeaturesToGrid()
{
// 将特征分配到格子中
int reserve_num = 0.5f*keypoints_num_ / (FRAME_GRID_COLS*FRAME_GRID_ROWS);
for (unsigned int i = 0; i < FRAME_GRID_COLS; i++)
for (unsigned int j = 0; j < FRAME_GRID_ROWS; j++)
grid_[i][j].reserve(reserve_num);

for (int i = 0; i < keypoints_num_; i++)
{
const cv::KeyPoint &kp = features_[i].undistored_keypoint_;

int grid_pos_x, grid_pos_y;
if (posInGrid(kp, grid_pos_x, grid_pos_y))
grid_[grid_pos_x][grid_pos_y].push_back(i);
}
}

基础工作 完成之后,下一步就是去估计相机的初始位姿,怎么估计,考虑输入输出。
输入:两帧图像,输出:两帧对应相机的位姿。不过对于输入的图像帧不是普通帧而是关键帧,也就是带有约束条件的帧。
这边跟orb-slam一样,将视觉里程计的部分放到Tracking类中,构建初始位姿就放在Initializer类中。
那下一步就在Initializer类中添加两个方法,一个是addFirstFrame,另外一个是addSecondFrame。

具体添加第一帧,预估第二帧匹配的特征点的位置,然后初始化初始匹配。这边对初始关键帧的选取是特征点数要大于100

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
bool Initializer::addFirstFrame(FramePtr ref_frame)
{
ref_features_ = ref_frame->features_;
if (ref_features_.size() < 100)
{
OPENSLAM_WARN << "First image has less than 100 features. Retry in more textured environment." << std::endl;
return false;
}
int keypoints_num = ref_frame->getKeypointsNum();
prev_matched_.resize(keypoints_num);
for (size_t i = 0; i < keypoints_num; i++)
prev_matched_[i] = ref_frame->features_[i]->undistored_keypoint_.pt;
std::fill(init_matchex_.begin(), init_matchex_.end(), -1);
// 赋值给属性并设置初始位姿
ref_frame_ = ref_frame;
ref_frame_->T_f_w_ = cv::Mat::eye(4, 4, CV_32F);
return true;
}

下一步选择第二帧,第二帧的选择,首先特征点数也必须要大于100,然后两帧之间寻找匹配,具体匹配思路,根据选取的第一帧的特征点的位置作为输入,然后再第二帧该位置半径r的范围内,寻找可能匹配的点,注意这边的匹配只考虑了原始图像即尺度图像的第一层的特征。

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
std::vector<size_t> Frame::getFeaturesInArea(const float &x, const float  &y,
const float &r, const int min_level, const int max_level) const
{
std::vector<size_t> indices;
indices.reserve(keypoints_num_);
// 将特征点限制到格子的范围区间
const int min_cell_x = std::max(0, (int)floor((x - min_bound_x_ - r)*(1 / grid_element_height_)));
if (min_cell_x >= FRAME_GRID_COLS)
return indices;

const int max_cell_x = std::min((int)FRAME_GRID_COLS - 1, (int)ceil((x - min_bound_x_ + r)*(1 / grid_element_height_)));
if (max_cell_x < 0)
return indices;

const int min_cell_y = std::max(0, (int)floor((y - min_bound_y_ - r)*(1 / grid_element_width_)));
if (min_cell_y >= FRAME_GRID_ROWS)
return indices;

const int max_cell_y = std::min((int)FRAME_GRID_ROWS - 1, (int)ceil((y - min_bound_y_ + r)*(1 / grid_element_width_)));
if (max_cell_y < 0)
return indices;

const bool check_levels = (min_level>0) || (max_level >= 0);

for (int ix = min_cell_x; ix <= max_cell_x; ix++)
{
for (int iy = min_cell_y; iy <= max_cell_y; iy++)
{
// 存储了特征点的索引
const std::vector<size_t> cell = grid_[ix][iy];
if (cell.empty())
continue;

for (size_t j = 0, jend = cell.size(); j < jend; j++)
{
const cv::KeyPoint &undistored_keypoint = features_[cell[j]]->undistored_keypoint_;
// 再次对尺度进一步检测
if (check_levels)
{
if (undistored_keypoint.octave < min_level)
continue;
if (max_level >= 0)
{
if (undistored_keypoint.octave > max_level)
continue;
}
}
// 再次确定特征点是否在半径r的范围内寻找的
const float distx = undistored_keypoint.pt.x - x;
const float disty = undistored_keypoint.pt.y - y;
if (fabs(distx) < r && fabs(disty) < r)
indices.push_back(cell[j]);
}
}
}
return indices;
}

找到了可能的匹配点,下一步进行匹配计算,根据可能匹配特征点的描述子计算距离,确定最佳匹配,另外如果考虑特征点的方向,则将第一帧中的特征的方向角度减去对应第二帧的特征的方向角度,将值划分为直方图,则会在0度和360度左右对应的组距比较大,这样就可以对其它相差太大的角度可以进行剔除,具体细节如下:

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
int ORBmatcher::searchForInitialization(FramePtr ref_frame, FramePtr cur_frame,
std::vector<cv::Point2f> &prev_matched, std::vector<int> &matches_ref_cur, int window_size)
{
int nmatches = 0;
int keypoints_num = ref_frame->getKeypointsNum();
matches_ref_cur = std::vector<int>(keypoints_num, -1);
std::vector<int> rot_hist[HISTO_LENGTH];
for (int i = 0; i < HISTO_LENGTH; i++)
rot_hist[i].reserve(500);
// 计算直方图的比例因子
const float factor = 1.0f / HISTO_LENGTH;

std::vector<int> matched_distance(keypoints_num, (std::numeric_limits<int>::max)());
std::vector<int> matches_cur_ref(keypoints_num, -1);

for (size_t i1 = 0, iend1 = keypoints_num; i1 < iend1; i1++)
{
cv::KeyPoint kp1 = ref_frame->features_[i1]->undistored_keypoint_;
int level1 = kp1.octave;
if (level1 > 0)//只考虑了原始图像
continue;
// 在当前帧中查找可能匹配的特征的索引
std::vector<size_t> keypoint_indices = cur_frame->getFeaturesInArea(prev_matched[i1].x, prev_matched[i1].y, window_size, level1, level1);

if (keypoint_indices.empty())
continue;
// 获取对应参考帧的特征描述
cv::Mat cur_des = ref_frame->features_[i1]->descriptor_;

int best_dist = (std::numeric_limits<int>::max)();
int best_dist2 = (std::numeric_limits<int>::max)();
int best_index2 = -1;
// 对当前帧中可能的特征点进行遍历
for (std::vector<size_t>::iterator vit = keypoint_indices.begin(); vit != keypoint_indices.end(); vit++)
{
size_t i2 = *vit;
// 对应当前帧的特征描述
cv::Mat ref_des = cur_frame->features_[i2]->descriptor_;

int dist = descriptorDistance(cur_des, ref_des);

if (matched_distance[i2] <= dist)
continue;
// 找到最小的前两个距离
if (dist < best_dist)
{
best_dist2 = best_dist;
best_dist = dist;
best_index2 = i2;
}
else if (dist < best_dist2)
{
best_dist2 = dist;
}
}
// 确保最小距离小于阈值
if (best_dist <= TH_LOW)
{
// 再确保此最小距离乘以nn_ratio_要大于最小距离,主要确保该匹配比较鲁棒
if (best_dist < (float)best_dist2*nn_ratio_)
{
// 如果已经匹配,则说明当前特征已经有过对应,则就会有两个对应,移除该匹配
if (matches_cur_ref[best_index2] >= 0)
{
matches_ref_cur[matches_cur_ref[best_index2]] = -1;
nmatches--;
}
// 记录匹配
matches_ref_cur[i1] = best_index2;
matches_cur_ref[best_index2] = i1;
matched_distance[best_index2] = best_dist;
nmatches++;

if (is_check_orientation_)
{
float rot = ref_frame->features_[i1]->undistored_keypoint_.angle - cur_frame->features_[best_index2]->undistored_keypoint_.angle;
if (rot < 0.0)
rot += 360.0f;
int bin = round(rot*factor);
if (bin == HISTO_LENGTH)
bin = 0;
assert(bin >= 0 && bin < HISTO_LENGTH);
rot_hist[bin].push_back(i1);//得到直方图
}
}
}

}

if (is_check_orientation_)
{
int ind1 = -1;
int ind2 = -1;
int ind3 = -1;

computeThreeMaxima(rot_hist, HISTO_LENGTH, ind1, ind2, ind3);

for (int i = 0; i < HISTO_LENGTH; i++)
{
// 对可能的一致的方向就不予考虑
if (i == ind1 || i == ind2 || i == ind3)
continue;
// 对剩下方向不一致的匹配进行剔除
for (size_t j = 0, jend = rot_hist[i].size(); j < jend; j++)
{
int idx1 = rot_hist[i][j];
if (matches_ref_cur[idx1] >= 0)
{
matches_ref_cur[idx1] = -1;
nmatches--;
}
}
}

}

//更新 prev matched
for (size_t i1 = 0, iend1 = matches_ref_cur.size(); i1 < iend1; i1++)
if (matches_ref_cur[i1] >= 0)
prev_matched[i1] = cur_frame->features_[matches_ref_cur[i1]]->undistored_keypoint_.pt;

return nmatches;

}

这样就找到了相对于的匹配,对匹配数进行阈值控制,如果匹配数小于100,则重新选择第二帧。

由于篇幅关系,接下来通过单应矩阵或基础矩阵估计相机位姿部分下一篇博客再进行叙述。

文章目录
  1. 1. 目标
  2. 2. 基础数据结构确定
  3. 3. orb-slam中的初始位姿估计实现