From 3b42c8294463fafc2319974a3178405e68d22f0f Mon Sep 17 00:00:00 2001 From: laobai Date: Thu, 7 Apr 2022 23:28:17 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E5=8C=B9=E9=85=8D=E4=BB=A3?= =?UTF-8?q?=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/ORBmatcher.cc | 489 ++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 409 insertions(+), 80 deletions(-) diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc index 9129683..16daa97 100644 --- a/src/ORBmatcher.cc +++ b/src/ORBmatcher.cc @@ -36,16 +36,20 @@ namespace ORB_SLAM3 const int ORBmatcher::TH_LOW = 50; const int ORBmatcher::HISTO_LENGTH = 30; + // 构造函数,参数默认值为0.6,true ORBmatcher::ORBmatcher(float nnratio, bool checkOri): mfNNratio(nnratio), mbCheckOrientation(checkOri) { } + // 用于Tracking::SearchLocalPoints中匹配更多地图点 int ORBmatcher::SearchByProjection(Frame &F, const vector &vpMapPoints, const float th, const bool bFarPoints, const float thFarPoints) { int nmatches=0, left = 0, right = 0; + // 如果 th！=1 (RGBD 相机或者刚刚进行过重定位), 需要扩大范围搜索 const bool bFactor = th!=1.0; + // Step 1 遍历有效的局部地图点 for(size_t iMP=0; iMPmbTrackInView) { + // 通过距离预测的金字塔层数，该层数相对于当前的帧 const int &nPredictedLevel = pMP->mnTrackScaleLevel; // The size of the window will depend on the viewing direction + // Step 2 设定搜索搜索窗口的大小。取决于视角, 若当前视角和平均视角夹角较小时, r取一个较小的值 float r = RadiusByViewingCos(pMP->mTrackViewCos); + // 如果需要扩大范围搜索，则乘以阈值th if(bFactor) r*=th; + // Step 3 通过投影点以及搜索窗口和预测的尺度进行搜索, 找出搜索半径内的候选匹配点索引 const vector vIndices = - F.GetFeaturesInArea(pMP->mTrackProjX,pMP->mTrackProjY,r*F.mvScaleFactors[nPredictedLevel],nPredictedLevel-1,nPredictedLevel); + F.GetFeaturesInArea(pMP->mTrackProjX,pMP->mTrackProjY, // 该地图点投影到一帧上的坐标 + r*F.mvScaleFactors[nPredictedLevel], // 认为搜索窗口的大小和该特征点被追踪到时所处的尺度也有关系 + nPredictedLevel-1,nPredictedLevel); // 搜索的图层范围 + // 没找到候选的,就放弃对当前点的匹配 if(!vIndices.empty()){ const cv::Mat MPdescriptor = pMP->GetDescriptor(); + // 最优的次优的描述子距离和index int bestDist=256; int bestLevel= -1; int bestDist2=256; @@ -81,25 +93,35 @@ namespace ORB_SLAM3 int bestIdx =-1 ; // Get best and second matches with near keypoints + // Step 4 寻找候选匹配点中的最佳和次佳匹配点 for(vector::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++) { const size_t idx = *vit; + // 如果Frame中的该兴趣点已经有对应的MapPoint了,则退出该次循环 if(F.mvpMapPoints[idx]) if(F.mvpMapPoints[idx]->Observations()>0) continue; + //如果是双目数据 if(F.Nleft == -1 && F.mvuRight[idx]>0) { + //计算在X轴上的投影误差 const float er = fabs(pMP->mTrackProjXR-F.mvuRight[idx]); + //超过阈值,说明这个点不行,丢掉. + //这里的阈值定义是以给定的搜索范围r为参考,然后考虑到越近的点(nPredictedLevel越大), 相机运动时对其产生的影响也就越大, + //因此需要扩大其搜索空间. + //当给定缩放倍率为1.2的时候, mvScaleFactors 中的数据是: 1 1.2 1.2^2 1.2^3 ... if(er>r*F.mvScaleFactors[nPredictedLevel]) continue; } const cv::Mat &d = F.mDescriptors.row(idx); + // 计算地图点和候选投影点的描述子距离 const int dist = DescriptorDistance(MPdescriptor,d); + // 寻找描述子距离最小和次小的特征点和索引 if(distmfNNratio*bestDist2 表示最佳和次佳距离不满足阈值比例。理论来说 bestDist/bestDist2 越小越好 if(bestLevel==bestLevel2 && bestDist>mfNNratio*bestDist2) continue; @@ -212,30 +238,49 @@ namespace ORB_SLAM3 return nmatches; } + // 根据观察的视角来计算匹配的时的搜索窗口大小 float ORBmatcher::RadiusByViewingCos(const float &viewCos) { + // 当视角相差小于3.6°，对应cos(3.6°)=0.998，搜索范围是2.5，否则是4 if(viewCos>0.998) return 2.5; else return 4.0; } + /** + * @brief 通过词袋，对关键帧的特征点进行跟踪 + * + * @param[in] pKF 关键帧 + * @param[in] F 当前普通帧 + * @param[in] vpMapPointMatches F中地图点对应的匹配，NULL表示未匹配 + * @return int 成功匹配的数量 + */ int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector &vpMapPointMatches) { + // 获取该关键帧的地图点 const vector vpMapPointsKF = pKF->GetMapPointMatches(); + // 和普通帧F特征点的索引一致 vpMapPointMatches = vector(F.N,static_cast(NULL)); + // 取出关键帧的词袋特征向量 const DBoW2::FeatureVector &vFeatVecKF = pKF->mFeatVec; int nmatches=0; + // 特征点角度旋转差统计用的直方图 vector rotHist[HISTO_LENGTH]; for(int i=0;ifirst == Fit->first) + // Step 1：分别取出属于同一node的ORB特征点(只有属于同一node，才有可能是匹配点) + // first 元素就是node id，遍历 + if(KFit->first == Fit->first) { + // second 是该node内存储的feature index const vector vIndicesKF = KFit->second; const vector vIndicesF = Fit->second; + // Step 2：遍历KF中属于该node的特征点 for(size_t iKF=0; iKFisBad()) continue; + // 取出关键帧KF中该特征对应的描述子 + const cv::Mat &dKF= pKF->mDescriptors.row(realIdxKF); - const cv::Mat &dKF= pKF->mDescriptors.row(realIdxKF); - - int bestDist1=256; + int bestDist1=256; // 最好的距离（最小距离） int bestIdxF =-1 ; - int bestDist2=256; + int bestDist2=256; // 次好距离（倒数第二小距离） int bestDist1R=256; int bestIdxFR =-1 ; int bestDist2R=256; - + // Step 3：遍历F中属于该node的特征点，寻找最佳匹配点 for(size_t iF=0; iF(bestDist1)(bestDist2)) { + // Step 4.3：记录成功匹配特征点的对应的地图点(来自关键帧) vpMapPointMatches[bestIdxF]=pMP; + // 这里的realIdxKF是当前遍历到的关键帧的特征点id const cv::KeyPoint &kp = (!pKF->mpCamera2) ? pKF->mvKeysUn[realIdxKF] : (realIdxKF >= pKF -> NLeft) ? pKF -> mvKeysRight[realIdxKF - pKF -> NLeft] : pKF -> mvKeys[realIdxKF]; - + // Step 4.4：计算匹配点旋转角度差所在的直方图 if(mbCheckOrientation) { cv::KeyPoint &Fkp = (!pKF->mpCamera2 || F.Nleft == -1) ? F.mvKeys[bestIdxF] : (bestIdxF >= F.Nleft) ? F.mvKeysRight[bestIdxF - F.Nleft] - : F.mvKeys[bestIdxF]; - + : F.mvKeys[bestIdxF]; + // 所有的特征点的角度变化应该是一致的，通过直方图统计得到最准确的角度变化值 float rot = kp.angle-Fkp.angle; if(rot<0.0) rot+=360.0f; - int bin = round(rot*factor); + int bin = round(rot*factor);// 将rot分配到bin组, 四舍五入, 其实就是离散到对应的直方图组中 if(bin==HISTO_LENGTH) bin=0; assert(bin>=0 && bin= F.Nleft) ? F.mvKeysRight[bestIdxFR - F.Nleft] - : F.mvKeys[bestIdxFR]; + : F.mvKeys[bestIdxFR]; float rot = kp.angle-Fkp.angle; if(rot<0.0) @@ -393,26 +453,34 @@ namespace ORB_SLAM3 } else if(KFit->first < Fit->first) { + // 对齐 KFit = vFeatVecKF.lower_bound(Fit->first); } else { + // 对齐 Fit = F.mFeatVec.lower_bound(KFit->first); } } + // Step 5 根据方向剔除误匹配的点 if(mbCheckOrientation) { + // index int ind1=-1; int ind2=-1; int ind3=-1; + // 筛选出在旋转角度差落在在直方图区间内数量最多的前三个bin的索引 ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3); for(int i=0; i(NULL); @@ -433,31 +501,39 @@ namespace ORB_SLAM3 const float &cx = pKF->cx; const float &cy = pKF->cy; + // Decompose Scw + // Step 1 分解Sim变换矩阵 + //? 为什么要剥离尺度信息？ Sophus::SE3f Tcw = Sophus::SE3f(Scw.rotationMatrix(),Scw.translation()/Scw.scale()); - Eigen::Vector3f Ow = Tcw.inverse().translation(); + Eigen::Vector3f Ow = Tcw.inverse().translation(); // 世界坐标系下相机光心坐标 // Set of MapPoints already found in the KeyFrame + // 使用set类型，记录前面已经成功的匹配关系，避免重复匹配。并去除其中无效匹配关系（NULL） set spAlreadyFound(vpMatched.begin(), vpMatched.end()); spAlreadyFound.erase(static_cast(NULL)); int nmatches=0; // For each Candidate MapPoint Project and Match + // Step 2 遍历闭环KF及其共视KF的所有地图点（不考虑当前KF已经匹配的地图点）投影到当前KF for(int iMP=0, iendMP=vpPoints.size(); iMPisBad() || spAlreadyFound.count(pMP)) continue; // Get 3D Coords. + // Step 2.2 投影到当前KF的图像坐标并判断是否有效 Eigen::Vector3f p3Dw = pMP->GetWorldPos(); // Transform into Camera Coords. Eigen::Vector3f p3Dc = Tcw * p3Dw; // Depth must be positive + // 深度值必须为正 if(p3Dc(2)<0.0) continue; @@ -465,12 +541,15 @@ namespace ORB_SLAM3 const Eigen::Vector2f uv = pKF->mpCamera->project(p3Dc); // Point must be inside the image + // 在图像范围内 if(!pKF->IsInImage(uv(0),uv(1))) continue; // Depth must be inside the scale invariance region of the point + // 判断距离是否在有效距离内 const float maxDistance = pMP->GetMaxDistanceInvariance(); const float minDistance = pMP->GetMinDistanceInvariance(); + // 地图点到相机光心的向量 Eigen::Vector3f PO = p3Dw-Ow; const float dist = PO.norm(); @@ -478,16 +557,20 @@ namespace ORB_SLAM3 continue; // Viewing angle must be less than 60 deg + // 观察角度小于60° Eigen::Vector3f Pn = pMP->GetNormal(); if(PO.dot(Pn)<0.5*dist) continue; + // 根据当前这个地图点距离当前KF光心的距离,预测该点在当前KF中的尺度(图层) int nPredictedLevel = pMP->PredictScale(dist,pKF); // Search in a radius + // 根据尺度确定搜索半径 const float radius = th*pKF->mvScaleFactors[nPredictedLevel]; + // Step 2.3 搜索候选匹配点 const vector vIndices = pKF->GetFeaturesInArea(uv(0),uv(1),radius); if(vIndices.empty()) @@ -498,6 +581,7 @@ namespace ORB_SLAM3 int bestDist = 256; int bestIdx = -1; + // Step 2.4 遍历候选匹配点，找到最佳匹配点 for(vector::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++) { const size_t idx = *vit; @@ -506,6 +590,7 @@ namespace ORB_SLAM3 const int &kpLevel= pKF->mvKeysUn[idx].octave; + // 不在一个尺度也不行 if(kpLevelnPredictedLevel) continue; @@ -520,6 +605,7 @@ namespace ORB_SLAM3 } } + // 该MapPoint与bestIdx对应的特征点匹配成功 if(bestDist<=TH_LOW*ratioHamming) { vpMatched[bestIdx]=pMP; @@ -527,7 +613,7 @@ namespace ORB_SLAM3 } } - + // Step 3 返回新的成功匹配的点对的数目 return nmatches; } @@ -648,45 +734,62 @@ namespace ORB_SLAM3 int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector &vbPrevMatched, vector &vnMatches12, int windowSize) { int nmatches=0; + // F1中特征点和F2中匹配关系，注意是按照F1特征点数目分配空间 vnMatches12 = vector(F1.mvKeysUn.size(),-1); + // Step 1 构建旋转直方图，HISTO_LENGTH = 30 vector rotHist[HISTO_LENGTH]; + // 每个bin里预分配500个，因为使用的是vector不够的话可以自动扩展容量 for(int i=0;i vMatchedDistance(F2.mvKeysUn.size(),INT_MAX); + // 从帧2到帧1的反向匹配，注意是按照F2特征点数目分配空间 vector vnMatches21(F2.mvKeysUn.size(),-1); + // 遍历帧1中的所有特征点 for(size_t i1=0, iend1=F1.mvKeysUn.size(); i10) continue; + // Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点 + // vbPrevMatched 输入的是参考帧 F1的特征点 + // windowSize = 100，输入最大最小金字塔层级 均为0 vector vIndices2 = F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1); + // 没有候选特征点，跳过 if(vIndices2.empty()) continue; + // 取出参考帧F1中当前遍历特征点对应的描述子 cv::Mat d1 = F1.mDescriptors.row(i1); - int bestDist = INT_MAX; - int bestDist2 = INT_MAX; - int bestIdx2 = -1; + int bestDist = INT_MAX; //最佳描述子匹配距离，越小越好 + int bestDist2 = INT_MAX; //次佳描述子匹配距离 + int bestIdx2 = -1; //最佳候选特征点在F2中的index + // Step 3 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的 for(vector::iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++) { size_t i2 = *vit; - + // 取出候选特征点对应的描述子 cv::Mat d2 = F2.mDescriptors.row(i2); - + // 计算两个特征点描述子距离 int dist = DescriptorDistance(d1,d2); if(vMatchedDistance[i2]<=dist) continue; - + // 如果当前匹配距离更小，更新最佳次佳距离 if(dist=0) { vnMatches12[vnMatches21[bestIdx2]]=-1; nmatches--; } + // 次优的匹配关系，双向建立 + // vnMatches12保存参考帧F1和F2匹配关系，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引 vnMatches12[i1]=bestIdx2; vnMatches21[bestIdx2]=i1; vMatchedDistance[bestIdx2]=bestDist; nmatches++; + // Step 5 计算匹配点旋转角度差所在的直方图 if(mbCheckOrientation) { + // 计算匹配特征点的角度差，这里单位是角度°，不是弧度 float rot = F1.mvKeysUn[i1].angle-F2.mvKeysUn[bestIdx2].angle; if(rot<0.0) rot+=360.0f; + // 前面factor = HISTO_LENGTH/360.0f + // bin = rot / 360.of * HISTO_LENGTH 表示当前rot被分配在第几个直方图bin int bin = round(rot*factor); + // 如果bin 满了又是一个轮回 if(bin==HISTO_LENGTH) bin=0; assert(bin>=0 && bin=0) vbPrevMatched[i1]=F2.mvKeysUn[vnMatches12[i1]].pt; @@ -762,8 +879,19 @@ namespace ORB_SLAM3 return nmatches; } + /* + * @brief 通过词袋，对关键帧的特征点进行跟踪，该函数用于闭环检测时两个关键帧间的特征点匹配 + * @details 通过bow对pKF和F中的特征点进行快速匹配（不属于同一node的特征点直接跳过匹配） + * 对属于同一node的特征点通过描述子距离进行匹配 + * 通过距离阈值、比例阈值和角度投票进行剔除误匹配 + * @param pKF1 KeyFrame1 + * @param pKF2 KeyFrame2 + * @param vpMatches12 pKF2中与pKF1匹配的MapPoint，vpMatches12[i]表示匹配的地图点，null表示没有匹配，i表示匹配的pKF1 特征点索引 + * @return 成功匹配的数量 + */ int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector &vpMatches12) { + // Step 1 分别取出两个关键帧的特征点、BoW 向量、地图点、描述子 const vector &vKeysUn1 = pKF1->mvKeysUn; const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec; const vector vpMapPoints1 = pKF1->GetMapPointMatches(); @@ -774,13 +902,17 @@ namespace ORB_SLAM3 const vector vpMapPoints2 = pKF2->GetMapPointMatches(); const cv::Mat &Descriptors2 = pKF2->mDescriptors; + // 保存匹配结果 vpMatches12 = vector(vpMapPoints1.size(),static_cast(NULL)); vector vbMatched2(vpMapPoints2.size(),false); + // Step 2 构建旋转直方图，HISTO_LENGTH = 30 vector rotHist[HISTO_LENGTH]; for(int i=0;ifirst == f2it->first) { + // 遍历KF中属于该node的特征点 for(size_t i1=0, iend1=f1it->second.size(); i1second[i1]; @@ -813,6 +947,7 @@ namespace ORB_SLAM3 int bestIdx2 =-1 ; int bestDist2=256; + // Step 4 遍历KF2中属于该node的特征点，找到了最优及次优匹配点 for(size_t i2=0, iend2=f2it->second.size(); i2second[i2]; @@ -823,6 +958,7 @@ namespace ORB_SLAM3 MapPoint* pMP2 = vpMapPoints2[idx2]; + // 如果已经有匹配的点，或者遍历到的特征点对应的地图点无效 if(vbMatched2[idx2] || !pMP2) continue; @@ -845,6 +981,7 @@ namespace ORB_SLAM3 } } + // Step 5 对匹配结果进行检查，满足阈值、最优/次优比例，记录旋转直方图信息 if(bestDist1(bestDist1)(bestDist2)) @@ -881,6 +1018,7 @@ namespace ORB_SLAM3 } } + // Step 6 检查旋转直方图分布，剔除差异较大的匹配 if(mbCheckOrientation) { int ind1=-1; @@ -911,6 +1049,7 @@ namespace ORB_SLAM3 const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec; //Compute epipole in second image + // Step 1 计算KF1的相机中心在KF2图像平面的二维像素坐标 Sophus::SE3f T1w = pKF1->GetPose(); Sophus::SE3f T2w = pKF2->GetPose(); Sophus::SE3f Tw2 = pKF2->GetPoseInverse(); // for convenience @@ -938,69 +1077,89 @@ namespace ORB_SLAM3 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 vbMatched2(pKF2->N,false); - vector vMatches12(pKF1->N,-1); + int nmatches=0; + // 记录匹配是否成功，避免重复匹配 + vector vbMatched2(pKF2->N,false); + vector vMatches12(pKF1->N,-1); + // 用于统计匹配点对旋转差的直方图 vector rotHist[HISTO_LENGTH]; for(int i=0;ifirst对应node编号，f1it->second对应属于该node的所有特特征点编号 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(); + // Step 2.1：遍历pKF1和pKF2中的node节点 while(f1it!=f1end && f2it!=f2end) { + // 如果f1it和f2it属于同一个node节点才会进行匹配，这就是BoW加速匹配原理 if(f1it->first == f2it->first) { + // Step 2.2：遍历属于同一node节点(id：f1it->first)下的所有特征点 for(size_t i1=0, iend1=f1it->second.size(); i1second[i1]; - + + // Step 2.3：通过特征点索引idx1在pKF1中取出对应的MapPoint MapPoint* pMP1 = pKF1->GetMapPoint(idx1); - + // If there is already a MapPoint skip + // 由于寻找的是未匹配的特征点，所以pMP1应该为NULL if(pMP1) { continue; } - + // 如果mvuRight中的值大于0，表示是双目，且该特征点有深度值 const bool bStereo1 = (!pKF1->mpCamera2 && pKF1->mvuRight[idx1]>=0); if(bOnlyStereo) if(!bStereo1) continue; + // Step 2.4：通过特征点索引idx1在pKF1中取出对应的特征点 const cv::KeyPoint &kp1 = (pKF1 -> NLeft == -1) ? pKF1->mvKeysUn[idx1] : (idx1 < pKF1 -> NLeft) ? pKF1 -> mvKeys[idx1] - : pKF1 -> mvKeysRight[idx1 - pKF1 -> NLeft]; + : pKF1 -> mvKeysRight[idx1 - pKF1 -> NLeft]; const bool bRight1 = (pKF1 -> NLeft == -1 || idx1 < pKF1 -> NLeft) ? false - : true; - + : true; + //if(bRight1) continue; + // 通过特征点索引idx1在pKF1中取出对应的特征点的描述子 const cv::Mat &d1 = pKF1->mDescriptors.row(idx1); - + int bestDist = TH_LOW; int bestIdx2 = -1; - + + // Step 2.5：遍历该node节点下(f2it->first)对应KF2中的所有特征点 for(size_t i2=0, iend2=f2it->second.size(); i2second[i2]; - + + // 通过特征点索引idx2在pKF2中取出对应的MapPoint MapPoint* pMP2 = pKF2->GetMapPoint(idx2); - + // If we have already matched or there is a MapPoint skip + // 如果pKF2当前特征点索引idx2已经被匹配过或者对应的3d点非空，那么跳过这个索引idx2 if(vbMatched2[idx2] || pMP2) continue; @@ -1009,24 +1168,32 @@ namespace ORB_SLAM3 if(bOnlyStereo) if(!bStereo2) continue; - + + // 通过特征点索引idx2在pKF2中取出对应的特征点的描述子 const cv::Mat &d2 = pKF2->mDescriptors.row(idx2); - + + // Step 2.6 计算idx1与idx2在两个关键帧中对应特征点的描述子距离 const int dist = DescriptorDistance(d1,d2); - + if(dist>TH_LOW || dist>bestDist) continue; + // 通过特征点索引idx2在pKF2中取出对应的特征点 const cv::KeyPoint &kp2 = (pKF2 -> NLeft == -1) ? pKF2->mvKeysUn[idx2] : (idx2 < pKF2 -> NLeft) ? pKF2 -> mvKeys[idx2] - : pKF2 -> mvKeysRight[idx2 - pKF2 -> NLeft]; + : pKF2 -> mvKeysRight[idx2 - pKF2 -> NLeft]; const bool bRight2 = (pKF2 -> NLeft == -1 || idx2 < pKF2 -> NLeft) ? false - : true; - + : true; + //? 为什么双目就不需要判断像素点到极点的距离的判断？ + // 因为双目模式下可以左右互匹配恢复三维点 if(!bStereo1 && !bStereo2 && !pKF1->mpCamera2) { const float distex = ep(0)-kp2.pt.x; const float distey = ep(1)-kp2.pt.y; + // Step 2.7 极点e2到kp2的像素距离如果小于阈值th,认为kp2对应的MapPoint距离pKF1相机太近，跳过该匹配点对 + // 作者根据kp2金字塔尺度因子(scale^n，scale=1.2，n为层数)定义阈值th + // 金字塔层数从0到7，对应距离 sqrt(100*pKF2->mvScaleFactors[kp2.octave]) 是10-20个像素 + //? 对这个阈值的有效性持怀疑态度 if(distex*distex+distey*distey<100*pKF2->mvScaleFactors[kp2.octave]) { continue; @@ -1069,23 +1236,30 @@ namespace ORB_SLAM3 } + // Step 2.8 计算特征点kp2到kp1对应极线的距离是否小于阈值 if(bCoarse || pCamera1->epipolarConstrain(pCamera2,kp1,kp2,R12,t12,pKF1->mvLevelSigma2[kp1.octave],pKF2->mvLevelSigma2[kp2.octave])) // MODIFICATION_2 { + // bestIdx2，bestDist 是 kp1 对应 KF2中的最佳匹配点 index及匹配距离 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]; + : pKF2 -> mvKeysRight[bestIdx2 - pKF2 -> NLeft]; + // 记录匹配结果 vMatches12[idx1]=bestIdx2; + // !记录已经匹配，避免重复匹配。原作者漏掉！可以添加下面代码 + // vbMatched2[bestIdx2]=true; nmatches++; + // 记录旋转差直方图信息 if(mbCheckOrientation) { + // angle：角度，表示匹配点对的方向差。 float rot = kp1.angle-kp2.angle; if(rot<0.0) rot+=360.0f; @@ -1111,6 +1285,7 @@ namespace ORB_SLAM3 } } + // Step 3 用旋转差直方图来筛掉错误匹配对 if(mbCheckOrientation) { int ind1=-1; @@ -1124,7 +1299,8 @@ namespace ORB_SLAM3 if(i==ind1 || i==ind2 || i==ind3) continue; for(size_t j=0, jend=rotHist[i].size(); jisBad()) { count_bad++; @@ -1195,21 +1374,24 @@ namespace ORB_SLAM3 continue; } + // 将地图点变换到关键帧的相机坐标系下 Eigen::Vector3f p3Dw = pMP->GetWorldPos(); Eigen::Vector3f p3Dc = Tcw * p3Dw; // Depth must be positive + // 深度值为负，跳过 if(p3Dc(2)<0.0f) { count_negdepth++; continue; } - + // Step 2 得到地图点投影到关键帧的图像坐标 const float invz = 1/p3Dc(2); const Eigen::Vector2f uv = pCamera->project(p3Dc); // Point must be inside the image + // 投影点需要在有效范围内 if(!pKF->IsInImage(uv(0),uv(1))) { count_notinim++; @@ -1224,12 +1406,14 @@ namespace ORB_SLAM3 const float dist3D = PO.norm(); // Depth must be inside the scale pyramid of the image + // Step 3 地图点到关键帧相机光心距离需满足在有效范围内 if(dist3DmaxDistance) { count_dist++; continue; } // Viewing angle must be less than 60 deg + // Step 4 地图点的平均观测方向（正视程度）要小于60° Eigen::Vector3f Pn = pMP->GetNormal(); if(PO.dot(Pn)<0.5*dist3D) @@ -1241,8 +1425,9 @@ namespace ORB_SLAM3 int nPredictedLevel = pMP->PredictScale(dist3D,pKF); // Search in a radius + // 根据MapPoint的深度确定尺度，从而确定搜索范围 const float radius = th*pKF->mvScaleFactors[nPredictedLevel]; - + // Step 5 在投影点附近搜索窗口内找到候选匹配点的索引 const vector vIndices = pKF->GetFeaturesInArea(uv(0),uv(1),radius,bRight); if(vIndices.empty()) @@ -1252,7 +1437,7 @@ namespace ORB_SLAM3 } // Match to the most similar keypoint in the radius - + // Step 6 遍历寻找最佳匹配点（最小描述子距离） const cv::Mat dMP = pMP->GetDescriptor(); int bestDist = 256; @@ -1261,36 +1446,42 @@ namespace ORB_SLAM3 { size_t idx = *vit; const cv::KeyPoint &kp = (pKF -> NLeft == -1) ? pKF->mvKeysUn[idx] - : (!bRight) ? pKF -> mvKeys[idx] - : pKF -> mvKeysRight[idx]; + : (!bRight) ? pKF -> mvKeys[idx] + : pKF -> mvKeysRight[idx]; const int &kpLevel= kp.octave; - + // 金字塔层级要接近（同一层或小一层），否则跳过 if(kpLevelnPredictedLevel) continue; + // 计算投影点与候选匹配特征点的距离，如果偏差很大，直接跳过 if(pKF->mvuRight[idx]>=0) { // Check reprojection error in stereo + // 双目情况 const float &kpx = kp.pt.x; const float &kpy = kp.pt.y; const float &kpr = pKF->mvuRight[idx]; const float ex = uv(0)-kpx; const float ey = uv(1)-kpy; + // 右目数据的偏差也要考虑进去 const float er = ur-kpr; const float e2 = ex*ex+ey*ey+er*er; + //自由度为3, 误差小于1个像素,这种事情95%发生的概率对应卡方检验阈值为7.82 if(e2*pKF->mvInvLevelSigma2[kpLevel]>7.8) continue; } else { + // 单目情况 const float &kpx = kp.pt.x; const float &kpy = kp.pt.y; const float ex = uv(0)-kpx; const float ey = uv(1)-kpy; const float e2 = ex*ex+ey*ey; + // 自由度为2的，卡方检验阈值5.99（假设测量有一个像素的偏差） if(e2*pKF->mvInvLevelSigma2[kpLevel]>5.99) continue; } @@ -1301,7 +1492,7 @@ namespace ORB_SLAM3 const int dist = DescriptorDistance(dMP,dKF); - if(distGetMapPoint(bestIdx); if(pMPinKF) { + // 如果最佳匹配点有对应有效地图点，选择被观测次数最多的那个替换 if(!pMPinKF->isBad()) { if(pMPinKF->Observations()>pMP->Observations()) @@ -1324,6 +1518,7 @@ namespace ORB_SLAM3 } else { + // 如果最佳匹配点没有对应地图点，添加观测信息 pMP->AddObservation(pKF,bestIdx); pKF->AddMapPoint(pMP,bestIdx); } @@ -1337,6 +1532,17 @@ namespace ORB_SLAM3 return nFused; } + + /** + * @brief 闭环矫正中使用。将当前关键帧闭环匹配上的关键帧及其共视关键帧组成的地图点投影到当前关键帧，融合地图点 + * + * @param[in] pKF 当前关键帧 + * @param[in] Scw 当前关键帧经过闭环Sim3 后的世界到相机坐标系的Sim变换 + * @param[in] vpPoints 与当前关键帧闭环匹配上的关键帧及其共视关键帧组成的地图点 + * @param[in] th 搜索范围系数 + * @param[out] vpReplacePoint 替换的地图点 + * @return int 融合（替换和新增）的地图点数目 + */ int ORBmatcher::Fuse(KeyFrame *pKF, Sophus::Sim3f &Scw, const vector &vpPoints, float th, vector &vpReplacePoint) { // Get Calibration Parameters for later projection @@ -1346,26 +1552,31 @@ namespace ORB_SLAM3 const float &cy = pKF->cy; // Decompose Scw + // Step 1 将Sim3转化为SE3并分解 Sophus::SE3f Tcw = Sophus::SE3f(Scw.rotationMatrix(),Scw.translation()/Scw.scale()); Eigen::Vector3f Ow = Tcw.inverse().translation(); // Set of MapPoints already found in the KeyFrame + // 当前帧已有的匹配地图点 const set spAlreadyFound = pKF->GetMapPoints(); int nFused=0; - + // 与当前帧闭环匹配上的关键帧及其共视关键帧组成的地图点 const int nPoints = vpPoints.size(); // For each candidate MapPoint project and match + // 遍历所有的地图点 for(int iMP=0; iMPisBad() || spAlreadyFound.count(pMP)) continue; // Get 3D Coords. + // Step 2 地图点变换到当前相机坐标系下 Eigen::Vector3f p3Dw = pMP->GetWorldPos(); // Transform into Camera Coords. @@ -1376,13 +1587,16 @@ namespace ORB_SLAM3 continue; // Project into Image + // Step 3 得到地图点投影到当前帧的图像坐标 const Eigen::Vector2f uv = pKF->mpCamera->project(p3Dc); // Point must be inside the image + // 投影点必须在图像范围内 if(!pKF->IsInImage(uv(0),uv(1))) continue; // Depth must be inside the scale pyramid of the image + // Step 4 根据距离是否在图像合理金字塔尺度范围内和观测角度是否小于60度判断该地图点是否有效 const float maxDistance = pMP->GetMaxDistanceInvariance(); const float minDistance = pMP->GetMinDistanceInvariance(); Eigen::Vector3f PO = p3Dw-Ow; @@ -1401,15 +1615,17 @@ namespace ORB_SLAM3 const int nPredictedLevel = pMP->PredictScale(dist3D,pKF); // Search in a radius + // 计算搜索范围 const float radius = th*pKF->mvScaleFactors[nPredictedLevel]; + // Step 5 在当前帧内搜索匹配候选点 const vector vIndices = pKF->GetFeaturesInArea(uv(0),uv(1),radius); if(vIndices.empty()) continue; // Match to the most similar keypoint in the radius - + // Step 6 寻找最佳匹配点（没有用到次佳匹配的比例） const cv::Mat dMP = pMP->GetDescriptor(); int bestDist = INT_MAX; @@ -1434,57 +1650,84 @@ namespace ORB_SLAM3 } // If there is already a MapPoint replace otherwise add new measurement + // Step 7 替换或新增地图点 if(bestDist<=TH_LOW) { MapPoint* pMPinKF = pKF->GetMapPoint(bestIdx); if(pMPinKF) { + // 如果这个地图点已经存在，则记录要替换信息 + // 这里不能直接替换，原因是需要对地图点加锁后才能替换，否则可能会crash。所以先记录，在加锁后替换 if(!pMPinKF->isBad()) vpReplacePoint[iMP] = pMPinKF; } else { + // 如果这个地图点不存在，直接添加 pMP->AddObservation(pKF,bestIdx); pKF->AddMapPoint(pMP,bestIdx); } nFused++; } } - + // 融合（替换和新增）的地图点数目 return nFused; } + /** + * @brief 通过Sim3变换，搜索两个关键帧中更多的匹配点对 + * （之前使用SearchByBoW进行特征点匹配时会有漏匹配） + * @param[in] pKF1 当前帧 + * @param[in] pKF2 闭环候选帧 + * @param[in] vpMatches12 i表示匹配的pKF1 特征点索引，vpMatches12[i]表示匹配的地图点，null表示没有匹配 + * @param[in] s12 pKF2 到 pKF1 的Sim 变换中的尺度 + * @param[in] R12 pKF2 到 pKF1 的Sim 变换中的旋转矩阵 + * @param[in] t12 pKF2 到 pKF1 的Sim 变换中的平移向量 + * @param[in] th 搜索窗口的倍数 + * @return int 新增的匹配点对数目 + */ int ORBmatcher::SearchBySim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector &vpMatches12, const Sophus::Sim3f &S12, const float th) { + // Step 1： 准备工作 const float &fx = pKF1->fx; const float &fy = pKF1->fy; const float &cx = pKF1->cx; const float &cy = pKF1->cy; + // 从world到camera1的变换 + // 从world到camera2的变换 // Camera 1 & 2 from world Sophus::SE3f T1w = pKF1->GetPose(); Sophus::SE3f T2w = pKF2->GetPose(); //Transformation between cameras + // Sim3 的逆 Sophus::Sim3f S21 = S12.inverse(); + // 取出关键帧中的地图点 const vector vpMapPoints1 = pKF1->GetMapPointMatches(); const int N1 = vpMapPoints1.size(); const vector vpMapPoints2 = pKF2->GetMapPointMatches(); const int N2 = vpMapPoints2.size(); + // pKF1中特征点的匹配情况，有匹配为true，否则false vector vbAlreadyMatched1(N1,false); + // pKF2中特征点的匹配情况，有匹配为true，否则false vector vbAlreadyMatched2(N2,false); + // Step 2：记录已经匹配的特征点 for(int i=0; i(pMP->GetIndexInKeyFrame(pKF2)); if(idx2>=0 && idx2 vnMatch2(N2,-1); // Transform from KF1 to KF2 and search + // Step 3：通过Sim变换，寻找 pKF1 中特征点和 pKF2 中的新的匹配 + // 之前使用SearchByBoW进行特征点匹配时会有漏匹配 for(int i1=0; i1isBad()) continue; + // Step 3.1：通过Sim变换，将pKF1的地图点投影到pKF2中的图像坐标 Eigen::Vector3f p3Dw = pMP->GetWorldPos(); + // 把pKF1的地图点从world坐标系变换到camera1坐标系 Eigen::Vector3f p3Dc1 = T1w * p3Dw; + // 再通过Sim3将该地图点从camera1变换到camera2坐标系 Eigen::Vector3f p3Dc2 = S21 * p3Dc1; // Depth must be positive if(p3Dc2(2)<0.0) continue; + // 投影到camera2图像坐标 (u,v) const float invz = 1.0/p3Dc2(2); const float x = p3Dc2(0)*invz; const float y = p3Dc2(1)*invz; @@ -1531,11 +1781,14 @@ namespace ORB_SLAM3 continue; // Compute predicted octave + // Step 3.2：预测投影的点在图像金字塔哪一层 const int nPredictedLevel = pMP->PredictScale(dist3D,pKF2); // Search in a radius + // 计算特征点搜索半径 const float radius = th*pKF2->mvScaleFactors[nPredictedLevel]; + // Step 3.3：搜索该区域内的所有候选匹配特征点 const vector vIndices = pKF2->GetFeaturesInArea(u,v,radius); if(vIndices.empty()) @@ -1546,6 +1799,7 @@ namespace ORB_SLAM3 int bestDist = INT_MAX; int bestIdx = -1; + // Step 3.4：遍历所有候选特征点，寻找最佳匹配点（并未使用次佳最佳比例约束） for(vector::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++) { const size_t idx = *vit; @@ -1572,7 +1826,9 @@ namespace ORB_SLAM3 } } - // Transform from KF2 to KF2 and search + // Transform from KF2 to KF1 and search + // Step 4：通过Sim变换，寻找 pKF2 中特征点和 pKF1 中的新的匹配 + // 具体步骤同上 for(int i2=0; i2 rotHist[HISTO_LENGTH]; for(int i=0;iCurrentFrame.mb && !bMono; - const bool bBackward = -tlc(2)>CurrentFrame.mb && !bMono; + // 判断前进还是后退 + const bool bForward = tlc(2)>CurrentFrame.mb && !bMono; // 非单目情况，如果Z大于基线，则表示相机明显前进 + const bool bBackward = -tlc(2)>CurrentFrame.mb && !bMono; // 非单目情况，如果-Z小于基线，则表示相机明显后退 + // Step 3 对于前一帧的每一个地图点，通过相机投影模型，得到投影到当前帧的像素坐标 for(int i=0; iGetWorldPos(); Eigen::Vector3f x3Dc = Tcw * x3Dw; @@ -1710,26 +1993,33 @@ namespace ORB_SLAM3 if(invzc<0) continue; + // 投影到当前帧中 Eigen::Vector2f uv = CurrentFrame.mpCamera->project(x3Dc); if(uv(0)CurrentFrame.mnMaxX) continue; if(uv(1)CurrentFrame.mnMaxY) continue; - + // 认为投影前后地图点的尺度信息不变 int nLastOctave = (LastFrame.Nleft == -1 || i < LastFrame.Nleft) ? LastFrame.mvKeys[i].octave - : LastFrame.mvKeysRight[i - LastFrame.Nleft].octave; + : LastFrame.mvKeysRight[i - LastFrame.Nleft].octave; // Search in a window. Size depends on scale - float radius = th*CurrentFrame.mvScaleFactors[nLastOctave]; + // 单目：th = 7，双目：th = 15 + float radius = th*CurrentFrame.mvScaleFactors[nLastOctave]; // 尺度越大，搜索范围越大 + // 记录候选匹配点的id vector vIndices2; - if(bForward) + // Step 4 根据相机的前后前进方向来判断搜索尺度范围。 + // 以下可以这么理解，例如一个有一定面积的圆点，在某个尺度n下它是一个特征点 + // 当相机前进时，圆点的面积增大，在某个尺度m下它是一个特征点，由于面积增大，则需要在更高的尺度下才能检测出来 + // 当相机后退时，圆点的面积减小，在某个尺度m下它是一个特征点，由于面积减小，则需要在更低的尺度下才能检测出来 + if(bForward) // 前进,则上一帧兴趣点在所在的尺度nLastOctave<=nCurOctave vIndices2 = CurrentFrame.GetFeaturesInArea(uv(0),uv(1), radius, nLastOctave); - else if(bBackward) + else if(bBackward) // 后退,则上一帧兴趣点在所在的尺度0<=nCurOctave<=nLastOctave vIndices2 = CurrentFrame.GetFeaturesInArea(uv(0),uv(1), radius, 0, nLastOctave); - else + else // 在[nLastOctave-1, nLastOctave+1]中搜索 vIndices2 = CurrentFrame.GetFeaturesInArea(uv(0),uv(1), radius, nLastOctave-1, nLastOctave+1); if(vIndices2.empty()) @@ -1740,16 +2030,19 @@ namespace ORB_SLAM3 int bestDist = 256; int bestIdx2 = -1; + // Step 5 遍历候选匹配点，寻找距离最小的最佳匹配点 for(vector::const_iterator vit=vIndices2.begin(), vend=vIndices2.end(); vit!=vend; vit++) { const size_t i2 = *vit; + // 如果该特征点已经有对应的MapPoint了,则退出该次循环 if(CurrentFrame.mvpMapPoints[i2]) if(CurrentFrame.mvpMapPoints[i2]->Observations()>0) continue; if(CurrentFrame.Nleft == -1 && CurrentFrame.mvuRight[i2]>0) { + // 双目和rgbd的情况，需要保证右图的点也在搜索半径以内 const float ur = uv(0) - CurrentFrame.mbf*invzc; const float er = fabs(ur - CurrentFrame.mvuRight[i2]); if(er>radius) @@ -1767,11 +2060,13 @@ namespace ORB_SLAM3 } } + // 最佳匹配距离要小于设定阈值 if(bestDist<=TH_HIGH) { CurrentFrame.mvpMapPoints[bestIdx2]=pMP; nmatches++; + // Step 6 计算匹配点旋转角度差所在的直方图 if(mbCheckOrientation) { cv::KeyPoint kpLF = (LastFrame.Nleft == -1) ? LastFrame.mvKeysUn[i] @@ -1779,8 +2074,8 @@ namespace ORB_SLAM3 : LastFrame.mvKeysRight[i - LastFrame.Nleft]; cv::KeyPoint kpCF = (CurrentFrame.Nleft == -1) ? CurrentFrame.mvKeysUn[bestIdx2] - : (bestIdx2 < CurrentFrame.Nleft) ? CurrentFrame.mvKeys[bestIdx2] - : CurrentFrame.mvKeysRight[bestIdx2 - CurrentFrame.Nleft]; + : (bestIdx2 < CurrentFrame.Nleft) ? CurrentFrame.mvKeys[bestIdx2] + : CurrentFrame.mvKeysRight[bestIdx2 - CurrentFrame.Nleft]; float rot = kpLF.angle-kpCF.angle; if(rot<0.0) rot+=360.0f; @@ -1796,7 +2091,7 @@ namespace ORB_SLAM3 Eigen::Vector2f uv = CurrentFrame.mpCamera->project(x3Dr); int nLastOctave = (LastFrame.Nleft == -1 || i < LastFrame.Nleft) ? LastFrame.mvKeys[i].octave - : LastFrame.mvKeysRight[i - LastFrame.Nleft].octave; + : LastFrame.mvKeysRight[i - LastFrame.Nleft].octave; // Search in a window. Size depends on scale float radius = th*CurrentFrame.mvScaleFactors[nLastOctave]; @@ -1862,6 +2157,7 @@ namespace ORB_SLAM3 } //Apply rotation consistency + // Step 7 进行旋转一致检测，剔除不一致的匹配 if(mbCheckOrientation) { int ind1=-1; @@ -1872,6 +2168,7 @@ namespace ORB_SLAM3 for(int i=0; i &sAlreadyFound, const float th , const int ORBdist) { int nmatches = 0; @@ -1894,6 +2201,7 @@ namespace ORB_SLAM3 Eigen::Vector3f Ow = Tcw.inverse().translation(); // Rotation Histogram (to check rotation consistency) + // Step 1 建立旋转直方图，用于检测旋转一致性 vector rotHist[HISTO_LENGTH]; for(int i=0;i vpMPs = pKF->GetMapPointMatches(); + // Step 2 遍历关键帧中的每个地图点，通过相机投影模型，得到投影到当前帧的像素坐标 for(size_t i=0, iend=vpMPs.size(); iisBad() && !sAlreadyFound.count(pMP)) { //Project @@ -1931,11 +2241,14 @@ namespace ORB_SLAM3 if(dist3DmaxDistance) continue; + //预测尺度 int nPredictedLevel = pMP->PredictScale(dist3D,&CurrentFrame); // Search in a window + // 搜索半径和尺度相关 const float radius = th*CurrentFrame.mvScaleFactors[nPredictedLevel]; + // Step 3 搜索候选匹配点 const vector vIndices2 = CurrentFrame.GetFeaturesInArea(uv(0), uv(1), radius, nPredictedLevel-1, nPredictedLevel+1); if(vIndices2.empty()) @@ -1945,7 +2258,7 @@ namespace ORB_SLAM3 int bestDist = 256; int bestIdx2 = -1; - + // Step 4 遍历候选匹配点，寻找距离最小的最佳匹配点 for(vector::const_iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++) { const size_t i2 = *vit; @@ -1967,7 +2280,7 @@ namespace ORB_SLAM3 { CurrentFrame.mvpMapPoints[bestIdx2]=pMP; nmatches++; - + // Step 5 计算匹配点旋转角度差所在的直方图 if(mbCheckOrientation) { float rot = pKF->mvKeysUn[i].angle-CurrentFrame.mvKeysUn[bestIdx2].angle; @@ -1985,6 +2298,7 @@ namespace ORB_SLAM3 } } + // Step 6 进行旋转一致检测，剔除不一致的匹配 if(mbCheckOrientation) { int ind1=-1; @@ -2009,6 +2323,15 @@ namespace ORB_SLAM3 return nmatches; } + /** + * @brief 筛选出在旋转角度差落在在直方图区间内数量最多的前三个bin的索引 + * + * @param[in] histo 匹配特征点对旋转方向差直方图 + * @param[in] L 直方图尺寸 + * @param[in & out] ind1 bin值第一大对应的索引 + * @param[in & out] ind2 bin值第二大对应的索引 + * @param[in & out] ind3 bin值第三大对应的索引 + */ void ORBmatcher::ComputeThreeMaxima(vector* histo, const int L, int &ind1, int &ind2, int &ind3) { int max1=0; @@ -2041,6 +2364,7 @@ namespace ORB_SLAM3 } } + // 如果差距太大了,说明次优的非常不好,这里就索性放弃了,都置为-1 if(max2<0.1f*(float)max1) { ind2=-1; @@ -2053,8 +2377,9 @@ namespace ORB_SLAM3 } -// Bit set count operation from -// http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel + // Bit set count operation from + // Hamming distance：两个二进制串之间的汉明距离，指的是其不同位数的个数 + // http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel int ORBmatcher::DescriptorDistance(const cv::Mat &a, const cv::Mat &b) { const int *pa = a.ptr(); @@ -2062,9 +2387,13 @@ namespace ORB_SLAM3 int dist=0; + // 8*32=256bit + for(int i=0; i<8; i++, pa++, pb++) { - unsigned int v = *pa ^ *pb; + unsigned int v = *pa ^ *pb; // 相等为0,不等为1 + // 下面的操作就是计算其中bit为1的个数了,这个操作看上面的链接就好 + // 其实我觉得也还阔以直接使用8bit的查找表,然后做32次寻址操作就完成了;不过缺点是没有利用好CPU的字长 v = v - ((v >> 1) & 0x55555555); v = (v & 0x33333333) + ((v >> 2) & 0x33333333); dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;