文章目录
一、函数意义
这两个函数是局部见图的核心函数之二，作用是删除不好的地图点，为创造新的地图点。学习局部建图线程和跟踪线程一样，先要知道什么是局部见图，局部建图就是优化局部的地图点，包括删除不好的地图点和增加新的地图点，以及后续对地图点位置的优化调整。大家都知道，SLAM是即时定位与实时建图，在ORB-SLAM2中，追踪线程主要负责即时定位，局部建图线程主要负责实时建图。
二、LocalMapping::MapPointCulling()
1. 函数讲解
本函数的作用是，筛选需要筛选的点，将不符合要求的地图点删除。需要筛选地图点的列表在下面那个函数中产生。这里在代码中是先运行MapPointCulling()，后运行CreateNewMapPoints()，但实际上是先创建新的地图点，然后检查新的地图点是否符合要求。这里删除的地图点有三种
- 被标记为isBad的点
- 跟踪到该点的帧数远远小于预计跟踪到它的帧数（0.25倍）
- 该点创建到现在已经经历了两个关键帧，但是观测数值小于阈值（单目2次，双目3次）
2. 函数代码
/**
* @brief 检查新增地图点，根据地图点的观测情况剔除质量不好的新增的地图点
* mlpRecentAddedMapPoints：存储新增的地图点，这里是要删除其中不靠谱的
*/
void LocalMapping::MapPointCulling()
{
// Check Recent Added MapPoints
list<MapPoint*>::iterator lit = mlpRecentAddedMapPoints.begin();
const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;
// 设置观测次数的阈值
int nThObs;
if(mbMonocular)
nThObs = 2;
else
nThObs = 3;
const int cnThObs = nThObs;
// 遍历所有新增的地图点
while(lit!=mlpRecentAddedMapPoints.end())
{
// 获取地图点指针
MapPoint* pMP = *lit;
// 将坏点删除
if(pMP->isBad())
{
lit = mlpRecentAddedMapPoints.erase(lit);
}
// 跟踪到该地图点的帧数相比预计可观测到该地图点的帧数的比例小于25%，从地图中删除
else if(pMP->GetFoundRatio()<0.25f )
{
pMP->SetBadFlag();
lit = mlpRecentAddedMapPoints.erase(lit);
}
// 从该点建立开始，到现在已经过了不小于2个关键帧
// 但是观测到该点的相机数却不超过阈值cnThObs，从地图中删除
else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs)
{
pMP->SetBadFlag();
lit = mlpRecentAddedMapPoints.erase(lit);
}
// 从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点
// 因此没有SetBadFlag()，仅从队列中删除
else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)
lit = mlpRecentAddedMapPoints.erase(lit);
else
lit++;
}
}
三、LocalMapping::CreateNewMapPoints()
1. 函数讲解
本函数的作用是产生新的地图点，产生新地图点的方式是，先找到与当前关键帧相邻关系最好的关键帧（双目10个，单目20，寻找方式如下图所示），然后遍历这些关键帧，将它们与当前关键帧进行特征匹配，找出匹配的点（这里的匹配方式是词袋加速匹配，中见加了一个F12对极约束的判断条件）。找出匹配的点后，删除视角差不符合要求的点，然后将地图点投影到当前帧和相邻帧中，计算投影误差，删除掉投影误差大的点。最后计算两个关键帧相机中心到地图点世界坐标之间的模场，删除模长为0的点（几乎不可能，模长为0表示地图点和相机中心重合），然后计算两个模长的比值，以及带上金字塔尺度的比值，删除两种比值相差太大的点。意思是两帧相邻关系很好，不应该出现匹配的特征点对应的金字塔层级相差太大。接下来就是善后工作，给地图点添加能观测到它的关键帧，个关键帧添加这个地图点的观测。这些新生成的地图点并不是直接就能用，还要经过MapPointCulling()的筛选。这段代码看起来很长其实冗余信息很多，可以将其改到只有原来一半的长度，有兴趣的话可以自己改改看。
2. 函数代码
// 生成新的地图带点
void LocalMapping::CreateNewMapPoints()
{
// Retrieve neighbor keyframes in covisibility graph
// 设置共视关键帧数量，对于单目相机，
// 需要更多共视关系较好的关键帧来建立地图，因此如果是单目模式，将 nn 设置为 20，否则为 10。
int nn = 10;
if(mbMonocular)
nn=20;
// 得到与该关键帧连接的前N个最强共视关键帧(已按权值排序)，双目10个，单目20
const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
// 初始化特征匹配器
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);
// 将相机的旋转矩阵和平移向量组合成一个齐次变换矩阵Tcw1
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;
const float ratioFactor = 1.5f*mpCurrentKeyFrame->mfScaleFactor;
// 记录三角化成功的地图点数目
int nnew=0;
// Search matches with epipolar restriction and triangulate
// 遍历与该关键帧连接的前N个最强共视关键帧
for(size_t i=0; i<vpNeighKFs.size(); i++)
{
// 如果关键帧队列中成员数为0，则返回
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);
// 双目情况
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
// 根据两个关键帧的位姿计算它们之间的基础矩阵F
// 基础矩阵，通常记作F，描述了两个视图（或相机图像）之间的几何关系。
// 其主要作用是在两张图像中找到对应点（缩减匹配范围）
cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2);
// Search matches that fullfil epipolar constraint
// 创建匹配索引的容器
vector<pair<size_t,size_t> > vMatchedIndices;
// 利用基础矩阵F12极线约束，用BoW加速匹配两个关键帧的未匹配的特征点，产生新的匹配点对
// 但是这个函数没用用F12来加速匹配，而是当成了一个判断条件
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
// 对每对匹配通过三角化生成3D点,和 Triangulate函数差不多
// 遍历所有的匹配点
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->mvKeysUn[idx1];
// mvuRight中存放着双目的深度值，如果不是双目，其值将为-1
const float kp1_ur=mpCurrentKeyFrame->mvuRight[idx1];
// 判断双目的条件，无深度信息则为false，有正的深度信息则为true
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
// 特征点反投影,其实得到的是在各自相机坐标系下的一个非归一化的方向向量,和这个点的反投影射线重合
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);
// ray1 和 ray2 代表特征点在世界坐标系中的方向，起始于相机光心，指向该特征点。
cv::Mat ray1 = Rwc1*xn1;
cv::Mat ray2 = Rwc2*xn2;
// 计算视当前帧和邻接关键帧差角的余弦值：
// ray1.dot(ray2) 计算 ray1 和 ray2 之间的点积，
// cv::norm(ray1) 和 cv::norm(ray2) 分别为 ray1 和 ray2 的模长。
const float cosParallaxRays = ray1.dot(ray2)/(cv::norm(ray1)*cv::norm(ray2));
// cosParallaxStereo1 和 cosParallaxStereo2表示当前关键帧和相邻关键帧的“立体视差角余弦值”
// +1是为了初始化一个较大的值
float cosParallaxStereo = cosParallaxRays+1;
float cosParallaxStereo1 = cosParallaxStereo;
float cosParallaxStereo2 = cosParallaxStereo;
// 传感器是双目相机,并且当前的关键帧的这个点有对应的深度
// 假设是平行的双目相机，计算出双目相机观察这个点的时候的视差角余弦
// bStereo1为true，表示当前关键帧中有深度信息（双目或者RGBD）
if(bStereo1)
// 这个是以基线长度为对边，地图点在当前帧中的深度为邻边组成角的余弦值
cosParallaxStereo1 = cos(2*atan2(mpCurrentKeyFrame->mb/2,mpCurrentKeyFrame->mvDepth[idx1]));
// 传感器是双目相机,并且邻接的关键帧的这个点有对应的深度，和上面一样操作
// bStereo2为true，表示邻接关键帧中有深度信息（双目或者RGBD）
else if(bStereo2)
// 这个是以基线长度为对边，地图点在相邻关键帧中的深度为邻边组成角的余弦值
cosParallaxStereo2 = cos(2*atan2(pKF2->mb/2,pKF2->mvDepth[idx2]));
// 得到当前帧和邻接帧中视差最小的一个
cosParallaxStereo = min(cosParallaxStereo1,cosParallaxStereo2);
// 三角化恢复3D点
// 创建三角化的矩阵
cv::Mat x3D;
// cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998)表明视差角正常,0.9998 对应1°
// cosParallaxRays < cosParallaxStereo 表明匹配点对夹角大于双目本身观察三维点夹角
// 匹配点对夹角大，用三角法恢复3D点（重投影去除误差大的地图点）
// 将地图点的坐标通过矩阵转换的方式，将其投影到两个相机的坐标系下，
// 然后计算投影的点与地图点对应特征点的误差大小，误差超过阈值的点去除
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;
// Euclidean coordinates
// 归一化成为齐次坐标,然后提取前面三个维度作为欧式坐标
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
// 检测生成的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
// 计算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;
}
// 双目情况同理，多了一个右目的x轴误差
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
// 计算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
// 检查尺度连续性
// 世界坐标系下，3D点与当前关键帧相机间的向量，方向由相机指向3D点
cv::Mat normal1 = x3D-Ow1;
// 计算模长
float dist1 = cv::norm(normal1);
// 世界坐标系下，3D点与相邻关键帧相机间的向量，方向由相机指向3D点
cv::Mat normal2 = x3D-Ow2;
// 计算模长
float dist2 = cv::norm(normal2);
// 模长不能为0（点不能和相机中心重合）
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
// 三角化生成3D点成功，构造成MapPoint
MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap);
// 当前关键帧对该地图点添加观测
pMP->AddObservation(mpCurrentKeyFrame,idx1);
// 相邻关键帧对该地图点添加观测
pMP->AddObservation(pKF2,idx2);
// 将该地图点添加到当前关键帧的地图点列表中
mpCurrentKeyFrame->AddMapPoint(pMP,idx1);
// 将该地图点添加到上一关键帧的地图点列表中
pKF2->AddMapPoint(pMP,idx2);
// 计算地图点最具代表性的描述子
pMP->ComputeDistinctiveDescriptors();
// 这行代码更新地图点的法向量和深度信息
pMP->UpdateNormalAndDepth();
// 将新创建的地图点（pMP）添加到全局地图对象（mpMap）中
mpMap->AddMapPoint(pMP);
// 将新产生的点放入检测队列
// 这些MapPoints都会经过MapPointCulling函数的检验
mlpRecentAddedMapPoints.push_back(pMP);
// 新创建的地图点计数器nnew加1
nnew++;
}
}
}
四、总结
标签：关键帧,MapPointCulling,const,mpCurrentKeyFrame,LocalMapping,float,地图,----,cv From： https://blog.csdn.net/uzi_ccc/article/details/144113139这两个函数相当于局部见图最开始的部分，利用新产生的关键帧进行局部建图，后续还会对地图点和关键帧进行处理，函数里面最难理解的部分是视角差那部分，但你带着答案去看这一部分就不会觉得很难了。我们的相机拍摄频率是很高的，两个相邻的关键帧之间的相机运动不会太大，所以视角差不会太大，如果视角差过大，说明匹配错了，这个地图点就不应该生成。