文章目录
  1. 1. sophus库的添加
  2. 2. 初始位置确定
  3. 3. 选择第一帧
  4. 4. 选择第二帧
    1. 4.1. 特征跟踪
  5. 5. 下一步计划

前面一篇博客,我们已经进行了特征提取,前面我们也说了,我们接下来不是通过特征匹配的方式来寻找对应特征点,而是采用跟踪的方式。
在前面视觉里程计总述中讲诉了误差的累计,因此初始位置的确定显得尤为重要,要确保初始位置的尽可能的准确。这一篇博客主要讲述了如何更精确的确定初始位置。

sophus库的添加

对于sophus,目前我理解的范围就是对旋转和平移做了一个封装,可以更好的进行刚性变换。为了更方便的看懂,这里使用了sophus的非模板的版本,通过git clone https://github.com/strasdat/Sophus.git后继续git checkout a621ff
这边编译在vs2013上报错,在文件so2.cpp中,修改如下:

1
2
3
4
5
6
7
SO2::SO2()
{
//unit_complex_.real() = 1.;
//unit_complex_.imag() = 0.;
unit_complex_.real(1.);
unit_complex_.imag(0.);
}

对于李群李代数后续需进一步学习。

初始位置确定

初始位置的确定的基本思路,根据光流确定对应特征点,根据对应特征点计算两帧之间的单应矩阵,根据单应矩阵进行分解,计算出两帧之间的旋转和平移。
考虑到帧的旋转和平移,则在帧上添加属性,添加从世界坐标系(w)orld转到摄像机坐标系(f)rame的刚性变换Rt Sophus::SE3 Tf_w;,而对于特征添加特征对应的3D点,以及特征的单位方向向量(用于后期的三角定位),具体添加如下:

1
2
Vector3d f;           //!< 特征的单位方向向量
Point3D* point; //!< 指针指向跟特征对应的3D点

这里我们构建Point3D这个类,用于表示特征所对应的3D点,具体定义如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
class Point3D : Noncopyable
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Point3D(const Vector3d& pos);

~Point3D();
/// 添加特征到一个帧中
void addFrameRef(Feature* ftr);

public:
static int point_counter_; //!< 创建点的计数,用于设置唯一的id
int id_; //!< 点唯一的id
Vector3d pos_; //!< 点在世界坐标系中的位置
std::list<Feature*> obs_; //!< 对应这个点的特征
};

这样新的数据结构就修改和添加完成,我们接下来要做的事情就是进行整合。

选择第一帧

为了更好的确定初始位置,我们对第一帧的选择有一定的限制条件,我们要确保第一帧的图像中检测到的特征数大于100个。只有满足这样的条件,我们可以方便的进行下一步的特征跟踪,具体实现如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
InitResult Initialization::addFirstFrame(FramePtr frame_ref)
{
reset();
detectFeatures(frame_ref, px_ref_, f_ref_);
if (px_ref_.size() < 100)
{
std::cerr << "First image has less than 100 features. Retry in more textured environment." << std::endl;
return FAILURE;
}
// 将这一帧图像做为参考帧
frame_ref_ = frame_ref;
// 先设置当前帧的特征与参考帧的特征一致
px_cur_.insert(px_cur_.begin(), px_ref_.begin(), px_ref_.end());
return SUCCESS;
}

这边根据当前帧可以检测到特征点的位置,以及获得特征的单位向量,具体可以参考:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
/// 检测fast角度,输出的是对应的点和点的方向向量(可以考虑为点的反投影坐标)
void Initialization::detectFeatures(
FramePtr frame,
std::vector<cv::Point2f>& px_vec,
std::vector<Vector3d>& f_vec)
{
Features new_features;
FastDetector detector(
frame->img().cols, frame->img().rows, 25, 3);
detector.detect(frame.get(), frame->img_pyr_, 20.0, new_features);

// 返回特征位置和特征的单位向量
px_vec.clear(); px_vec.reserve(new_features.size());
f_vec.clear(); f_vec.reserve(new_features.size());
std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){
px_vec.push_back(cv::Point2f(ftr->px[0], ftr->px[1]));
f_vec.push_back(ftr->f);
delete ftr;
});
}

这边有一个疑惑,本来打算对帧frame类中的属性list修改为list,
这样就避免了目前对Feature new和delete不在一起的状况。
但是修改之后会报错,报错在对特征对象进行push_back的时候,目前还是仿作者采用指针的方式,后期进行修改。

选择第二帧

特征跟踪

第一帧确定好了之后,然后通过金字塔Lucas-Kanade光流方法计算前期特征的光流(稀疏光流),具体使用OpenCV的方法calcOpticalFlowPyrLK,具体如下:

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
void Initialization::trackKlt(
FramePtr frame_ref,
FramePtr frame_cur,
std::vector<cv::Point2f>& px_ref,
std::vector<cv::Point2f>& px_cur,
std::vector<Vector3d>& f_ref,
std::vector<Vector3d>& f_cur,
std::vector<double>& disparities)
{
const double klt_win_size = 30.0;
const int klt_max_iter = 30;
const double klt_eps = 0.001;
std::vector<uchar> status;
std::vector<float> error;
std::vector<float> min_eig_vec;
cv::TermCriteria termcrit(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, klt_max_iter, klt_eps);
cv::calcOpticalFlowPyrLK(frame_ref->img_pyr_[0], frame_cur->img_pyr_[0],
px_ref, px_cur,
status, error,
cv::Size2i(klt_win_size, klt_win_size),
4, termcrit, 0);//cv::OPTFLOW_USE_INITIAL_FLOW

std::vector<cv::Point2f>::iterator px_ref_it = px_ref.begin();
std::vector<cv::Point2f>::iterator px_cur_it = px_cur.begin();
std::vector<Vector3d>::iterator f_ref_it = f_ref.begin();
f_cur.clear(); f_cur.reserve(px_cur.size());
disparities.clear(); disparities.reserve(px_cur.size());
for (size_t i = 0; px_ref_it != px_ref.end(); ++i)
{
if (!status[i])//如果光流没有发现,则删除
{
px_ref_it = px_ref.erase(px_ref_it);
px_cur_it = px_cur.erase(px_cur_it);
f_ref_it = f_ref.erase(f_ref_it);
continue;
}
f_cur.push_back(frame_cur->c2f(px_cur_it->x, px_cur_it->y));//添加当前特征对应的单位向量
disparities.push_back(Vector2d(px_ref_it->x - px_cur_it->x, px_ref_it->y - px_cur_it->y).norm());//添加对应特征之间的距离
++px_ref_it;
++px_cur_it;
++f_ref_it;
}
}

主要通过跟踪确定下一帧特征的估计位置以及估计特征的单位向量。
而对于跟踪的特征数目要大于50,对于金字塔Lucas-Kanade光流方法的参数调节,后续根据论文及实验进行测试。
视觉里程计总述里面介绍到关键帧,给出图的形式表示两帧的距离很近会影响3D点的准确度。在这里对上述disparities的中值做一个阈值限制,如果其中值小于50,则认为选择的帧不是关键帧,不能进行3D点估计。具体如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
InitResult Initialization::addSecondFrame(FramePtr frame_cur)
{
trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, f_ref_, f_cur_, disparities_);
std::cout << "Init: KLT tracked " << disparities_.size() << " features" << std::endl;

// 符合光流跟踪的特征数
if (disparities_.size() < 50)
return FAILURE;

// 对两帧光流跟踪之后像素差值的中值
double disparity = getMedian(disparities_);
std::cout << "Init: KLT " << disparity << "px average disparity." << std::endl;
// 如果中值小于给定配置参数,则表明这一帧不是关键帧,也就是刚开始的时候两帧不能太近
if (disparity < 50.0)
return NO_KEYFRAME;
// 计算单应矩阵
computeHomography(
f_ref_, f_cur_,
frame_ref_->cam_->getFocalLength(), 2.0,
inliers_, xyz_in_cur_, T_cur_from_ref_);
......
}

接下来进行单应矩阵估计和分解,这边新建Homography类,用于处理单应矩阵估计和分解。具体过程如下:

1
2
3
4
5
6
7
8
9
10
11
bool Homography::computeSE3fromMatches()
{
calcFromMatches();//计算单应
bool res = decompose();// 将单应矩阵进行分解
if (!res)
return false;
computeMatchesInliers();// 计算匹配的内点数
findBestDecomposition();// 找出最好的分解
T_c2_from_c1 = decompositions[0].T;
return true;
}

首先先计算单应矩阵,目前采用OpenCV的函数cv::findHomography来进行计算,具体如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
void Homography::calcFromMatches()
{
std::vector<cv::Point2f> src_pts(fts_c1_.size()), dst_pts(fts_c1_.size());
for (size_t i = 0; i < fts_c1_.size(); ++i)
{
src_pts[i] = cv::Point2f(fts_c1_[i][0], fts_c1_[i][1]);
dst_pts[i] = cv::Point2f(fts_c2_[i][0], fts_c2_[i][1]);
}

cv::Mat cvH = cv::findHomography(src_pts, dst_pts, CV_RANSAC, 2. / focal_length_);
H_c2_from_c1_(0, 0) = cvH.at<double>(0, 0);
H_c2_from_c1_(0, 1) = cvH.at<double>(0, 1);
H_c2_from_c1_(0, 2) = cvH.at<double>(0, 2);
H_c2_from_c1_(1, 0) = cvH.at<double>(1, 0);
H_c2_from_c1_(1, 1) = cvH.at<double>(1, 1);
H_c2_from_c1_(1, 2) = cvH.at<double>(1, 2);
H_c2_from_c1_(2, 0) = cvH.at<double>(2, 0);
H_c2_from_c1_(2, 1) = cvH.at<double>(2, 1);
H_c2_from_c1_(2, 2) = cvH.at<double>(2, 2);
}

这边注意了,对于cv::findHomography的参数ransacReprojThreshold,这边单位不是像素,有1/focal_length的scale,更详细的使用参考:

下一步进行单应矩阵分解,具体过程可以参考[1],具体代码实现参考:

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
bool Homography::decompose()
{
decomp_size_ = 0;
JacobiSVD<MatrixXd> svd(H_c2_from_c1_, ComputeThinU | ComputeThinV);
Vector3d singular_values = svd.singularValues();
// 获得3个特征量
double d1 = fabs(singular_values[0]);
double d2 = fabs(singular_values[1]);
double d3 = fabs(singular_values[2]);

Matrix3d U = svd.matrixU();
Matrix3d V = svd.matrixV(); // VT^T

double s = U.determinant() * V.determinant();

double dPrime_PM = d2;

int nCase;
if (d1 != d2 && d2 != d3)
nCase = 1;
else if (d1 == d2 && d2 == d3)
nCase = 3;
else
nCase = 2;
if (nCase != 1)
{
printf("FATAL Homography Initialization: This motion case is not implemented or is degenerate. Try again. ");
return false;
}

double x1_PM;
double x2;
double x3_PM;
// 在情况1下(d1 != d3)
{ // Eq. 12
x1_PM = sqrt((d1*d1 - d2*d2) / (d1*d1 - d3*d3));
x2 = 0;
x3_PM = sqrt((d2*d2 - d3*d3) / (d1*d1 - d3*d3));
};

double e1[4] = { 1.0, -1.0, 1.0, -1.0 };
double e3[4] = { 1.0, 1.0, -1.0, -1.0 };

Vector3d np;
HomographyDecomposition decomp;

// Case 1, d' > 0:
decomp.d = s * dPrime_PM;
for (size_t signs = 0; signs < 4; signs++)
{
// Eq 13
decomp.R = Matrix3d::Identity();
double dSinTheta = (d1 - d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
double dCosTheta = (d1 * x3_PM * x3_PM + d3 * x1_PM * x1_PM) / d2;
decomp.R(0, 0) = dCosTheta;
decomp.R(0, 2) = -dSinTheta;
decomp.R(2, 0) = dSinTheta;
decomp.R(2, 2) = dCosTheta;
// Eq 14
decomp.t[0] = (d1 - d3) * x1_PM * e1[signs];
decomp.t[1] = 0.0;
decomp.t[2] = (d1 - d3) * -x3_PM * e3[signs];
np[0] = x1_PM * e1[signs];
np[1] = x2;
np[2] = x3_PM * e3[signs];
decomp.n = V * np;
decompositions_[decomp_size_++] = decomp;
}

// Case 1, d' < 0:
decomp.d = s * -dPrime_PM;
for (size_t signs = 0; signs < 4; signs++)
{
// Eq 15
decomp.R = -1 * Matrix3d::Identity();
double dSinPhi = (d1 + d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
double dCosPhi = (d3 * x1_PM * x1_PM - d1 * x3_PM * x3_PM) / d2;
decomp.R(0, 0) = dCosPhi;
decomp.R(0, 2) = dSinPhi;
decomp.R(2, 0) = dSinPhi;
decomp.R(2, 2) = -dCosPhi;

// Eq 16
decomp.t[0] = (d1 + d3) * x1_PM * e1[signs];
decomp.t[1] = 0.0;
decomp.t[2] = (d1 + d3) * x3_PM * e3[signs];

np[0] = x1_PM * e1[signs];
np[1] = x2;
np[2] = x3_PM * e3[signs];
decomp.n = V * np;

decompositions_[decomp_size_++] = decomp;
}

// 保存分解之后的旋转和平移,Eq 8
for (unsigned int i = 0; i < decomp_size_; i++)
{
Matrix3d R = s * U * decompositions_[i].R * V.transpose();
Vector3d t = U * decompositions_[i].t;
decompositions_[i].T = Sophus::SE3(R, t);
}
return true;
}

通过svd分解,根据特征值条件,构建了8组理论解。下一步就是找到最好的分解,在此之前先找到内点数(也就是图像中符合计算出的单应矩阵的特征点),通过内点来进行验证。具体给内点打上标识,过程如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
size_t Homography::computeMatchesInliers()
{
inliers_.clear(); inliers_.resize(fts_c1_.size());
size_t n_inliers = 0;
for (size_t i = 0; i < fts_c1_.size(); i++)
{
Vector2d projected = project2d(H_c2_from_c1_ * unproject2d(fts_c1_[i]));
Vector2d e = fts_c2_[i] - projected;
double e_px = focal_length_ * e.norm();//化为像素距离
inliers_[i] = (e_px < thresh_);//也就是残差在阈值范围内
n_inliers += inliers_[i];
}
return n_inliers;
}

下面根据两个判断,1、特征点在摄像机的前面,具体如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
for (size_t i = 0; i < decomp_size_; i++)
{
HomographyDecomposition &decom = decompositions_[i];
int positive = 0;
for (size_t m = 0; m < fts_c1_.size(); m++)
{
if (!inliers_[m])
continue;
const Vector2d& v2 = fts_c1_[m];
double visibility_test = (H_c2_from_c1_(2, 0) * v2[0] + H_c2_from_c1_(2, 1) * v2[1] + H_c2_from_c1_(2, 2)) / decom.d;
if (visibility_test > 0.0)
positive++;
}
decom.score = -positive;
}

2、通过参考平面的法向量与摄像机光线方向的夹角小于90度来进一步过滤。具体如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
for (size_t i = 0; i < decomp_size_; i++)
{
HomographyDecomposition &decom = decompositions_[i];
int positive = 0;
for (size_t m = 0; m < fts_c1_.size(); m++)
{
if (!inliers_[m])
continue;
Vector3d v3 = unproject2d(fts_c1_[m]);
double visibility_test = v3.dot(decom.n) / decom.d;
if (visibility_test > 0.0)
positive++;
};
decom.score = -positive;
}

提示:点乘为负,则说明夹角大于90度。
这样就剩下了2组可能的姿态,最后通过对上述计算的score计算比如,如下:

1
double ratio = (double)decompositions_[1].score / (double)decompositions_[0].score;

如果ratio的值小于0.9,则确定了分解,如果大于0.9,则通过计算的$R,t$计算本质矩阵后计算sampsonus error来进行判断。不过这边计算sampsonus error不是很明白为什么这么计算,有明白的,还望不吝赐教,具体过程如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
double sampsonusError(const Vector2d &v2Dash, const Matrix3d& essential, const Vector2d& v2)
{
Vector3d v3Dash = unproject2d(v2Dash);
Vector3d v3 = unproject2d(v2);

double error = v3Dash.transpose() * essential * v3;

Vector3d fv3 = essential * v3;
Vector3d fTv3Dash = essential.transpose() * v3Dash;

Vector2d fv3Slice = fv3.head<2>();
Vector2d fTv3DashSlice = fTv3Dash.head<2>();

return (error * error / (fv3Slice.dot(fv3Slice) + fTv3DashSlice.dot(fTv3DashSlice)));
}

分解结束之后计算内点和三角定点计算3D点,具体过程如下:

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
void Initialization::computeHomography(
const std::vector<Vector3d>& f_ref,
const std::vector<Vector3d>& f_cur,
double focal_length,
double reprojection_threshold,
std::vector<int>& inliers,
std::vector<Vector3d>& xyz_in_cur,
SE3& T_cur_from_ref)
{
std::vector<Vector2d, aligned_allocator<Vector2d> > uv_ref(f_ref.size());
std::vector<Vector2d, aligned_allocator<Vector2d> > uv_cur(f_cur.size());
for (size_t i = 0, i_max = f_ref.size(); i < i_max; ++i)
{
uv_ref[i] = project2d(f_ref[i]);
uv_cur[i] = project2d(f_cur[i]);
}
Homography Homography(uv_ref, uv_cur, focal_length, reprojection_threshold);
Homography.computeSE3fromMatches();
std::vector<int> outliers;
computeInliers(f_cur, f_ref,
Homography.T_c2_from_c1_.rotation_matrix(), Homography.T_c2_from_c1_.translation(),
reprojection_threshold, focal_length,
xyz_in_cur, inliers, outliers);
T_cur_from_ref = Homography.T_c2_from_c1_;
}

这边给出一个提示,因为要保证16字节对齐,对于Vector2d之类的容器形式要写成 std::vector<Vector2d, aligned_allocator<Vector2d>>,对于Vector3f 或 MatrixXd之类的,则不考虑,具体可以参考
http://eigen.tuxfamily.org/dox/group__TopicStlContainers.html
对于函数computeInliers首先先进行三角定位,确定两个关联特征对应的3D点,根据3D进行反投影,通过阈值对反投影的误差的关系确定内外点,具体过程如下:

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
double computeInliers(const std::vector<Vector3d>& features1, // c1
const std::vector<Vector3d>& features2, // c2
const Matrix3d& R, // R_c1_c2
const Vector3d& t, // c1_t
const double reproj_thresh,
double focal_length,
std::vector<Vector3d>& xyz_vec, // in frame c1
std::vector<int>& inliers,
std::vector<int>& outliers)
{
inliers.clear(); inliers.reserve(features1.size());
outliers.clear(); outliers.reserve(features1.size());
xyz_vec.clear(); xyz_vec.reserve(features1.size());
double tot_error = 0;
//三角化所有特征,然后计算投影误差和内点
for (size_t j = 0; j<features1.size(); ++j)
{
xyz_vec.push_back(triangulateFeatureNonLin(R, t, features1[j], features2[j]));
double e1 = reprojError(features1[j], xyz_vec.back(), focal_length);
double e2 = reprojError(features2[j], R.transpose()*(xyz_vec.back() - t), focal_length);
if (e1 > reproj_thresh || e2 > reproj_thresh)
outliers.push_back(j);
else
{
inliers.push_back(j);
tot_error += e1 + e2;
}
}
return tot_error;
}

但是这边三角定位确定3D点还是有点不太明白,具体如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
Vector3d triangulateFeatureNonLin(const Matrix3d& R, const Vector3d& t,
const Vector3d& feature1, const Vector3d& feature2)
{
Vector3d f2 = R * feature2;
Vector2d b;
b[0] = t.dot(feature1);
b[1] = t.dot(f2);
Matrix2d A;
A(0, 0) = feature1.dot(feature1);
A(1, 0) = feature1.dot(f2);
A(0, 1) = -A(1, 0);
A(1, 1) = -f2.dot(f2);
Vector2d lambda = A.inverse() * b;
Vector3d xm = lambda[0] * feature1;
Vector3d xn = t + lambda[1] * f2;
return (xm + xn) / 2;
}

到这里单应矩阵的估计、分解就结束了,结束之后要对计算得到的内点数添加阈值控制,要确保内点数大于40.
下一步就是尺度估计,离相机的距离不同,则图像的尺度不一样,采用的方法是计算上面计算的所有3D点所有深度的中值scene_depth_median,则此时尺度为scale = 1.0 / scene_depth_median;

注意:1、对于单目视觉里程计是不知道尺度信息的,也就是在第一帧的时候,我们假设了特征对应的3D点的深度为1。
2、也就是目前是通过计算下一帧所有特征对应的3D点深度的中值作为尺度计算值,这也就限制了算法的使用场景,定位相机应尽可能的对着一个平面,所以朝地是一个很好的选择

接下来计算当前帧的相机外参,具体如下:

1
2
3
4
5
// 计算相对变换SE3
frame_cur->T_f_w_ = T_cur_from_ref_ * frame_ref_->T_f_w_;

// 对位移变换添加尺度
frame_cur->T_f_w_.translation() = -frame_cur->T_f_w_.rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()));

这边注意要对位移变换添加尺度变换,原因是我们实际是假设深度为1的情况下进行计算,而实际深度是不是1,要转变到实际深度情况下的值。另外$pos = -R^{-1}t$
最后对每个内点创建3D点,设置特征,添加到这两帧中,具体如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
SE3 T_world_cur = frame_cur->T_f_w_.inverse();
for (std::vector<int>::iterator it = inliers_.begin(); it != inliers_.end(); ++it)
{
Vector2d px_cur(px_cur_[*it].x, px_cur_[*it].y);
Vector2d px_ref(px_ref_[*it].x, px_ref_[*it].y);
if (frame_ref_->cam_->isInFrame(px_cur.cast<int>(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast<int>(), 10) && xyz_in_cur_[*it].z() > 0)
{
Vector3d pos = T_world_cur * (xyz_in_cur_[*it] * scale);// 将相机下的点坐标转世界坐标
Point3D *new_point = new Point3D(pos);

Feature* ftr_cur = new Feature(frame_cur.get(), new_point, px_cur, f_cur_[*it], 0);
frame_cur->addFeature(ftr_cur);
// 将同一个点对应的特征保存起来,这样点删除了,对应的特征都可以删除
new_point->addFrameRef(ftr_cur);

Feature* ftr_ref = new Feature(frame_ref_.get(), new_point, px_ref, f_ref_[*it], 0);
frame_ref_->addFeature(ftr_ref);
new_point->addFrameRef(ftr_ref);
}
}

到这里整个初始位置的确定就结束了,主要思想就是通过光流跟踪获得对应特征点对,通过对应特征点对估计单应矩阵,将单应矩阵进行分解或者相机外参数据,这个里面主要要注意的是检测到特征的点数,跟踪的特征点数,计算单应矩阵特征的内点数进行阈值限定,以及scale的估计。
最后写一个简单的测试程序对上述过程进行简单测试,具体如下:

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
int main(int argc, char *argv[])
{

CmdLine cmd;
std::string first_frame_name;
std::string second_frame_name;

cmd.add(make_option('f', first_frame_name, "firstname"));
cmd.add(make_option('s', second_frame_name, "secondname"));
try {
if (argc == 1) throw std::string("Invalid command line parameter.");
cmd.process(argc, argv);
}
catch (const std::string& s) {
std::cerr << "Feature detector \nUsage: " << argv[0] << "\n"
<< "[-f|--firstname name]\n"
<< "[-s|--secondname name]\n"
<< std::endl;

std::cerr << s << std::endl;
return EXIT_FAILURE;
}
cv::Mat first_img(cv::imread(first_frame_name, 0));
cv::Mat second_img(cv::imread(second_frame_name, 0));
assert(first_img.type() == CV_8UC1 && !first_img.empty());
assert(second_img.type() == CV_8UC1 && !second_img.empty());

AbstractCamera* cam = new PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0);

FramePtr fisrt_frame(new Frame(cam, first_img, 0.0));
FramePtr second_frame(new Frame(cam, second_img, 1.0));

Initialization init;
init.addFirstFrame(fisrt_frame);
init.addSecondFrame(second_frame);
std::cout << second_frame->T_f_w_ << std::endl;

return 0;
}

结果如下:

下一步计划

上面完成了初始位置的确定,下一步就开始处理连续帧,进行motion estimation和mapping。

[1] Faugeras O D, Lustman F. Motion and structure from motion in a piecewise planar environment[J]. International Journal of Pattern Recognition and Artificial Intelligence, 1988, 2(03): 485-508.

文章目录
  1. 1. sophus库的添加
  2. 2. 初始位置确定
  3. 3. 选择第一帧
  4. 4. 选择第二帧
    1. 4.1. 特征跟踪
  5. 5. 下一步计划