目录
0.更新变量
本函数主要是根据词典向量匹配局部建图线程传来的关键帧与其共视程度最高的几帧进行匹配,利用这些匹配生成地图点,并将地图点存储在mlpRecentAddedMapPoints向量中待检验。
1.函数作用
用当前关键帧与相邻关键帧通过三角化产生新的地图点,使得跟踪更稳
2.code
void LocalMapping::CreateNewMapPoints() { // Retrieve neighbor keyframes in covisibility graph // nn表示搜索最佳共视关键帧的数目 // 不同传感器下要求不一样,单目的时候需要有更多的具有较好共视关系的关键帧来建立地图 int nn = 10; if(mbMonocular) nn=20; // Step 1:在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻关键帧 const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn); // 特征点匹配配置 最佳距离 < 0.6*次佳距离,比较苛刻了。不检查旋转 ORBmatcher matcher(0.6,false); // 取出当前帧从世界坐标系到相机坐标系的变换矩阵 cv::Mat Rcw1 = mpCurrentKeyFrame->GetRotation(); cv::Mat Rwc1 = Rcw1.t(); cv::Mat tcw1 = mpCurrentKeyFrame->GetTranslation(); cv::Mat Tcw1(3,4,CV_32F); Rcw1.copyTo(Tcw1.colRange(0,3)); tcw1.copyTo(Tcw1.col(3)); // 得到当前关键帧(左目)光心在世界坐标系中的坐标、内参 cv::Mat Ow1 = mpCurrentKeyFrame->GetCameraCenter(); const float &fx1 = mpCurrentKeyFrame->fx; const float &fy1 = mpCurrentKeyFrame->fy; const float &cx1 = mpCurrentKeyFrame->cx; const float &cy1 = mpCurrentKeyFrame->cy; const float &invfx1 = mpCurrentKeyFrame->invfx; const float &invfy1 = mpCurrentKeyFrame->invfy; // 用于后面的点深度的验证;这里的1.5是经验值 // mfScaleFactor = 1.2 const float ratioFactor = 1.5f*mpCurrentKeyFrame->mfScaleFactor; // 记录三角化成功的地图点数目 int nnew=0; // Search matches with epipolar restriction and triangulate // Step 2:遍历相邻关键帧,搜索匹配并用极线约束剔除误匹配,最终三角化 for(size_t i=0; i<vpNeighKFs.size(); i++) { // ! 疑似bug,正确应该是 if(i>0 && !CheckNewKeyFrames()) if(i>0 && CheckNewKeyFrames()) return; KeyFrame* pKF2 = vpNeighKFs[i]; // Check first that baseline is not too short // 相邻的关键帧光心在世界坐标系中的坐标 cv::Mat Ow2 = pKF2->GetCameraCenter(); // 基线向量,两个关键帧间的相机位移 cv::Mat vBaseline = Ow2-Ow1; // 基线长度 const float baseline = cv::norm(vBaseline); // Step 3:判断相机运动的基线是不是足够长 if(!mbMonocular) { // 如果是双目相机,关键帧间距小于本身的基线时不生成3D点 // 因为太短的基线下能够恢复的地图点不稳定 if(baseline<pKF2->mb) continue; } else { // 单目相机情况 // 相邻关键帧的场景深度中值 const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2); // 基线与景深的比例 const float ratioBaselineDepth = baseline/medianDepthKF2; // 如果比例特别小,基线太短恢复3D点不准,那么跳过当前邻接的关键帧,不生成3D点 if(ratioBaselineDepth<0.01) continue; } // Compute Fundamental Matrix // Step 4:根据两个关键帧的位姿计算它们之间的基础矩阵 cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2); // Search matches that fullfil epipolar constraint // Step 5:通过词袋对两关键帧的未匹配的特征点快速匹配,用极线约束抑制离群点,生成新的匹配点对 vector<pair<size_t,size_t> > vMatchedIndices; matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false); cv::Mat Rcw2 = pKF2->GetRotation(); cv::Mat Rwc2 = Rcw2.t(); cv::Mat tcw2 = pKF2->GetTranslation(); cv::Mat Tcw2(3,4,CV_32F); Rcw2.copyTo(Tcw2.colRange(0,3)); tcw2.copyTo(Tcw2.col(3)); const float &fx2 = pKF2->fx; const float &fy2 = pKF2->fy; const float &cx2 = pKF2->cx; const float &cy2 = pKF2->cy; const float &invfx2 = pKF2->invfx; const float &invfy2 = pKF2->invfy; // Triangulate each match // Step 6:对每对匹配通过三角化生成3D点,和 Triangulate函数差不多 const int nmatches = vMatchedIndices.size(); for(int ikp=0; ikp<nmatches; ikp++) { // Step 6.1:取出匹配特征点 // 当前匹配对在当前关键帧中的索引 const int &idx1 = vMatchedIndices[ikp].first; // 当前匹配对在邻接关键帧中的索引 const int &idx2 = vMatchedIndices[ikp].second; // 当前匹配在当前关键帧中的特征点 const cv::KeyPoint &kp1 = mpCurrentKeyFrame->mvKeysUn[idx1]; // mvuRight中存放着双目的深度值,如果不是双目,其值将为-1 const float kp1_ur=mpCurrentKeyFrame->mvuRight[idx1]; bool bStereo1 = kp1_ur>=0; // 当前匹配在邻接关键帧中的特征点 const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2]; // mvuRight中存放着双目的深度值,如果不是双目,其值将为-1 const float kp2_ur = pKF2->mvuRight[idx2]; bool bStereo2 = kp2_ur>=0; // Check parallax between rays // Step 6.2:利用匹配点反投影得到视差角 // 特征点反投影,其实得到的是在各自相机坐标系下的一个非归一化的方向向量,和这个点的反投影射线重合 cv::Mat xn1 = (cv::Mat_<float>(3,1) << (kp1.pt.x-cx1)*invfx1, (kp1.pt.y-cy1)*invfy1, 1.0); cv::Mat xn2 = (cv::Mat_<float>(3,1) << (kp2.pt.x-cx2)*invfx2, (kp2.pt.y-cy2)*invfy2, 1.0); // 由相机坐标系转到世界坐标系(得到的是那条反投影射线的一个同向向量在世界坐标系下的表示,还是只能够表示方向),得到视差角余弦值 cv::Mat ray1 = Rwc1*xn1; cv::Mat ray2 = Rwc2*xn2; // 匹配点射线夹角余弦值 const float cosParallaxRays = ray1.dot(ray2)/(cv::norm(ray1)*cv::norm(ray2)); // 加1是为了让cosParallaxStereo随便初始化为一个很大的值 float cosParallaxStereo = cosParallaxRays+1; // cosParallaxStereo1、cosParallaxStereo2在后面可能不存在,需要初始化为较大的值 float cosParallaxStereo1 = cosParallaxStereo; float cosParallaxStereo2 = cosParallaxStereo; // Step 6.3:对于双目,利用双目得到视差角;单目相机没有特殊操作 if(bStereo1) // 传感器是双目相机,并且当前的关键帧的这个点有对应的深度 // 假设是平行的双目相机,计算出双目相机观察这个点的时候的视差角余弦 cosParallaxStereo1 = cos(2*atan2(mpCurrentKeyFrame->mb/2,mpCurrentKeyFrame->mvDepth[idx1])); else if(bStereo2) // 传感器是双目相机,并且邻接的关键帧的这个点有对应的深度,和上面一样操作 cosParallaxStereo2 = cos(2*atan2(pKF2->mb/2,pKF2->mvDepth[idx2])); // 得到双目观测的视差角中最小的那个 cosParallaxStereo = min(cosParallaxStereo1,cosParallaxStereo2); // Step 6.4:三角化恢复3D点 cv::Mat x3D; // cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998)表明视差角正常,0.9998 对应1° // cosParallaxRays < cosParallaxStereo 表明匹配点对夹角大于双目本身观察三维点夹角 // 匹配点对夹角大,用三角法恢复3D点 // 参考:https://github.com/raulmur/ORB_SLAM2/issues/345 if(cosParallaxRays<cosParallaxStereo && cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998)) { // Linear Triangulation Method // 见Initializer.cc的 Triangulate 函数,实现是一样的,顶多就是把投影矩阵换成了变换矩阵 cv::Mat A(4,4,CV_32F); A.row(0) = xn1.at<float>(0)*Tcw1.row(2)-Tcw1.row(0); A.row(1) = xn1.at<float>(1)*Tcw1.row(2)-Tcw1.row(1); A.row(2) = xn2.at<float>(0)*Tcw2.row(2)-Tcw2.row(0); A.row(3) = xn2.at<float>(1)*Tcw2.row(2)-Tcw2.row(1); cv::Mat w,u,vt; cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV); x3D = vt.row(3).t(); // 归一化之前的检查 if(x3D.at<float>(3)==0) continue; // 归一化成为齐次坐标,然后提取前面三个维度作为欧式坐标 x3D = x3D.rowRange(0,3)/x3D.at<float>(3); } // 匹配点对夹角小,用双目恢复3D点 else if(bStereo1 && cosParallaxStereo1<cosParallaxStereo2) { // 如果是双目,用视差角更大的那个双目信息来恢复,直接用已知3D点反投影了 x3D = mpCurrentKeyFrame->UnprojectStereo(idx1); } else if(bStereo2 && cosParallaxStereo2<cosParallaxStereo1) { x3D = pKF2->UnprojectStereo(idx2); } else continue; //No stereo and very low parallax, 放弃 // 为方便后续计算,转换成为了行向量 cv::Mat x3Dt = x3D.t(); //Check triangulation in front of cameras // Step 6.5:检测生成的3D点是否在相机前方,不在的话就放弃这个点 float z1 = Rcw1.row(2).dot(x3Dt)+tcw1.at<float>(2); if(z1<=0) continue; float z2 = Rcw2.row(2).dot(x3Dt)+tcw2.at<float>(2); if(z2<=0) continue; //Check reprojection error in first keyframe // Step 6.6:计算3D点在当前关键帧下的重投影误差 const float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave]; const float x1 = Rcw1.row(0).dot(x3Dt)+tcw1.at<float>(0); const float y1 = Rcw1.row(1).dot(x3Dt)+tcw1.at<float>(1); const float invz1 = 1.0/z1; if(!bStereo1) { // 单目情况下 float u1 = fx1*x1*invz1+cx1; float v1 = fy1*y1*invz1+cy1; float errX1 = u1 - kp1.pt.x; float errY1 = v1 - kp1.pt.y; // 假设测量有一个像素的偏差,2自由度卡方检验阈值是5.991 if((errX1*errX1+errY1*errY1)>5.991*sigmaSquare1) continue; } else { // 双目情况 float u1 = fx1*x1*invz1+cx1; // 根据视差公式计算假想的右目坐标 float u1_r = u1 - mpCurrentKeyFrame->mbf*invz1; float v1 = fy1*y1*invz1+cy1; float errX1 = u1 - kp1.pt.x; float errY1 = v1 - kp1.pt.y; float errX1_r = u1_r - kp1_ur; // 自由度为3,卡方检验阈值是7.8 if((errX1*errX1+errY1*errY1+errX1_r*errX1_r)>7.8*sigmaSquare1) continue; } //Check reprojection error in second keyframe // 计算3D点在另一个关键帧下的重投影误差,操作同上 const float sigmaSquare2 = pKF2->mvLevelSigma2[kp2.octave]; const float x2 = Rcw2.row(0).dot(x3Dt)+tcw2.at<float>(0); const float y2 = Rcw2.row(1).dot(x3Dt)+tcw2.at<float>(1); const float invz2 = 1.0/z2; if(!bStereo2) { float u2 = fx2*x2*invz2+cx2; float v2 = fy2*y2*invz2+cy2; float errX2 = u2 - kp2.pt.x; float errY2 = v2 - kp2.pt.y; if((errX2*errX2+errY2*errY2)>5.991*sigmaSquare2) continue; } else { float u2 = fx2*x2*invz2+cx2; float u2_r = u2 - mpCurrentKeyFrame->mbf*invz2; float v2 = fy2*y2*invz2+cy2; float errX2 = u2 - kp2.pt.x; float errY2 = v2 - kp2.pt.y; float errX2_r = u2_r - kp2_ur; if((errX2*errX2+errY2*errY2+errX2_r*errX2_r)>7.8*sigmaSquare2) continue; } //Check scale consistency // Step 6.7:检查尺度连续性 // 世界坐标系下,3D点与相机间的向量,方向由相机指向3D点 cv::Mat normal1 = x3D-Ow1; float dist1 = cv::norm(normal1); cv::Mat normal2 = x3D-Ow2; float dist2 = cv::norm(normal2); if(dist1==0 || dist2==0) continue; // ratioDist是不考虑金字塔尺度下的距离比例 const float ratioDist = dist2/dist1; // 金字塔尺度因子的比例 const float ratioOctave = mpCurrentKeyFrame->mvScaleFactors[kp1.octave]/pKF2->mvScaleFactors[kp2.octave]; /*if(fabs(ratioDist-ratioOctave)>ratioFactor) continue;*/ // 距离的比例和图像金字塔的比例不应该差太多,否则就跳过 if(ratioDist*ratioFactor<ratioOctave || ratioDist>ratioOctave*ratioFactor) continue; // Triangulation is succesfull // Step 6.8:三角化生成3D点成功,构造成MapPoint MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap); // Step 6.9:为该MapPoint添加属性: // a.观测到该MapPoint的关键帧 pMP->AddObservation(mpCurrentKeyFrame,idx1); pMP->AddObservation(pKF2,idx2); mpCurrentKeyFrame->AddMapPoint(pMP,idx1); pKF2->AddMapPoint(pMP,idx2); // b.该MapPoint的描述子 pMP->ComputeDistinctiveDescriptors(); // c.该MapPoint的平均观测方向和深度范围 pMP->UpdateNormalAndDepth(); mpMap->AddMapPoint(pMP); // Step 6.10:将新产生的点放入检测队列 // 这些MapPoints都会经过MapPointCulling函数的检验 mlpRecentAddedMapPoints.push_back(pMP); nnew++; } } }
3.代码解析
3.1 准备部分
对于双目和RGB-D相机,我们设置nn的值为10。对于单目相机,我们设置nn的值为20。
获取当前关键帧的共视关键帧中共视程度最高的nn帧相邻关键帧,用vpNeighKFs变量保存。
/** * @brief 得到与该关键帧连接的前N个最强共视关键帧(已按权值排序) * * @param[in] N 设定要取出的关键帧数目 * @return vector<KeyFrame*> 满足权重条件的关键帧集合 */ vector<KeyFrame*> KeyFrame::GetBestCovisibilityKeyFrames(const int &N) { unique_lock<mutex> lock(mMutexConnections); if((int)mvpOrderedConnectedKeyFrames.size()<N) // 如果总数不够,就返回所有的关键帧 return mvpOrderedConnectedKeyFrames; else // 取前N个最强共视关键帧 return vector<KeyFrame*>(mvpOrderedConnectedKeyFrames.begin(),mvpOrderedConnectedKeyFrames.begin()+N); }
mvpOrderedConnectedKeyFrames变量中存储着和当前帧共视程度从大到小的关键帧,其在LocalMapping::ProcessNewKeyFrame中的UpdateConnections进行更新:
ORB-SLAM2 --- KeyFrame::UpdateConnections 函数解析https://blog.csdn.net/qq_41694024/article/details/128537286 取出当前帧从世界坐标系到相机坐标系的变换矩阵:
Rcw1、Rwc1、tcw1、Tcw1。
当前关键帧(左目)光心在世界坐标系中的坐标、内参:
Ow1、fx1、fy1、cx1、cy1、invfx1、invfy1。
获取地图点深度最大值:ratioFactor,mfScaleFactor是图像金字塔的尺度因子。
初始化记录三角化点的数量变量nnew = 0
3.2 遍历相邻关键帧,搜索匹配并用极线约束剔除误匹配
3.2.1 基础矩阵与本质矩阵的几何关系推演
在第一帧的坐标系下,设的空间位置为
根据针孔相机模型,两个像素点的像素位置为:,这里为相机内参矩阵,为两个坐标系的相机运动。
(注意:为什么是,因为是世界坐标即在参考系下看见的坐标,但不知道第二个坐标系看到的是什么样子,于是乘以,转化为第二个坐标系即看到的坐标值,那是什么,是,是在参考系下的像素矩阵)
具体来说,这里计算的是和,因为它们把第一个坐标系下的坐标转换到第二个坐标系下。也可以把它们写成李代数形式。
有时,我们会使用齐次坐标表示像素点。在使用齐次坐标时,一个向量将等于它自身乘上任意的非零常数。这通常用于表达一个投影关系。例如,和成投影关系,它们在齐次坐标的意义下是相等的。我们称这种相等关系为尺度意义下相等(equal up to a scale),记作:那么,上述两个投影关系可写为:
现在取
这里的是两个像素点的归一化平面上的坐标。代入上式,得
两边同时左乘。回忆^的定义,这相当于两侧同时与做外积:
两侧同时左乘:
等式左侧,是一个与和都垂直的向量。它再和做内积时,将得到0。
由于等式左侧严格为零,乘以任意非零常数之后也为零,于是我们可以把写成通常的等号。因此,我们就得到:
重新带入有
这两个式子都称为对极约束。它的几何意义是三者共面。
对极约束中同时包含了平移和旋转。我们把中间部分记作两个矩阵:基础矩阵(Fundamental Matrix)F和本质矩阵(Essential Matrix)E,于是可以进一步简化对极约束:
3.2.2 函数解析:匹配特征点
我们遍历每一个它的满足条件共视关键帧vpNeighKFs。
判断①:是否有新的跟踪线程传来的关键帧,若有结束此函数。
判断②:判断相机基线长度是否满足要求:
计算两帧之间的相机光心距离(baseline)。如果为单目情况判断基线和场景中值(该关键帧中能观测到的地图点z的中值)的比例,如果比例特别小,基线太短恢复3D点不准,那么跳过当前邻接的关键帧,不生成3D点;如果为双目情况关键帧间距小于本身的基线时不生成3D点。
通过词袋对两关键帧的未匹配的特征点快速匹配,用极线约束抑制离群点,生成新的匹配点对:方法如下
根据两个关键帧的位姿计算它们之间的基础矩阵:(上文有解释)
// 根据两关键帧的姿态计算两个关键帧之间的基础矩阵 cv::Mat LocalMapping::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2) { // 先构造两帧之间的R12,t12 cv::Mat R1w = pKF1->GetRotation(); cv::Mat t1w = pKF1->GetTranslation(); cv::Mat R2w = pKF2->GetRotation(); cv::Mat t2w = pKF2->GetTranslation(); cv::Mat R12 = R1w*R2w.t(); cv::Mat t12 = -R1w*R2w.t()*t2w+t1w; // 得到 t12 的反对称矩阵 cv::Mat t12x = SkewSymmetricMatrix(t12); const cv::Mat &K1 = pKF1->mK; const cv::Mat &K2 = pKF2->mK; // Essential Matrix: t12叉乘R12 // Fundamental Matrix: inv(K1)*E*inv(K2) return K1.t().inv()*t12x*R12*K2.inv(); }
通过词袋对两关键帧的未匹配的特征点快速匹配,用极线约束抑制离群点,生成新的匹配点对:
vMatchedIndices记录着第一帧和第二帧的匹配关系,是vector<pair<size_t,size_t> >型变量。
我们利用匹配的特征点就可以生成地图点,经过一系列的检查该地图点质量合格后,对关键帧、地图点双向添加:
pMP->AddObservation(mpCurrentKeyFrame,idx1); pMP->AddObservation(pKF2,idx2); mpCurrentKeyFrame->AddMapPoint(pMP,idx1); pKF2->AddMapPoint(pMP,idx2);
同样地,添加完地图点后要计算该地图点最有特征的描述子:
ORB-SLAM2 --- MapPoint::ComputeDistinctiveDescriptors 函数解析https://blog.csdn.net/qq_41694024/article/details/128515977 更新该地图点的平均观测方向和深度范围。
在局部地图上添加该地图点:mpMap->AddMapPoint(pMP);
将该地图点放入检验序列:mlpRecentAddedMapPoints.push_back(pMP);