CreateNewMapPoints在新插入的关键帧的邻近的关键帧中，通过词袋模型与新插入关键帧中没有匹配的ORB特征进行匹配，并抛弃其中不满足对极约束的匹配点对，接着通过三角化生成地图点
GetBestCovisibilityKeyFrames：找出与当前关键帧共视程度最高前nn帧vpNeighKFs
如果是IMU模式，在vpNeighKFs不够nn帧的情况下，将当前关键帧的前n帧prevKF依次添加到vpNeighKFs中
// Retrieve neighbor keyframes in covisibility graph
int nn = 10;
// For stereo inertial case
if(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;
}
}
vpNeighKFs与当前关键帧的基线：基线： b a s e l i n e = ∣ O w 2 ? O w 1 ∣ < b baseline = \left|Ow_{2} -Ow_{1}\right| < b baseline=∣Ow2??Ow1?∣<b
KeyFrame* pKF2 = vpNeighKFs[i];
GeometricCamera* pCamera1 = mpCurrentKeyFrame->mpCamera, *pCamera2 = pKF2->mpCamera;
// Check first that baseline is not too short
Eigen::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;
}
SearchForTriangulation主要用于LocalMapping生成新的地图点，通过寻找当前关键帧和相邻关键帧中未匹配的特征点之间的匹配关系，主要用Bow来加速搜索，以及极线校验来判断是否能够生成地图点，其输入参数如下：
| 参数 | 说明 |
|---|---|
| pkF1 | 当前关键帧 |
| pkF2 | 邻近关键帧 |
| vMatchedPairs | 匹配关系 |
| bOnlyStereo | 双目 |
| bCoarse |
Sophus::SE3f T1w = pKF1->GetPose();
Sophus::SE3f T2w = pKF2->GetPose();
Sophus::SE3f Tw2 = pKF2->GetPoseInverse(); // for convenience
Eigen::Vector3f Cw = pKF1->GetCameraCenter();
Eigen::Vector3f C2 = T2w * Cw;
Eigen::Vector2f ep = pKF2->mpCamera->project(C2);
vbMatched2[idx2]TH_LOW epipolarConstrainbestDist，以及最佳的匹配bestIdx2 Sophus::SE3f T12;
Sophus::SE3f Tll, Tlr, Trl, Trr;
Eigen::Matrix3f R12; // for fastest computation
Eigen::Vector3f t12; // for fastest computation
GeometricCamera* 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 node
int 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 skip
if(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 skip
if(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
?
t
12
∧
?
R
12
?
K
2
?
1
F_{12} = K_1^{-T}\cdot t_{12}^{\wedge}\cdot R_{12}\cdot K_{2}^{-1}
F12?=K1?T??t12∧??R12??K2?1?
l = x 1 T ? F 12 l = x_{1}^T\cdot F_{12} l=x1T??F12?
d = a ? a + b ? b a x 2 + b y 2 + c d=\frac{a*a + b*b}{ax_2+by_2+c} d=ax2?+by2?+ca?a+b?b?
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]));
}
检查视差角是否满足要求
cosParallaxRays：
c
o
s
θ
=
(
R
w
c
1
?
x
1
)
?
(
R
w
c
2
?
x
2
)
∣
R
w
c
1
?
x
1
∣
?
∣
R
w
c
2
?
x
2
∣
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|}
cosθ=∣Rwc1??x1?∣?∣Rwc2??x2?∣(Rwc1??x1?)?(Rwc2??x2?)?
cosParallaxStereo：
c
o
s
(
2
arctan
?
(
b
2
d
)
)
cos\left(2 \arctan\left(\frac{\frac{b}{2}}{d}\right)\right)
cos(2arctan(d2b??))
通过三角化对每对匹配点生成新的3D点
检查3D点是否在相机前方： z > 0 z > 0 z>0
判断3D点在关键帧下的重投影误差
检查尺度是否合理： d i s t 1 d i s t 2 ≈ σ 1 σ 2 \frac{dist_1}{dist_2}\approx \frac{\sigma _{1}}{\sigma _{2}} dist2?dist1??≈σ2?σ1??
将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 match
const 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 rays
Eigen::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 cameras
float 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 keyframe
const 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 keyframe
const 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 consistency
Eigen::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)) // MODIFICATION
continue;
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 succesfull
MapPoint* 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);
}