ORB_SLAM3 LocalMapping中CreateNewMapPoints

本文主要是介绍ORB_SLAM3 LocalMapping中CreateNewMapPoints，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

CreateNewMapPoints在新插入的关键帧的邻近的关键帧中，通过词袋模型与新插入关键帧中没有匹配的ORB特征进行匹配，并抛弃其中不满足对极约束的匹配点对，接着通过三角化生成地图点

1.获取共视关键帧

GetBestCovisibilityKeyFrames：找出与当前关键帧共视程度最高前nn帧vpNeighKFs

如果是IMU模式，在vpNeighKFs不够nn帧的情况下，将当前关键帧的前n帧prevKF依次添加到vpNeighKFs中

    // Retrieve neighbor keyframes in covisibility graphint nn = 10;// For stereo inertial caseif(mbMonocular)nn=30;vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);if (mbInertial){KeyFrame* pKF = mpCurrentKeyFrame;int count=0;while((vpNeighKFs.size()<=nn)&&(pKF->mPrevKF)&&(count++<nn)){vector<KeyFrame*>::iterator it = std::find(vpNeighKFs.begin(), vpNeighKFs.end(), pKF->mPrevKF);if(it==vpNeighKFs.end())vpNeighKFs.push_back(pKF->mPrevKF);pKF = pKF->mPrevKF;}}

2.生成地图点

1.检验vpNeighKFs与当前关键帧的基线：

基线：$baseline=\left|O{w}_2-O{w}_1\right|<b$

• 双目：$baseline<b$
  • 移动的距离小于基线$b$，生成的地图点不可靠
• 单目：$ratio=\frac{baseline}{medianDepth}$
  • 当前关键帧的地图点深度特别远，基线短，生成的地图点不可靠

        KeyFrame* pKF2 = vpNeighKFs[i];GeometricCamera* pCamera1 = mpCurrentKeyFrame->mpCamera, *pCamera2 = pKF2->mpCamera;// Check first that baseline is not too shortEigen::Vector3f Ow2 = pKF2->GetCameraCenter();Eigen::Vector3f vBaseline = Ow2-Ow1;const float baseline = vBaseline.norm();if(!mbMonocular){if(baseline<pKF2->mb)continue;}else{const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2);const float ratioBaselineDepth = baseline/medianDepthKF2;if(ratioBaselineDepth<0.01)continue;}

2.通过Bow和极线约束进行匹配搜索

SearchForTriangulation主要用于LocalMapping生成新的地图点，通过寻找当前关键帧和相邻关键帧中未匹配的特征点之间的匹配关系，主要用Bow来加速搜索，以及极线校验来判断是否能够生成地图点，其输入参数如下：

参数	说明
pkF1	当前关键帧
pkF2	邻近关键帧
vMatchedPairs	匹配关系
bOnlyStereo	双目
bCoarse

• 计算KF1光心在KF2上的极点：
  $$\begin{array}{ccc}{c}_2& =& T_{c_{2}w}\cdot t_{w{c}_1}\\ e& =& \pi \left(c_2\right)\end{array}$$

        Sophus::SE3f T1w = pKF1->GetPose();Sophus::SE3f T2w = pKF2->GetPose();Sophus::SE3f Tw2 = pKF2->GetPoseInverse(); // for convenienceEigen::Vector3f Cw = pKF1->GetCameraCenter();Eigen::Vector3f C2 = T2w * Cw;Eigen::Vector2f ep = pKF2->mpCamera->project(C2);

• 利用Bow的FeatureVector加速搜索
  • 需要KF1和KF2的特征点都未匹配
  • 去除重复匹配vbMatched2[idx2]
  • 描述子距离小于阈值TH_LOW
  • 对于单目，特征点到极点的距离需要小于一定阈值
  • 极线校验：KF2上的特征点到KF1对应极线的距离小于阈值epipolarConstrain
  • 得到最佳的描述子距离bestDist，以及最佳的匹配bestIdx2

        Sophus::SE3f T12;Sophus::SE3f Tll, Tlr, Trl, Trr;Eigen::Matrix3f R12; // for fastest computationEigen::Vector3f t12; // for fastest computationGeometricCamera* pCamera1 = pKF1->mpCamera, *pCamera2 = pKF2->mpCamera;//单目if(!pKF1->mpCamera2 && !pKF2->mpCamera2){T12 = T1w * Tw2;R12 = T12.rotationMatrix();t12 = T12.translation();}else{//双目Sophus::SE3f Tr1w = pKF1->GetRightPose();Sophus::SE3f Twr2 = pKF2->GetRightPoseInverse();Tll = T1w * Tw2;Tlr = T1w * Twr2;Trl = Tr1w * Tw2;Trr = Tr1w * Twr2;}Eigen::Matrix3f Rll = Tll.rotationMatrix(), Rlr  = Tlr.rotationMatrix(), Rrl  = Trl.rotationMatrix(), Rrr  = Trr.rotationMatrix();Eigen::Vector3f tll = Tll.translation(), tlr = Tlr.translation(), trl = Trl.translation(), trr = Trr.translation();// Find matches between not tracked keypoints// Matching speed-up by ORB Vocabulary// Compare only ORB that share the same nodeint nmatches=0;vector<bool> vbMatched2(pKF2->N,false);vector<int> vMatches12(pKF1->N,-1);//旋转直方图vector<int> rotHist[HISTO_LENGTH];for(int i=0;i<HISTO_LENGTH;i++)rotHist[i].reserve(500);const float factor = 1.0f/HISTO_LENGTH;DBoW2::FeatureVector::const_iterator f1it = vFeatVec1.begin();DBoW2::FeatureVector::const_iterator f2it = vFeatVec2.begin();DBoW2::FeatureVector::const_iterator f1end = vFeatVec1.end();DBoW2::FeatureVector::const_iterator f2end = vFeatVec2.end();while(f1it!=f1end && f2it!=f2end){if(f1it->first == f2it->first){for(size_t i1=0, iend1=f1it->second.size(); i1<iend1; i1++){const size_t idx1 = f1it->second[i1];MapPoint* pMP1 = pKF1->GetMapPoint(idx1);// If there is already a MapPoint skipif(pMP1){continue;}//双目，针孔模型const bool bStereo1 = (!pKF1->mpCamera2 && pKF1->mvuRight[idx1]>=0);if(bOnlyStereo)if(!bStereo1)continue;const cv::KeyPoint &kp1 = (pKF1 -> NLeft == -1) ? pKF1->mvKeysUn[idx1]: (idx1 < pKF1 -> NLeft) ? pKF1 -> mvKeys[idx1]: pKF1 -> mvKeysRight[idx1 - pKF1 -> NLeft];//为左还是右const bool bRight1 = (pKF1 -> NLeft == -1 || idx1 < pKF1 -> NLeft) ? false: true;const cv::Mat &d1 = pKF1->mDescriptors.row(idx1);int bestDist = TH_LOW;int bestIdx2 = -1;for(size_t i2=0, iend2=f2it->second.size(); i2<iend2; i2++){size_t idx2 = f2it->second[i2];MapPoint* pMP2 = pKF2->GetMapPoint(idx2);// If we have already matched or there is a MapPoint skipif(vbMatched2[idx2] || pMP2)continue;//双目，针孔const bool bStereo2 = (!pKF2->mpCamera2 &&  pKF2->mvuRight[idx2]>=0);if(bOnlyStereo)if(!bStereo2)continue;const cv::Mat &d2 = pKF2->mDescriptors.row(idx2);const int dist = DescriptorDistance(d1,d2);if(dist>TH_LOW || dist>bestDist)continue;const cv::KeyPoint &kp2 = (pKF2 -> NLeft == -1) ? pKF2->mvKeysUn[idx2]: (idx2 < pKF2 -> NLeft) ? pKF2 -> mvKeys[idx2]: pKF2 -> mvKeysRight[idx2 - pKF2 -> NLeft];//为左还是右                                                    		const bool bRight2 = (pKF2 -> NLeft == -1 || idx2 < pKF2 -> NLeft) ? false: true;//单目if(!bStereo1 && !bStereo2 && !pKF1->mpCamera2){const float distex = ep(0)-kp2.pt.x;const float distey = ep(1)-kp2.pt.y;if(distex*distex+distey*distey<100*pKF2->mvScaleFactors[kp2.octave]){continue;}}//双目if(pKF1->mpCamera2 && pKF2->mpCamera2){if(bRight1 && bRight2){R12 = Rrr;t12 = trr;T12 = Trr;pCamera1 = pKF1->mpCamera2;pCamera2 = pKF2->mpCamera2;}else if(bRight1 && !bRight2){R12 = Rrl;t12 = trl;T12 = Trl;pCamera1 = pKF1->mpCamera2;pCamera2 = pKF2->mpCamera;}else if(!bRight1 && bRight2){R12 = Rlr;t12 = tlr;T12 = Tlr;pCamera1 = pKF1->mpCamera;pCamera2 = pKF2->mpCamera2;}else{R12 = Rll;t12 = tll;T12 = Tll;pCamera1 = pKF1->mpCamera;pCamera2 = pKF2->mpCamera;}}if(bCoarse || pCamera1->epipolarConstrain(pCamera2,kp1,kp2,R12,t12,pKF1->mvLevelSigma2[kp1.octave],pKF2->mvLevelSigma2[kp2.octave])) // MODIFICATION_2{bestIdx2 = idx2;bestDist = dist;}}if(bestIdx2>=0){const cv::KeyPoint &kp2 = (pKF2 -> NLeft == -1) ? pKF2->mvKeysUn[bestIdx2]: (bestIdx2 < pKF2 -> NLeft) ? pKF2 -> mvKeys[bestIdx2]: pKF2 -> mvKeysRight[bestIdx2 - pKF2 -> NLeft];vMatches12[idx1]=bestIdx2;nmatches++;if(mbCheckOrientation){float rot = kp1.angle-kp2.angle;if(rot<0.0)rot+=360.0f;int bin = round(rot*factor);if(bin==HISTO_LENGTH)bin=0;assert(bin>=0 && bin<HISTO_LENGTH);rotHist[bin].push_back(idx1);}}}f1it++;f2it++;}else if(f1it->first < f2it->first){f1it = vFeatVec1.lower_bound(f2it->first);}else{f2it = vFeatVec2.lower_bound(f1it->first);}}

针孔模型：
$$F_{12}=K_1^{-T}\cdot t_{12}^{\wedge }\cdot R_{12}\cdot K_2^{-1}$$

$$l=x_1^T\cdot F_{12}$$

$$d=\frac{a\ast a+b\ast b}{a{x}_2+b{y}_2+c}$$

• 利用旋转直方图，检测方向一致性

        if(mbCheckOrientation){int ind1=-1;int ind2=-1;int ind3=-1;ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);for(int i=0; i<HISTO_LENGTH; i++){if(i==ind1 || i==ind2 || i==ind3)continue;for(size_t j=0, jend=rotHist[i].size(); j<jend; j++){vMatches12[rotHist[i][j]]=-1;nmatches--;}}}vMatchedPairs.clear();vMatchedPairs.reserve(nmatches);for(size_t i=0, iend=vMatches12.size(); i<iend; i++){if(vMatches12[i]<0)continue;vMatchedPairs.push_back(make_pair(i,vMatches12[i]));}

3.对于每一对匹配对校验并三角化

• 检查视差角是否满足要求
  cosParallaxRays：
  $$cos\theta =\frac{\left(R_{wc1}\cdot x_1\right)\cdot \left(R_{wc2}\cdot x_2\right)}{\left|R_{wc1}\cdot x_1\right|\cdot \left|R_{wc2}\cdot x_2\right|}$$
  cosParallaxStereo：
  $$cos\left(2\arctan \left(\frac{\frac{b}{2}}{d}\right)\right)$$

• 通过三角化对每对匹配点生成新的3D点

• 检查3D点是否在相机前方：$z>0$

• 判断3D点在关键帧下的重投影误差

• 检查尺度是否合理：$\frac{dis{t}_1}{dis{t}_2}\approx \frac{{\sigma }_1}{{\sigma }_2}$

• 将3D点生成地图点，并添加相应属性

• 将该地图点添加到地图中

• 将该地图点添加到mlpRecentAddedMapPoints中，经过MapPointCulling函数的检验

        Sophus::SE3<float> sophTcw2 = pKF2->GetPose();Eigen::Matrix<float,3,4> eigTcw2 = sophTcw2.matrix3x4();Eigen::Matrix<float,3,3> Rcw2 = eigTcw2.block<3,3>(0,0);Eigen::Matrix<float,3,3> Rwc2 = Rcw2.transpose();Eigen::Vector3f tcw2 = sophTcw2.translation();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 matchconst int nmatches = vMatchedIndices.size();for(int ikp=0; ikp<nmatches; ikp++){const int &idx1 = vMatchedIndices[ikp].first;const int &idx2 = vMatchedIndices[ikp].second;const cv::KeyPoint &kp1 = (mpCurrentKeyFrame -> NLeft == -1) ? mpCurrentKeyFrame->mvKeysUn[idx1]: (idx1 < mpCurrentKeyFrame -> NLeft) ? mpCurrentKeyFrame -> mvKeys[idx1]: mpCurrentKeyFrame -> mvKeysRight[idx1 - mpCurrentKeyFrame -> NLeft];const float kp1_ur=mpCurrentKeyFrame->mvuRight[idx1];//双目bool bStereo1 = (!mpCurrentKeyFrame->mpCamera2 && kp1_ur>=0);//右const bool bRight1 = (mpCurrentKeyFrame -> NLeft == -1 || idx1 < mpCurrentKeyFrame -> NLeft) ? false: true;const cv::KeyPoint &kp2 = (pKF2 -> NLeft == -1) ? pKF2->mvKeysUn[idx2]: (idx2 < pKF2 -> NLeft) ? pKF2 -> mvKeys[idx2]: pKF2 -> mvKeysRight[idx2 - pKF2 -> NLeft];const float kp2_ur = pKF2->mvuRight[idx2];//双目bool bStereo2 = (!pKF2->mpCamera2 && kp2_ur>=0);//右const bool bRight2 = (pKF2 -> NLeft == -1 || idx2 < pKF2 -> NLeft) ? false: true;if(mpCurrentKeyFrame->mpCamera2 && pKF2->mpCamera2){if(bRight1 && bRight2){sophTcw1 = mpCurrentKeyFrame->GetRightPose();Ow1 = mpCurrentKeyFrame->GetRightCameraCenter();sophTcw2 = pKF2->GetRightPose();Ow2 = pKF2->GetRightCameraCenter();pCamera1 = mpCurrentKeyFrame->mpCamera2;pCamera2 = pKF2->mpCamera2;}else if(bRight1 && !bRight2){sophTcw1 = mpCurrentKeyFrame->GetRightPose();Ow1 = mpCurrentKeyFrame->GetRightCameraCenter();sophTcw2 = pKF2->GetPose();Ow2 = pKF2->GetCameraCenter();pCamera1 = mpCurrentKeyFrame->mpCamera2;pCamera2 = pKF2->mpCamera;}else if(!bRight1 && bRight2){sophTcw1 = mpCurrentKeyFrame->GetPose();Ow1 = mpCurrentKeyFrame->GetCameraCenter();sophTcw2 = pKF2->GetRightPose();Ow2 = pKF2->GetRightCameraCenter();pCamera1 = mpCurrentKeyFrame->mpCamera;pCamera2 = pKF2->mpCamera2;}else{sophTcw1 = mpCurrentKeyFrame->GetPose();Ow1 = mpCurrentKeyFrame->GetCameraCenter();sophTcw2 = pKF2->GetPose();Ow2 = pKF2->GetCameraCenter();pCamera1 = mpCurrentKeyFrame->mpCamera;pCamera2 = pKF2->mpCamera;}eigTcw1 = sophTcw1.matrix3x4();Rcw1 = eigTcw1.block<3,3>(0,0);Rwc1 = Rcw1.transpose();tcw1 = sophTcw1.translation();eigTcw2 = sophTcw2.matrix3x4();Rcw2 = eigTcw2.block<3,3>(0,0);Rwc2 = Rcw2.transpose();tcw2 = sophTcw2.translation();}// Check parallax between raysEigen::Vector3f xn1 = pCamera1->unprojectEig(kp1.pt);Eigen::Vector3f xn2 = pCamera2->unprojectEig(kp2.pt);Eigen::Vector3f ray1 = Rwc1 * xn1;Eigen::Vector3f ray2 = Rwc2 * xn2;const float cosParallaxRays = ray1.dot(ray2)/(ray1.norm() * ray2.norm());float cosParallaxStereo = cosParallaxRays+1;float cosParallaxStereo1 = cosParallaxStereo;float cosParallaxStereo2 = cosParallaxStereo;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]));if (bStereo1 || bStereo2) totalStereoPts++;cosParallaxStereo = min(cosParallaxStereo1,cosParallaxStereo2);Eigen::Vector3f x3D;bool goodProj = false;bool bPointStereo = false;if(cosParallaxRays<cosParallaxStereo && cosParallaxRays>0 && (bStereo1 || bStereo2 ||(cosParallaxRays<0.9996 && mbInertial) || (cosParallaxRays<0.9998 && !mbInertial))){goodProj = GeometricTools::Triangulate(xn1, xn2, eigTcw1, eigTcw2, x3D);if(!goodProj)continue;}else if(bStereo1 && cosParallaxStereo1<cosParallaxStereo2){countStereoAttempt++;bPointStereo = true;goodProj = mpCurrentKeyFrame->UnprojectStereo(idx1, x3D);}else if(bStereo2 && cosParallaxStereo2<cosParallaxStereo1){countStereoAttempt++;bPointStereo = true;goodProj = pKF2->UnprojectStereo(idx2, x3D);}else{continue; //No stereo and very low parallax}if(goodProj && bPointStereo)countStereoGoodProj++;if(!goodProj)continue;//Check triangulation in front of camerasfloat z1 = Rcw1.row(2).dot(x3D) + tcw1(2);if(z1<=0)continue;float z2 = Rcw2.row(2).dot(x3D) + tcw2(2);if(z2<=0)continue;//Check reprojection error in first keyframeconst float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave];const float x1 = Rcw1.row(0).dot(x3D)+tcw1(0);const float y1 = Rcw1.row(1).dot(x3D)+tcw1(1);const float invz1 = 1.0/z1;if(!bStereo1){cv::Point2f uv1 = pCamera1->project(cv::Point3f(x1,y1,z1));float errX1 = uv1.x - kp1.pt.x;float errY1 = uv1.y - kp1.pt.y;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;if((errX1*errX1+errY1*errY1+errX1_r*errX1_r)>7.8*sigmaSquare1)continue;}//Check reprojection error in second keyframeconst float sigmaSquare2 = pKF2->mvLevelSigma2[kp2.octave];const float x2 = Rcw2.row(0).dot(x3D)+tcw2(0);const float y2 = Rcw2.row(1).dot(x3D)+tcw2(1);const float invz2 = 1.0/z2;if(!bStereo2){cv::Point2f uv2 = pCamera2->project(cv::Point3f(x2,y2,z2));float errX2 = uv2.x - kp2.pt.x;float errY2 = uv2.y - 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 consistencyEigen::Vector3f normal1 = x3D - Ow1;float dist1 = normal1.norm();Eigen::Vector3f normal2 = x3D - Ow2;float dist2 = normal2.norm();if(dist1==0 || dist2==0)continue;if(mbFarPoints && (dist1>=mThFarPoints||dist2>=mThFarPoints)) // MODIFICATIONcontinue;const float ratioDist = dist2/dist1;const float ratioOctave = mpCurrentKeyFrame->mvScaleFactors[kp1.octave]/pKF2->mvScaleFactors[kp2.octave];if(ratioDist*ratioFactor<ratioOctave || ratioDist>ratioOctave*ratioFactor)continue;// Triangulation is succesfullMapPoint* pMP = new MapPoint(x3D, mpCurrentKeyFrame, mpAtlas->GetCurrentMap());if (bPointStereo)countStereo++;pMP->AddObservation(mpCurrentKeyFrame,idx1);pMP->AddObservation(pKF2,idx2);mpCurrentKeyFrame->AddMapPoint(pMP,idx1);pKF2->AddMapPoint(pMP,idx2);pMP->ComputeDistinctiveDescriptors();pMP->UpdateNormalAndDepth();mpAtlas->AddMapPoint(pMP);mlpRecentAddedMapPoints.push_back(pMP);}

这篇关于ORB_SLAM3 LocalMapping中CreateNewMapPoints的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---