ORBSLAM2的初始化器Initializer
初始化器Initializer
初始化器仅用于单目SLAM,因为双目或RGBD本身已经有深度信息了,所以不需要初始化。此处略过一些通用性的基础内容,比如本质矩阵/单应矩阵求解,以及从其中恢复位姿的步骤。
单目初始化器会同时计算本质矩阵和单应矩阵,在优先选择单应矩阵的基础上,选择得分更高的结果。
此处的得分是双向单应投影归一化坐标后的距离卡方拒绝域的距离。
卡方检验像素误差与分数计算
在计算本质/单应矩阵时,需要基于RANSAC来剔除外点,而剔除外点的原则时基于卡方检验。在计算前还会对特征点坐标进行归一化。
由于观测噪声默认是服从高斯分布的,而像素坐标系仅有两个自由度(u和v),因此可以对单应/本质变换进行自由度为2的卡方检验,当单应/本质矩阵的双向投影误差过大时会认为对应的特征点在卡方检验的拒绝域内,这样就排除了外点。
2自由度卡方检验5%的显著性水平的拒绝域为5.99,其含义为"帧1中第i个匹配特征点投影到帧2后的投影误差"和"帧2中第i个匹配特征点投影到帧1后的投影误差"均小于5.99时,认为该匹配点是内点。
通过RANSAC算出来的得分最高的组的分数就是用于比较本质矩阵和单应矩阵的分数。
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
|
float Initializer::CheckHomography( const cv::Mat &H21, const cv::Mat &H12, vector<bool> &vbMatchesInliers, float sigma) {
const int N = mvMatches12.size();
const float h{ij} = H21.at<float>(i, j); ...
const float h{ij}inv = H12.at<float>(i, j); ...
const float th = 5.991;
const float invSigmaSquare = 1.0 / (sigma * sigma);
for (int i = 0; i < N; i++) { bool bIn = true;
const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first]; const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second]; const float u1 = kp1.pt.x; const float v1 = kp1.pt.y; const float u2 = kp2.pt.x; const float v2 = kp2.pt.y;
const float w2in1inv = 1.0 / (h31inv * u2 + h32inv * v2 + h33inv); const float u2in1 = (h11inv * u2 + h12inv * v2 + h13inv) * w2in1inv; const float v2in1 = (h21inv * u2 + h22inv * v2 + h23inv) * w2in1inv;
const float squareDist1 = (u1 - u2in1) * (u1 - u2in1) + (v1 - v2in1) * (v1 - v2in1); const float chiSquare1 = squareDist1 * invSigmaSquare;
if (chiSquare1 > th) bIn = false; else score += th - chiSquare1;
const float w1in2inv = 1.0 / (h31 * u1 + h32 * v1 + h33); const float u1in2 = (h11 * u1 + h12 * v1 + h13) * w1in2inv; const float v1in2 = (h21 * u1 + h22 * v1 + h23) * w1in2inv;
const float squareDist2 = (u2 - u1in2) * (u2 - u1in2) + (v2 - v1in2) * (v2 - v1in2); const float chiSquare2 = squareDist2 * invSigmaSquare;
if (chiSquare2 > th) bIn = false; else score += th - chiSquare2;
if (bIn) vbMatchesInliers[i] = true; else vbMatchesInliers[i] = false; } return score; }
|
校验解
每个本质/单应矩阵可以分解出不止一对R和t,因此需要选出最好的R、t组合。至于本质/单应矩阵的分解和三角化求空间点比较基础就不再赘述了。
在分解出若干组R、t后,会选择使得相机前方产生最多3D点的R、t作为最终结果。
v1.5.1