本次讲解的内容为单目初始化中使用单应矩阵恢复位姿的函数Initializer::ReconstructH
函数中所用到的数学原理请参考Motion and structure from motion in a piecewise plannar environment
首先是函数参数
bool Initializer::ReconstructH(vector<bool> &vbMatchesInliers, // 匹配点对的内点标记
cv::Mat &H21, // 从参考帧到当前帧的单应矩阵
cv::Mat &K, // 相机的内参数矩阵
cv::Mat &R21, // 计算出来的相机旋转
cv::Mat &t21, // 计算出来的相机平移
vector<cv::Point3f> &vP3D, // 世界坐标系下,三角化测量特征点对之后得到的特征点的空间坐标
vector<bool> &vbTriangulated, // 特征点是否成功三角化的标记
float minParallax, // 对特征点的三角化测量中,认为其测量有效时需要满足的最小视差角(如果视差角过小则会引起非常大的观测误差),单位是角度
int minTriangulated) // 为了进行运动恢复,所需要的最少的三角化测量成功的点个数
在函数中主要进行了以下步骤
- 相关变量初始化
// 统计匹配的特征点对中属于内点(Inlier)或有效点个数 int N=0; for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++) if(vbMatchesInliers[i]) N++; // We recover 8 motion hypotheses using the method of Faugeras et al. // Motion and structure from motion in a piecewise planar environment. // International Journal of Pattern Recognition and Artificial Intelligence, 1988 // 参考SLAM十四讲第二版p170-p171 // H = K * (R - t * n / d) * K_inv // 其中: K表示内参数矩阵 // K_inv 表示内参数矩阵的逆 // R 和 t 表示旋转和平移向量 // n 表示平面法向量 // 令 H = K * A * K_inv // 则 A = k_inv * H * k cv::Mat invK = K.inv(); cv::Mat A = invK*H21*K; // 对矩阵A进行SVD分解 // A 等待被进行奇异值分解的矩阵 // w 奇异值矩阵 // U 奇异值分解左矩阵 // Vt 奇异值分解右矩阵,注意函数返回的是转置 // cv::SVD::FULL_UV 全部分解 // A = U * w * Vt cv::Mat U,w,Vt,V; cv::SVD::compute(A, w, U, Vt, cv::SVD::FULL_UV); // 根据文献eq(8),计算关联变量 V=Vt.t(); // 计算变量s = det(U) * det(V) // 因为det(V)==det(Vt), 所以 s = det(U) * det(Vt) float s = cv::determinant(U)*cv::determinant(Vt); // 取得矩阵的各个奇异值 float d1 = w.at<float>(0); float d2 = w.at<float>(1); float d3 = w.at<float>(2); // SVD分解正常情况下特征值di应该是正的,且满足d1>=d2>=d3 if(d1/d2<1.00001 || d2/d3<1.00001) { return false; } // 在ORBSLAM中没有对奇异值 d1 d2 d3按照论文中描述的关系进行分类讨论, 而是直接进行了计算 // 定义8中情况下的旋转矩阵、平移向量和空间向量 vector<cv::Mat> vR, vt, vn; vR.reserve(8); vt.reserve(8); vn.reserve(8);
- 计算
d' > 0 时的 4 组解
// Step 1.1 讨论 d' > 0 时的 4 组解 // 根据论文eq.(12)有 // x1 = e1 * sqrt((d1 * d1 - d2 * d2) / (d1 * d1 - d3 * d3)) // x2 = 0 // x3 = e3 * sqrt((d2 * d2 - d2 * d2) / (d1 * d1 - d3 * d3)) // 令 aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3)) // aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3)) // 则 // x1 = e1 * aux1 // x3 = e3 * aux2 // 因为 e1,e2,e3 = 1 or -1 // 所以有x1和x3有四种组合 // x1 = {aux1,aux1,-aux1,-aux1} // x3 = {aux3,-aux3,aux3,-aux3} float aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3)); float aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3)); float x1[] = {aux1,aux1,-aux1,-aux1}; float x3[] = {aux3,-aux3,aux3,-aux3}; // 根据论文eq.(13)有 // sin(theta) = e1 * e3 * sqrt(( d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)) /(d1 + d3)/d2 // cos(theta) = (d2* d2 + d1 * d3) / (d1 + d3) / d2 // 令 aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2) // 则 sin(theta) = e1 * e3 * aux_stheta // cos(theta) = (d2*d2+d1*d3)/((d1+d3)*d2) // 因为 e1 e2 e3 = 1 or -1 // 所以 sin(theta) = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta} float aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2); float ctheta = (d2*d2+d1*d3)/((d1+d3)*d2); float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta}; // 计算旋转矩阵 R' //根据不同的e1 e3组合所得出来的四种R t的解 // | ctheta 0 -aux_stheta| | aux1| // Rp = | 0 1 0 | tp = | 0 | // | aux_stheta 0 ctheta | |-aux3| // | ctheta 0 aux_stheta| | aux1| // Rp = | 0 1 0 | tp = | 0 | // |-aux_stheta 0 ctheta | | aux3| // | ctheta 0 aux_stheta| |-aux1| // Rp = | 0 1 0 | tp = | 0 | // |-aux_stheta 0 ctheta | |-aux3| // | ctheta 0 -aux_stheta| |-aux1| // Rp = | 0 1 0 | tp = | 0 | // | aux_stheta 0 ctheta | | aux3| // 开始遍历这四种情况中的每一种 for(int i=0; i<4; i++) { //生成Rp,就是eq.(8) 的 R' cv::Mat Rp=cv::Mat::eye(3,3,CV_32F); Rp.at<float>(0,0)=ctheta; Rp.at<float>(0,2)=-stheta[i]; Rp.at<float>(2,0)=stheta[i]; Rp.at<float>(2,2)=ctheta; // eq.(8) 计算R cv::Mat R = s*U*Rp*Vt; // 保存 vR.push_back(R); // eq. (14) 生成tp cv::Mat tp(3,1,CV_32F); tp.at<float>(0)=x1[i]; tp.at<float>(1)=0; tp.at<float>(2)=-x3[i]; tp*=d1-d3; // 这里虽然对t有归一化,并没有决定单目整个SLAM过程的尺度 // 因为CreateInitialMapMonocular函数对3D点深度会缩放,然后反过来对 t 有改变 // eq.(8)恢复原始的t cv::Mat t = U*tp; vt.push_back(t/cv::norm(t)); // 构造法向量np cv::Mat np(3,1,CV_32F); np.at<float>(0)=x1[i]; np.at<float>(1)=0; np.at<float>(2)=x3[i]; // eq.(8) 恢复原始的法向量 cv::Mat n = V*np; //看PPT 16页的图,保持平面法向量向上 if(n.at<float>(2)<0) n=-n; // 添加到vector vn.push_back(n); }
- 计算
d' < 0 时的 4 组解
// Step 1.2 讨论 d' < 0 时的 4 组解 float aux_sphi = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1-d3)*d2); // cos_theta项 float cphi = (d1*d3-d2*d2)/((d1-d3)*d2); // 考虑到e1,e2的取值,这里的sin_theta有两种可能的解 float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi}; // 对于每种由e1 e3取值的组合而形成的四种解的情况 for(int i=0; i<4; i++) { // 计算旋转矩阵 R' cv::Mat Rp=cv::Mat::eye(3,3,CV_32F); Rp.at<float>(0,0)=cphi; Rp.at<float>(0,2)=sphi[i]; Rp.at<float>(1,1)=-1; Rp.at<float>(2,0)=sphi[i]; Rp.at<float>(2,2)=-cphi; // 恢复出原来的R cv::Mat R = s*U*Rp*Vt; // 然后添加到vector中 vR.push_back(R); // 构造tp cv::Mat tp(3,1,CV_32F); tp.at<float>(0)=x1[i]; tp.at<float>(1)=0; tp.at<float>(2)=x3[i]; tp*=d1+d3; // 恢复出原来的t cv::Mat t = U*tp; // 归一化之后加入到vector中,要提供给上面的平移矩阵都是要进行过归一化的 vt.push_back(t/cv::norm(t)); // 构造法向量np cv::Mat np(3,1,CV_32F); np.at<float>(0)=x1[i]; np.at<float>(1)=0; np.at<float>(2)=x3[i]; // 恢复出原来的法向量 cv::Mat n = V*np; // 保证法向量指向上方 if(n.at<float>(2)<0) n=-n; // 添加到vector中 vn.push_back(n); }
- 遍历所有解,选择产生相机前方最多3D点的解为最优解
// 最好的good点 int bestGood = 0; // 其次最好的good点 int secondBestGood = 0; // 最好的解的索引,初始值为-1 int bestSolutionIdx = -1; // 最大的视差角 float bestParallax = -1; // 存储最好解对应的,对特征点对进行三角化测量的结果 vector<cv::Point3f> bestP3D; // 最佳解所对应的,那些可以被三角化测量的点的标记 vector<bool> bestTriangulated; // Instead of applying the visibility constraints proposed in the WFaugeras' paper (which could fail for points seen with low parallax) // We reconstruct all hypotheses and check in terms of triangulated points and parallax // Step 2. 对 8 组解进行验证,并选择产生相机前方最多3D点的解为最优解 for(size_t i=0; i<8; i++) { // 第i组解对应的比较大的视差角 float parallaxi; // 三角化测量之后的特征点的空间坐标 vector<cv::Point3f> vP3Di; // 特征点对是否被三角化的标记 vector<bool> vbTriangulatedi; // 调用 Initializer::CheckRT(), 计算good点的数目 int nGood = CheckRT(vR[i],vt[i], //当前组解的旋转矩阵和平移向量 mvKeys1,mvKeys2, //特征点 mvMatches12,vbMatchesInliers, //特征匹配关系以及Inlier标记 K, //相机的内参数矩阵 vP3Di, //存储三角化测量之后的特征点空间坐标的 4.0*mSigma2, //三角化过程中允许的最大重投影误差 vbTriangulatedi, //特征点是否被成功进行三角测量的标记 parallaxi); // 这组解在三角化测量的时候的比较大的视差角 // 更新历史最优和次优的解 // 保留最优的和次优的解.保存次优解的目的是看看最优解是否突出 if(nGood>bestGood) { // 如果当前组解的good点数是历史最优,那么之前的历史最优就变成了历史次优 secondBestGood = bestGood; // 更新历史最优点 bestGood = nGood; // 最优解的组索引为i(就是当前次遍历) bestSolutionIdx = i; // 更新变量 bestParallax = parallaxi; bestP3D = vP3Di; bestTriangulated = vbTriangulatedi; } // 如果当前组的good计数小于历史最优但却大于历史次优 else if(nGood>secondBestGood) { // 说明当前组解是历史次优点,更新之 secondBestGood = nGood; } }
- 根据最优解恢复的3d点的数量、平均视差以及最优解和次优解恢复的3d点数量的比值来判断位姿恢复是否成功
// Step 3 选择最优解。要满足下面的四个条件 // 1. good点数最优解明显大于次优解,这里取0.75经验值 // 2. 视角差大于规定的阈值 // 3. good点数要大于规定的最小的被三角化的点数量 // 4. good数要足够多,达到总数的90%以上 if(secondBestGood<0.75*bestGood && bestParallax>=minParallax && bestGood>minTriangulated && bestGood>0.9*N) { // 从最佳的解的索引访问到R,t vR[bestSolutionIdx].copyTo(R21); vt[bestSolutionIdx].copyTo(t21); // 获得最佳解时,成功三角化的三维点,以后作为初始地图点使用 vP3D = bestP3D; // 获取特征点的被成功进行三角化的标记 vbTriangulated = bestTriangulated; //返回真,找到了最好的解 return true; } return false;