一步步实现slam3-初始位置估计2

上一步已经找到了初始的两帧，并且得到对应的匹配，那下一步就根据匹配点计算相应的单应矩阵和基础矩阵。
具体计算就是随机选取8个匹配点，启动两个线程分别计算单应矩阵和基础矩阵。具体计算两个矩阵就不在叙述，我在代码中添加了比较详细的注释。
计算单应矩阵

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

cv::Mat Initializer::calcHFromMatches(const std::vector<cv::Point2f> &points_ref, const std::vector<cv::Point2f> &points_cur) { /** * x = H y ，则对向量 x和Hy 进行叉乘为0，即： * * | 0 -1 v2| |a b c| |u1| |0| * | 1 0 -u2| * |d e f| * |v1| = |0| * |-v2 u2 0| |g h 1| |1 | |0| * * 矩阵化简得： * * (-d+v2*g)*u1 + (-e+v2*h)*v1 + -f+v2 |0| * (a-u2*g)*u1 + (b-u2*h)*v1 + c-u2 = |0| * (-v2*a+u2*d)*u1 + (-v2*b+u2*e)*v1 + -v2*c+u2*f |0| * * 0*a + 0*b + 0*c - u1*d - v1*e - 1*f + v2*u1*g + v2*v1*h + v2 = 0 * u1*a+v1*b + 1*c + 0*d + 0*e +0*f - u2*u1*g - u2*v1*h - u2 = 0 * -v2*u1*a -v2*v1*b -v2*c +u2*u1*d +u2*v1*e +u2*f +0*g +0*h + 0 = 0 */ const int N = points_ref.size(); cv::Mat A(2 * N, 9, CV_32F); for (int i = 0; i < N; i++) { const float u1 = points_ref[i].x; const float v1 = points_ref[i].y; const float u2 = points_cur[i].x; const float v2 = points_cur[i].y; A.at<float>(2 * i, 0) = 0.0; A.at<float>(2 * i, 1) = 0.0; A.at<float>(2 * i, 2) = 0.0; A.at<float>(2 * i, 3) = -u1; A.at<float>(2 * i, 4) = -v1; A.at<float>(2 * i, 5) = -1; A.at<float>(2 * i, 6) = v2*u1; A.at<float>(2 * i, 7) = v2*v1; A.at<float>(2 * i, 8) = v2; A.at<float>(2 * i + 1, 0) = u1; A.at<float>(2 * i + 1, 1) = v1; A.at<float>(2 * i + 1, 2) = 1; A.at<float>(2 * i + 1, 3) = 0.0; A.at<float>(2 * i + 1, 4) = 0.0; A.at<float>(2 * i + 1, 5) = 0.0; A.at<float>(2 * i + 1, 6) = -u2*u1; A.at<float>(2 * i + 1, 7) = -u2*v1; A.at<float>(2 * i + 1, 8) = -u2; } cv::Mat u, w, vt; // 通过svd进行最小二乘求解 cv::SVDecomp(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV); return vt.row(8).reshape(0, 3); }

 

计算基础矩阵

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

cv::Mat Initializer::calcFFromMatches(const std::vector<cv::Point2f> &points_ref, const std::vector<cv::Point2f> &points_cur) { /** * 构建基础矩阵的约束方程，给定一对点对应m=(u1,v1,1)T,m'=(u2,v2,1)T * 满足基础矩阵F m'TFm=0,令F=(f_ij),则约束方程可以化简为： * u2u1f_11+u2v1f_12+u2f_13+v2u1f_21+v2v1f_22+v2f_23+u1f_31+v1f_32+f_33=0 * 令f = (f_11,f_12,f_13,f_21,f_22,f_23,f_31,f_32,f_33) * 则(u2u1,u2v1,u2,v2u1,v2v1,v2,u1,v1,1)f=0; * 这样，给定N个对应点就可以得到线性方程组Af=0 * A就是一个N*9的矩阵，由于基础矩阵是非零的，所以f是一个非零向量，即 * 线性方程组有非零解，另外基础矩阵的秩为2，重要的约束条件 */ const int N = points_ref.size(); cv::Mat A(N, 9, CV_32F); for (int i = 0; i < N; i++) { const float u1 = points_ref[i].x; const float v1 = points_ref[i].y; const float u2 = points_cur[i].x; const float v2 = points_cur[i].y; A.at<float>(i, 0) = u2*u1; A.at<float>(i, 1) = u2*v1; A.at<float>(i, 2) = u2; A.at<float>(i, 3) = v2*u1; A.at<float>(i, 4) = v2*v1; A.at<float>(i, 5) = v2; A.at<float>(i, 6) = u1; A.at<float>(i, 7) = v1; A.at<float>(i, 8) = 1; } cv::Mat u, w, vt; cv::SVDecomp(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV); cv::Mat Fpre = vt.row(8).reshape(0, 3); cv::SVDecomp(Fpre, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV); w.at<float>(2) = 0; return u*cv::Mat::diag(w)*vt; }

 

下一步就是如何选择，也就是到底何时采用基础矩阵，何时采用单应矩阵。这边作者将计算好的单应矩阵或者基础矩阵重新计算投影误差，具体作者设置的阈值这边我也不是很明白，把论文中这个部分贴在这边。

这样基本的位姿初始化就结束了，具体添加第二帧的代码如下：

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

bool Initializer::addSecondFrame(FramePtr cur_frame) { cur_features_ = cur_frame->features_; if (cur_features_.size() < 100) { OPENSLAM_WARN << "Second image has less than 100 features. Retry in more textured environment." << std::endl; return false; } // 寻找对应匹配 ORBmatcher matcher(0.9, true); int nmatches = matcher.searchForInitialization(ref_frame_, cur_frame, prev_matched_, init_matchex_, 100); // 检测是否有足够的匹配 if (nmatches < 100) { OPENSLAM_WARN << "Matching number has less than 100. " << std::endl; return false; } cur_features_ = cur_frame->features_; // 给出当前帧与参考帧的匹配 matches_ref_cur_.clear(); matches_ref_cur_.reserve(cur_features_.size()); is_matched_ref_.resize(ref_features_.size()); for (size_t i = 0, iend = init_matchex_.size(); i < iend; i++) { if (init_matchex_[i] >= 0) { matches_ref_cur_.push_back(std::make_pair(i, init_matchex_[i])); is_matched_ref_[i] = true; } else { is_matched_ref_[i] = false; } } const int N = matches_ref_cur_.size(); // 用于随机取8组数据的全部索引 std::vector<size_t> all_indices; all_indices.reserve(N); std::vector<size_t> available_indices; for (int i = 0; i < N; i++) { all_indices.push_back(i); } // 生成一组8个点的数据集用于RANSAC的迭代 ransac_sets_ = std::vector< std::vector<size_t> >(max_iter_num_, std::vector<size_t>(8, 0)); // 这边随机改time(0) srand(time(0)); for (int it = 0; it < max_iter_num_; it++) { available_indices = all_indices; // 在所有数据集中选择8组 for (size_t j = 0; j < 8; j++) { int d = available_indices.size(); int randi = int(((double)rand() / ((double)RAND_MAX + 1.0)) * d); int idx = available_indices[randi]; ransac_sets_[it][j] = idx; available_indices[randi] = available_indices.back(); available_indices.pop_back(); } } // 启动线程用于并行计算基础矩阵和单应矩阵 std::vector<bool> matches_inliers_H, matches_inliers_F; float SH, SF; cv::Mat H, F; std::thread threadH(&Initializer::findHomography, this, std::ref(matches_inliers_H), std::ref(SH), std::ref(H)); std::thread threadF(&Initializer::findFundamental, this, std::ref(matches_inliers_F), std::ref(SF), std::ref(F)); // 等待直到两个线程结束 threadH.join(); threadF.join(); // 计算scores的比值 float RH = SH / (SH + SF); cv::Mat R_cur_ref; //当前相机的旋转,相对于上一帧，上一帧为初始帧姿态为I单位阵 cv::Mat t_cur_ref; // 当前相机的平移 std::vector<cv::Point3f> init_3d_points; std::vector<bool> is_triangulated; // 初始化匹配当中，三角定位是否成功 cv::Mat K = cur_frame->cam_->cvK(); // 具体采用基础矩阵还是单应矩阵分解计算初始结构取决于ratio (0.40-0.45) if (RH > 0.40) { if (!reconstructH(matches_inliers_H, H, K, R_cur_ref, t_cur_ref, init_3d_points, is_triangulated, 1.0, 50)) return false; } else { if (!reconstructF(matches_inliers_F, F, K, R_cur_ref, t_cur_ref, init_3d_points, is_triangulated, 1.0, 50)) return false; } for (size_t i = 0, iend = init_matchex_.size(); i < iend; i++) { if (init_matchex_[i] >= 0 && !is_triangulated[i]) { init_matchex_[i] = -1; nmatches--; } } // 设置当前帧的位姿 cv::Mat Tcw = cv::Mat::eye(4, 4, CV_32F); R_cur_ref.copyTo(Tcw.rowRange(0, 3).colRange(0, 3)); t_cur_ref.copyTo(Tcw.rowRange(0, 3).col(3)); cur_frame->T_f_w_ = Tcw; return true; }