ORB 特征提取
- ORBextractor.cc
operator ORB提取操作
步骤：
- 计算高斯图像金字塔 ：ComputePyramid
- 提取特征点并生成四叉树： ComputeKeyPointsOctTree
- 逐层计算特征点描述子： computeDescriptors
- 对图像进行高斯模糊
- 计算特征点描述子
- 对特征点进行尺度缩放
代码：
int ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints,
OutputArray _descriptors, std::vector<int> &vLappingArea)
{
//cout << "[ORBextractor]: Max Features: " << nfeatures << endl;
// 图像为空判断
if(_image.empty())
return -1;
Mat image = _image.getMat();
assert(image.type() == CV_8UC1 );
// Pre-compute the scale pyramid
// 构建图像金字塔
ComputePyramid(image);
// 计算每层图像的 fast角点
vector < vector<KeyPoint> > allKeypoints;
ComputeKeyPointsOctTree(allKeypoints);
//ComputeKeyPointsOld(allKeypoints);
Mat descriptors;
int nkeypoints = 0;
for (int level = 0; level < nlevels; ++level)
nkeypoints += (int)allKeypoints[level].size();
if( nkeypoints == 0 )
_descriptors.release();
else
{
// 为每个关键帧描述子分配32位内存
_descriptors.create(nkeypoints, 32, CV_8U);
descriptors = _descriptors.getMat();
}
//_keypoints.clear();
//_keypoints.reserve(nkeypoints);
_keypoints = vector<cv::KeyPoint>(nkeypoints);
int offset = 0;
//Modified for speeding up stereo fisheye matching
// 为加快立体鱼眼匹配而修改 遍历 图像金字塔
int monoIndex = 0, stereoIndex = nkeypoints-1;
for (int level = 0; level < nlevels; ++level)
{
// 取出该层所有特征点，若特征点个数为0，则continue
vector<KeyPoint>& keypoints = allKeypoints[level];
int nkeypointsLevel = (int)keypoints.size();
if(nkeypointsLevel==0)
continue;
// preprocess the resized image
// 对图像进行高斯模糊，用BORDER_REFLECT_101方法处理边缘
Mat workingMat = mvImagePyramid[level].clone();
GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);
// Compute the descriptors
//Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel);
// 计算描述子,offset是偏移量，此处取出的是该层的描述子
Mat desc = cv::Mat(nkeypointsLevel, 32, CV_8U);
computeDescriptors(workingMat, keypoints, desc, pattern);
offset += nkeypointsLevel;
// 再进一步遍历当前图层上的每一个特征点，根据对应的缩放系数缩放它的像素坐标，再根据它与LappingArea的关系，
// 确定当前特征点描述子在总描述子容器中的位置，如果是keypoint->pt.x >= vLappingArea[0] &&
// keypoint->pt.x <= vLappingArea[1]，那么它的描述子放在后面，否则放在前面。
float scale = mvScaleFactor[level]; //getScale(level, firstLevel, scaleFactor);
int i = 0;
for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),
keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint){
// Scale keypoint coordinates
if (level != 0){
keypoint->pt *= scale; // 按缩放因子进行坐标缩放
}
//
if(keypoint->pt.x >= vLappingArea[0] && keypoint->pt.x <= vLappingArea[1]){
_keypoints.at(stereoIndex) = (*keypoint);
desc.row(i).copyTo(descriptors.row(stereoIndex));
stereoIndex--;
}
else{
_keypoints.at(monoIndex) = (*keypoint);
desc.row(i).copyTo(descriptors.row(monoIndex));
monoIndex++;
}
i++;
}
}
//cout << "[ORBextractor]: extracted " << _keypoints.size() << " KeyPoints" << endl;
return monoIndex;
}
ComputePyramid 图像金字塔操作
/**
* @brief ComputePyramid 计算图像金字塔，按一定的尺度比例获取nlevels张分辨率不同的图像
* @param image 图像
*/
void ORBextractor::ComputePyramid(cv::Mat image)
{
for (int level = 0; level < nlevels; ++level)
{
float scale = mvInvScaleFactor[level];
// 基于该尺度变换的后图像的尺寸
Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale));
// 相当于给原图像加边，在计算关键点时便不需要检测所加边缘，又能保证原图像边缘的关键点也能检测
Size wholeSize(sz.width + EDGE_THRESHOLD*2, sz.height + EDGE_THRESHOLD*2);
//temp通过Mat类的一种构造函数获得了尺寸及图像的类型
Mat temp(wholeSize, image.type()), masktemp;
//mvImagePyramid[level]通过 赋值与 temp 相关联
mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height));
// Compute the resized image
if( level != 0 )
{ //把上一层图片缩放成 sz
resize(mvImagePyramid[level-1], mvImagePyramid[level], sz, 0, 0, INTER_LINEAR);
//在图像周围以对称的方式加一个宽度为EDGE_THRESHOLD的边，便于后面的计算
//在后面计算特征点时的时候省去边缘的判断； 可以省去加边操作，然后在后面计算的时候做原图像的边缘判断即可
copyMakeBorder(mvImagePyramid[level], temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
BORDER_REFLECT_101+BORDER_ISOLATED);
}
else
{ //第一张为原图分辨率，无需缩放，进行了加边操作
copyMakeBorder(image, temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
BORDER_REFLECT_101);
}
}
}
ComputeKeyPointsOctTree 计算四叉树的特征点
简介：
- @brief ComputeKeyPointsOctTree 计算四叉树的特征点，函数名字后面的OctTree只是说明了在过滤和分配特征点时所使用的方式
- @param allKeypoints 所有的特征点，这里第一层vector存储的是某图层里面的所有特征点，第二层存储的是整个图像金字塔中的所有图层里面的所有特征点
流程：
- 特征点提取按照图像层级提取，每层中图像按
w=35提取 - 遍历每层金字塔，提取
Fast特征到vToDistributeKeys-
计算这层图像的坐标边界值，减去边界
EDGE_THRESHOLD，同时+3
这里的3是因为在计算FAST特征点的时候，需要建立一个半径为3的圆 -
预分配特征的采集空间，一般地都是过量采集，所以这里大小是
nfeatures*10 -
计算进行特征点提取的图像区域尺寸： 宽+高，进而计算出特征区域(网格)的行数和列数
ncols=width/w，nrows=height/w，取整
-
遍历特征区域(网格)，先遍历行
- 计算当前网格初始行 和 最大行坐标
- 如果初始的行坐标就已经超过了有效的图像边界了，就跳过这一行
- 如果图像的大小导致不能够正好划分出来整齐的图像网格，那么就要委屈最后一行了
- 遍历当前网格(特征区域)的列
- 得到特征区域的最小列数和最需大列数，需与 bord相关联（与行一样）
- 在该区域中检测fast角点， vKeysCell:存储角点位置的容器,iniThFAST:检测阈值，true:使能非极大值抑制
- 若未提取出fast特征时，按最小阈值 minThFAST 提取特征值，与上使用相同的函数
- 若特征值不为空时，将fast特征放入
vToDistributeKeys容器- 这些角点的坐标都是基于图像cell的，现在我们要先将其恢复到当前的【坐标边界】下的坐标
- 这样做是因为在下面使用八叉树法整理特征点的时候将会使用得到这个坐标
-
声明一个对当前图层的特征点的容器的引用,并且调整其大小为欲提取出来的特征点个数
-
基于兴趣点对特征点进行剔除：DistributeOctTree 函数
- vToDistributeKeys：当前图层提取出来的特征点，也即是等待剔除的特征点
- 根据mnFeaturesPerLevel,即该层的兴趣点数,对特征点进行剔除,返回值是一个保存有特征点的vector容器，含有剔除后的保留下来的特征点, 得到的特征点的坐标
-
计算当前层的尺寸，PATCH_SIZE是对于底层的初始图像来说的，现在要根据当前图层的尺度缩放倍数进行缩放得到缩放后的PATCH大小 和特征点的方向计算有关
-
获取剔除过程后保留下来的特征点数目, 然后开始遍历这些特征点，恢复其在当前图层图像坐标系下的坐标
- 对每一个保留下来的特征点，恢复到相对于当前图层“边缘扩充图像下”的坐标系的坐标
- 加减边界实现
- 记录计算方向的patch，缩放后对应的大小， 又被称作为特征点半径
- 对每一个保留下来的特征点，恢复到相对于当前图层“边缘扩充图像下”的坐标系的坐标
-
- 然后计算这些特征点的方向信息，注意这里还是分层计算的
- 遍历每个特征点，计算其方向：computeOrientation 函数
代码：
void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoints)
{
// 重新调整图像层数 关键帧
allKeypoints.resize(nlevels);
// 图像cell的尺寸，是个正方形，可以理解为边长in像素坐标
const float W = 35;
// 遍历每层金字塔，提取`Fast`特征到`vToDistributeKeys`
for (int level = 0; level < nlevels; ++level)
{
// 计算这层图像的坐标边界值，这里的 3 是因为在计算FAST特征点的时候，需要建立一个半径为3的圆
const int minBorderX = EDGE_THRESHOLD-3;
const int minBorderY = minBorderX;
const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3;
const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3;
// 预分配的空间，一般地都是过量采集，所以这里大小是nfeatures*10
vector<cv::KeyPoint> vToDistributeKeys;
vToDistributeKeys.reserve(nfeatures*10);
// 计算进行特征点提取的图像区域尺寸： 宽+高
const float width = (maxBorderX-minBorderX);
const float height = (maxBorderY-minBorderY);
// 每个区域35像素，计算 特征区域(网格)的行数和列数
const int nCols = width/W;
const int nRows = height/W;
const int wCell = ceil(width/nCols);
const int hCell = ceil(height/nRows);
// 遍历特征区域(网格)，先遍历行
for(int i=0; i<nRows; i++)
{
// 计算当前网格初始行 和 最大行坐标
const float iniY =minBorderY+i*hCell;
// 注意：这里的+6=+3+3，即考虑到了多出来以便进行FAST特征点提取用的3像素边界
float maxY = iniY+hCell+6;
// 如果初始的行坐标就已经超过了有效的图像边界了，就跳过这一行
if(iniY>=maxBorderY-3)
continue;
//如果图像的大小导致不能够正好划分出来整齐的图像网格，那么就要委屈最后一行了
if(maxY>maxBorderY)
maxY = maxBorderY;
// 遍历 当前网格(特征区域)的列
for(int j=0; j<nCols; j++)
{
// 得到特征区域的最小列数和最需大列数，需与 bord相关联
const float iniX =minBorderX+j*wCell;
float maxX = iniX+wCell+6;
if(iniX>=maxBorderX-6)
continue;
if(maxX>maxBorderX)
maxX = maxBorderX;
vector<cv::KeyPoint> vKeysCell;
// 在该区域中检测fast角点， vKeysCell:存储角点位置的容器,iniThFAST:检测阈值，true:使能非极大值抑制
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
vKeysCell,iniThFAST,true);
/*if(bRight && j <= 13){
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
vKeysCell,10,true);
}
else if(!bRight && j >= 16){
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
vKeysCell,10,true);
}
else{
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
vKeysCell,iniThFAST,true);
}*/
// 若未提取出fast特征时，按最小阈值 minThFAST 提取特征值
if(vKeysCell.empty())
{
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
vKeysCell,minThFAST,true); //true 使能非极大值抑制
/*if(bRight && j <= 13){
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
vKeysCell,5,true);
}
else if(!bRight && j >= 16){
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
vKeysCell,5,true);
}
else{
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
vKeysCell,minThFAST,true);
}*/
}
// 若特征值不为空时，将fast特征放入`vToDistributeKeys`容器
if(!vKeysCell.empty())
{
/// 这些角点的坐标都是基于图像cell的，现在我们要先将其恢复到当前的【坐标边界】下的坐标
/// 这样做是因为在下面使用八叉树法整理特征点的时候将会使用得到这个坐标
for(vector<cv::KeyPoint>::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++)
{ // 转换为 bord坐标了
(*vit).pt.x+=j*wCell;
(*vit).pt.y+=i*hCell;
vToDistributeKeys.push_back(*vit);
}
}
}
}
//声明一个对当前图层的特征点的容器的引用,并且调整其大小为欲提取出来的特征点个数
vector<KeyPoint> & keypoints = allKeypoints[level];
keypoints.reserve(nfeatures);
// 根据mnFeaturesPerLevel,即该层的兴趣点数,对特征点进行剔除,返回值是一个保存有特征点的vector容器，含有剔除后的保留下来的特征点,
// 得到的特征点的坐标，vToDistributeKeys：当前图层提取出来的特征点，也即是等待剔除的特征点
keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX,
minBorderY, maxBorderY,mnFeaturesPerLevel[level], level);
// 计算当前层的尺寸，PATCH_SIZE是对于底层的初始图像来说的，现在要根据当前图层的尺度缩放倍数进行缩放得到缩放后的PATCH大小 和特征点的方向计算有关
const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level];
// Add border to coordinates and scale information
// 获取剔除过程后保留下来的特征点数目, 然后开始遍历这些特征点，恢复其在当前图层图像坐标系下的坐标
const int nkps = keypoints.size();
for(int i=0; i<nkps ; i++)
{
//对每一个保留下来的特征点，恢复到相对于当前图层“边缘扩充图像下”的坐标系的坐标
keypoints[i].pt.x+=minBorderX;
keypoints[i].pt.y+=minBorderY;
keypoints[i].octave=level;
// 记录计算方向的patch，缩放后对应的大小， 又被称作为特征点半径
keypoints[i].size = scaledPatchSize;
}
}
// compute orientations
//然后计算这些特征点的方向信息，注意这里还是分层计算的
for (int level = 0; level < nlevels; ++level)
computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);
}
DistributeOctTree 均匀化特征点
简介：
-
使用四叉树法对一个图像金字塔图层中的特征点进行平均和分发
-
参数：
vToDistributeKeys： 等待进行分配到四叉树中的特征点minX,maxX,minY,maxY：当前图层的图像的边界，坐标都是在“半径扩充图像”坐标系下的坐标(+3)N： 希望提取出的特征点个数level：指定的金字塔图层，并未使用
-
返回：
vector<cv::KeyPoint>：已经均匀分散好的特征点vector容器
步骤：
- 根据宽高比确定初始节点数目
- 生成初始提取器节点，遍历初始节点个数
- 构造一个提取器节点
ni - 确定提取器节点的图像边界，注意这里和提取FAST角点区域相同
- 刚才生成的提取节点添加到列表中
- 构造一个提取器节点
- 将特征点分配到子提取器节点中
- 按特征点的横轴位置，分配给属于那个图像区域的提取器节点（最初的提取器节点）
- 遍历此提取器节点列表，标记那些不可再分裂的节点，删除那些没有分配到特征点的节点
- 如果初始的提取器节点所分配到的特征点个数为1
- 那么就标志位置位，表示此节点不可再分
- 如果一个提取器节点没有被分配到特征点，那么就从列表中直接删除它
- 如果上面的这些情况和当前的特征点提取器节点无关，那么就只是更新迭代器
- 如果初始的提取器节点所分配到的特征点个数为1
- 声明一个vector用于存储节点的vSize和句柄对，预设大小
lNodes.size()*4
这个变量记录了在一次分裂循环中，那些可以再继续进行分裂的节点中包含的特征点数目和其句柄 - 根据兴趣点分布,利用4叉树方法对图像进行划分区域
- 更新迭代次数计数器，只是记录，并未起到作用
- 保存当前节点个数，prev在这里理解为“保留”比较好
- 重新定位迭代器指向列表头部
- 将目前的子区域进行划分，开始遍历列表中所有的提取器节点，并进行分解或者保留
- 如果节点只包含一个点不细分继续，跳过当前节点，继续下一个
- 如果当前的提取器节点具有超过一个的特征点，那么就要进行继续细分
- 再细分成四个子区域，对每个子区域操作一样，举一个粒子
- 如果这里分出来的子区域中有特征点，那么就将这个子区域的节点添加到提取器节点的列表中
- 再判断其中子提取器节点中的特征点数目是否大于1
- 如果有超过一个的特征点，那么待展开的节点计数
nToExpand++ - 保存这个特征点数目和节点指针的信息
- 如果有超过一个的特征点，那么待展开的节点计数
- 当这个母节点expand之后就从列表中删除它了，能够进行分裂操作说明至少有一个子节点的区域中特征点的数量是>1的
- 再细分成四个子区域，对每个子区域操作一样，举一个粒子
- 停止这个过程的条件有两个，满足其中一个即可：
- 1、当前的节点数已经超过了要求的特征点数，
- 2、当前所有的节点中都只包含一个特征点
- 可以展开的子节点个数
nToExpand x3，是因为一分四之后，会删除原来的主节点，所以乘以3
如果再分裂一次那么数目就要超了，这里想办法尽可能使其刚刚达到或者超过要求的特征点个数时就退出- 一直循环，直到结束标志位被置位
- 获取当前的list中的节点个数
- Prev这里是应该是保留的意思吧，保留那些还可以分裂的节点的信息, 这里是深拷贝
- 对需要划分的节点进行排序，对pair对的第一个元素进行排序，默认是从小到大排序
- 遍历这个存储了pair对的vector，注意是从后往前遍历
- 对每个需要进行分裂的节点进行分裂，一份四，与上第一层分裂类似
- 判断是是否超过了需要的特征点数？是的话就退出，不是的话就继续这个分裂过程，直到刚刚达到或者超过要求的特征点个数
- 一直循环，直到结束标志位被置位
- 保留每个区域响应值最大的一个兴趣点
- 遍历这个节点列表，大小已经满足
>N了- 得到指向第一个特征点的指针，后面作为最大响应值对应的关键点
- 开始遍历这个节点区域中的特征点容器中的特征点，注意是从1开始哟，0已经用过了
- 若超出最大响应值，则更新最大响应值
- 遍历这个节点列表，大小已经满足
- 返回最终结果容器，其中保存有分裂出来的区域中，我们最感兴趣、响应值最大的特征点
vector<cv::KeyPoint> ORBextractor::DistributeOctTree(const vector<cv::KeyPoint>& vToDistributeKeys, const int &minX,
const int &maxX, const int &minY, const int &maxY, const int &N, const int &level)
{
/// Step 1 根据宽高比确定初始节点数目
// Compute how many initial nodes
// 根据宽高比确定初始节点数目，一般是1或者2
// bug: 如果宽高比小于0.5，nIni=0 ??，后面hx会报错,除以0不得凉凉
const int nIni = round(static_cast<float>(maxX-minX)/(maxY-minY));
const float hX = static_cast<float>(maxX-minX)/nIni;
// 存储有提取器节点的列表
list<ExtractorNode> lNodes;
// 存储初始提取器节点指针的vector,然后重新设置其大小
vector<ExtractorNode*> vpIniNodes;
vpIniNodes.resize(nIni);
/// Step 2 生成初始提取器节点，遍历初始节点个数
for(int i=0; i<nIni; i++)
{
// 构造一个提取器节点 ni
ExtractorNode ni;
// 确定提取器节点的图像边界,注意这里和提取FAST角点区域相同，都是“半径扩充图像”，特征点坐标从0开始
ni.UL = cv::Point2i(hX*static_cast<float>(i),0);
ni.UR = cv::Point2i(hX*static_cast<float>(i+1),0);
ni.BL = cv::Point2i(ni.UL.x,maxY-minY);
ni.BR = cv::Point2i(ni.UR.x,maxY-minY);
ni.vKeys.reserve(vToDistributeKeys.size());
// 将刚才生成的提取节点添加到列表中
lNodes.push_back(ni); // 虽然这里的ni是局部变量，但是由于这里的push_back()是拷贝参数的内容到一个
// 新的对象中然后再添加到列表中，所以当本函数退出之后这里的内存不会成为“野指针”
vpIniNodes[i] = &lNodes.back();
}
/// Step 3 将特征点分配到子提取器节点中
//Associate points to childs
for(size_t i=0;i<vToDistributeKeys.size();i++)
{
// 按特征点的横轴位置，分配给属于那个图像区域的提取器节点（最初的提取器节点）
const cv::KeyPoint &kp = vToDistributeKeys[i];
vpIniNodes[kp.pt.x/hX]->vKeys.push_back(kp);
}
/// Step 4 遍历此提取器节点列表，标记那些不可再分裂的节点，删除那些没有分配到特征点的节点
list<ExtractorNode>::iterator lit = lNodes.begin();
while(lit!=lNodes.end())
{
if(lit->vKeys.size()==1) // 如果初始的提取器节点所分配到的特征点个数为1
{
lit->bNoMore=true; // 那么就标志位置位，表示此节点不可再分
lit++;
}
else if(lit->vKeys.empty()) // 如果一个提取器节点没有被分配到特征点，那么就从列表中直接删除它
lit = lNodes.erase(lit);
else // 如果上面的这些情况和当前的特征点提取器节点无关，那么就只是更新迭代器
lit++;
}
bool bFinish = false;
int iteration = 0;
// 声明一个vector用于存储节点的vSize和句柄对
// 这个变量记录了在一次分裂循环中，那些可以再继续进行分裂的节点中包含的特征点数目和其句柄
vector<pair<int,ExtractorNode*> > vSizeAndPointerToNode;
// 调整大小，这里的意思是一个初始化节点将“分裂”成为四个，当然实际上不会有那么多，这里多分配了一些只是预防万一
vSizeAndPointerToNode.reserve(lNodes.size()*4);
/// Step 5 根据兴趣点分布,利用4叉树方法对图像进行划分区域
while(!bFinish)
{
// 更新迭代次数计数器，只是记录，并未起到作用
iteration++;
// 保存当前节点个数，prev在这里理解为“保留”比较好
int prevSize = lNodes.size();
// 重新定位迭代器指向列表头部
lit = lNodes.begin();
// 需要展开的节点计数，这个一直保持累计，不清零
int nToExpand = 0;
vSizeAndPointerToNode.clear();
// 将目前的子区域进行划分，开始遍历列表中所有的提取器节点，并进行分解或者保留
while(lit!=lNodes.end())
{
// 如果提取器节点只有一个特征点，
if(lit->bNoMore)
{
// If node only contains one point do not subdivide and continue
// 如果节点只包含一个点不细分继续，跳过当前节点，继续下一个
lit++;
continue;
}
else
{
// If more than one point, subdivide
// 如果当前的提取器节点具有超过一个的特征点，那么就要进行继续细分
ExtractorNode n1,n2,n3,n4;
lit->DivideNode(n1,n2,n3,n4);// 再细分成四个子区域
// Add childs if they contain points
// 如果这里分出来的子区域中有特征点，那么就将这个子区域的节点添加到提取器节点的列表中
if(n1.vKeys.size()>0)
{
//注意这里也是添加到列表前面的
lNodes.push_front(n1);
if(n1.vKeys.size()>1) // 再判断其中子提取器节点中的特征点数目是否大于1
{
nToExpand++; // 如果有超过一个的特征点，那么“待展开的节点计数++”
//保存这个特征点数目和节点指针的信息
vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));
lNodes.front().lit = lNodes.begin();
}
}
// 后面操作类似
if(n2.vKeys.size()>0)
{
lNodes.push_front(n2);
if(n2.vKeys.size()>1)
{
nToExpand++;
vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front()));
lNodes.front().lit = lNodes.begin();
}
}
if(n3.vKeys.size()>0)
{
lNodes.push_front(n3);
if(n3.vKeys.size()>1)
{
nToExpand++;
vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front()));
lNodes.front().lit = lNodes.begin();
}
}
if(n4.vKeys.size()>0)
{
lNodes.push_front(n4);
if(n4.vKeys.size()>1)
{
nToExpand++;
vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front()));
lNodes.front().lit = lNodes.begin();
}
}
// 当这个母节点expand之后就从列表中删除它了，能够进行分裂操作说明至少有一个子节点的区域中特征点的数量是>1的
lit=lNodes.erase(lit);
continue;
}
}
// Finish if there are more nodes than required features
// or all nodes contain just one point
// 停止这个过程的条件有两个，满足其中一个即可：1、当前的节点数已经超过了要求的特征点数，2、当前所有的节点中都只包含一个特征点
if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
{
bFinish = true;
}
// Step 6 当再划分之后所有的Node数大于要求数目时,就慢慢划分直到使其刚刚达到或者超过要求的特征点个数
// 可以展开的子节点个数nToExpand x3，是因为一分四之后，会删除原来的主节点，所以乘以3
else if(((int)lNodes.size()+nToExpand*3)>N)
{
// 如果再分裂一次那么数目就要超了，这里想办法尽可能使其刚刚达到或者超过要求的特征点个数时就退出
// 一直循环，直到结束标志位被置位
while(!bFinish)
{
// 获取当前的list中的节点个数
prevSize = lNodes.size();
// Prev这里是应该是保留的意思吧，保留那些还可以分裂的节点的信息, 这里是深拷贝
vector<pair<int,ExtractorNode*> > vPrevSizeAndPointerToNode = vSizeAndPointerToNode;
vSizeAndPointerToNode.clear();
// 对需要划分的节点进行排序，对pair对的第一个元素进行排序，默认是从小到大排序
sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end());
// 遍历这个存储了pair对的vector，注意是从后往前遍历
for(int j=vPrevSizeAndPointerToNode.size()-1;j>=0;j--)
{
// 对每个需要进行分裂的节点进行分裂，一份四，与上第一层分裂类似
ExtractorNode n1,n2,n3,n4;
vPrevSizeAndPointerToNode[j].second->DivideNode(n1,n2,n3,n4);
// Add childs if they contain points
if(n1.vKeys.size()>0)
{
lNodes.push_front(n1);
if(n1.vKeys.size()>1)
{
vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));
lNodes.front().lit = lNodes.begin();
}
}
if(n2.vKeys.size()>0)
{
lNodes.push_front(n2);
if(n2.vKeys.size()>1)
{
vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front()));
lNodes.front().lit = lNodes.begin();
}
}
if(n3.vKeys.size()>0)
{
lNodes.push_front(n3);
if(n3.vKeys.size()>1)
{
vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front()));
lNodes.front().lit = lNodes.begin();
}
}
if(n4.vKeys.size()>0)
{
lNodes.push_front(n4);
if(n4.vKeys.size()>1)
{
vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front()));
lNodes.front().lit = lNodes.begin();
}
}
// 删除母节点，在这里其实应该是一级子节点
lNodes.erase(vPrevSizeAndPointerToNode[j].second->lit);
//判断是是否超过了需要的特征点数？是的话就退出，不是的话就继续这个分裂过程，直到刚刚达到或者超过要求的特征点个数
if((int)lNodes.size()>=N)
break;
}
// 判断是否达到了停止条件
if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
bFinish = true;
}
}
}
// Retain the best point in each node
// Step 7 保留每个区域响应值最大的一个兴趣点
vector<cv::KeyPoint> vResultKeys;
vResultKeys.reserve(nfeatures); // 调整大小为要提取的特征点数目
// 遍历这个节点列表
for(list<ExtractorNode>::iterator lit=lNodes.begin(); lit!=lNodes.end(); lit++)
{
vector<cv::KeyPoint> &vNodeKeys = lit->vKeys; // 得到这个节点区域中的特征点容器句柄
cv::KeyPoint* pKP = &vNodeKeys[0]; // 得到指向第一个特征点的指针，后面作为最大响应值对应的关键点
float maxResponse = pKP->response; //用第1个关键点响应值初始化最大响应值
// 开始遍历这个节点区域中的特征点容器中的特征点，注意是从1开始哟，0已经用过了
for(size_t k=1;k<vNodeKeys.size();k++)
{
if(vNodeKeys[k].response>maxResponse) //更新最大响应值
{
pKP = &vNodeKeys[k];
maxResponse = vNodeKeys[k].response;
}
}
vResultKeys.push_back(*pKP);
}
// 返回最终结果容器，其中保存有分裂出来的区域中，我们最感兴趣、响应值最大的特征点
return vResultKeys;
}
DivideNode 将提取器节点分成4个子节点
- brief： 将提取器节点分成4个子节点，同时也完成图像区域的划分、特征点归属的划分，以及相关标志位的置位
- 参数：左上、右上、左下、右下
步骤：
- 计算得到当前提取器节点所在图像区域的一半长宽，当然结果需要取整
- 下面的操作大同小异，将一个图像区域再细分成为四个小图像区块
- 遍历当前提取器节点的vkeys中存储的特征点，划分到四个区块中
- 获取这个特征点对象
- 判断这个特征点在当前特征点提取器节点图像的哪个区域，更严格地说是属于那个子图像区块
然后就将这个特征点追加到那个特征点提取器节点的vkeys中
- 判断每个子特征点提取器节点所在的图像中特征点的数目是否等于1
判断是否数目等于1的目的是确定这个节点还能不能再向下进行分裂
代码：
/**将提取器节点分成4个子节点
*/
void ExtractorNode::DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4)
{
// 得到当前提取器节点所在图像区域的一半长宽，当然结果需要取整
const int halfX = ceil(static_cast<float>(UR.x-UL.x)/2);
const int halfY = ceil(static_cast<float>(BR.y-UL.y)/2);
// 下面的操作大同小异，将一个图像区域再细分成为四个小图像区块
//Define boundaries of childs
n1.UL = UL;
n1.UR = cv::Point2i(UL.x+halfX,UL.y);
n1.BL = cv::Point2i(UL.x,UL.y+halfY);
n1.BR = cv::Point2i(UL.x+halfX,UL.y+halfY);
n1.vKeys.reserve(vKeys.size());
n2.UL = n1.UR;
n2.UR = UR;
n2.BL = n1.BR;
n2.BR = cv::Point2i(UR.x,UL.y+halfY);
n2.vKeys.reserve(vKeys.size());
n3.UL = n1.BL;
n3.UR = n1.BR;
n3.BL = BL;
n3.BR = cv::Point2i(n1.BR.x,BL.y);
n3.vKeys.reserve(vKeys.size());
n4.UL = n3.UR;
n4.UR = n2.BR;
n4.BL = n3.BR;
n4.BR = BR;
n4.vKeys.reserve(vKeys.size());
//Associate points to childs
// 遍历当前提取器节点的vkeys中存储的特征点，
for(size_t i=0;i<vKeys.size();i++)
{
//获取这个特征点对象
const cv::KeyPoint &kp = vKeys[i];
// 判断这个特征点在当前特征点提取器节点图像的哪个区域，更严格地说是属于那个子图像区块
// 然后就将这个特征点追加到那个特征点提取器节点的vkeys中
if(kp.pt.x<n1.UR.x)
{
if(kp.pt.y<n1.BR.y)
n1.vKeys.push_back(kp);
else
n3.vKeys.push_back(kp);
}
else if(kp.pt.y<n1.BR.y)
n2.vKeys.push_back(kp);
else
n4.vKeys.push_back(kp);
}
// 判断每个子特征点提取器节点所在的图像中特征点的数目（就是分配给子节点的特征点数目），然后做标记
// 这里判断是否数目等于1的目的是确定这个节点还能不能再向下进行分裂
if(n1.vKeys.size()==1)
n1.bNoMore = true;
if(n2.vKeys.size()==1)
n2.bNoMore = true;
if(n3.vKeys.size()==1)
n3.bNoMore = true;
if(n4.vKeys.size()==1)
n4.bNoMore = true;
}
computeOrientation 计算特征的方向
- 遍历每个特征，计算其特征的灰度旋转角度
static void computeOrientation(const Mat& image, vector<KeyPoint>& keypoints, const vector<int>& umax)
{
for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),
keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
{
keypoint->angle = IC_Angle(image, keypoint->pt, umax);
}
}
IC_Angle 灰度质心法（IC）计算特征的旋转
原理：
- 基本原理就是灰度质心法：每个特征点以几何中心P为圆心，画圆，计算出灰度质心Q，则P -> Q就是该特征点的方向。后面比较特征点时，先把方向统一，然后比较特征点描述子，这样特征点就不会因为旋转而不能匹配了。
步骤：
- 在一个小的图像块B中，定义图像块的矩为： m p q = ∑ x , y ∈ B x p y p I ( x , y ) , p , q = 0 , 1 {m_{pq}=\sum_{x,y \in B} x^p y^p I(x,y), \ \ p,q={0,1}} mpq=∑x,y∈BxpypI(x,y), p,q=0,1
- 通过矩可以找到图像块的质心： C = ( m 10 m 00 , m 01 m 00 ) {C=(\frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}})} C=(m00m10,m00m01)
- 连接图像块的几何中心与质心，得到一个方向向量 O C ⃗ {\vec {OC}} OC，其方向可定义为： θ = a t a n ( m 01 , m 10 ) {\theta = atan(m_{01},m_{10})} θ=atan(m01,m10)。
const int PATCH_SIZE = 31; //用来做灰度质心的圆的直径
const int HALF_PATCH_SIZE = 15; //圆的半径15（15+15+1=31，1为中心点的那个像素）
const int EDGE_THRESHOLD = 19; //边界阈值
/**
* @brief IC_Angle 灰度质心法（IC）计算特征的旋转
* @param image 输入图像
* @param pt 当前特征点坐标
* @param u_max 指定一行后的u轴边界坐标
* @return
*/
static float IC_Angle(const Mat& image, Point2f pt, const vector<int> & u_max)
{
//图像的矩，前者是按照图像块的y坐标加权，后者是按照图像块的x坐标加权
int m_01 = 0, m_10 = 0;
//获得这个特征点所在的图像块的中心点坐标灰度值的指针center
const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x));
// Treat the center line differently, v=0 //这条v=0中心线的计算需要特殊对待
for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
m_10 += u * center[u];
// Go line by line in the circuI853lar patch
// 该方法返回一个除以 Mat::elemSize1() 的矩阵步长。 快速访问任意矩阵元素可能很有用。
int step = (int)image.step1();
//注意这里是以v=0中心线为对称轴，然后对称地每成对的两行之间进行遍历，这样处理加快了计算速度
for (int v = 1; v <= HALF_PATCH_SIZE; ++v)
{
// Proceed over the two lines
//本来m_01应该是一列一列地计算的，但是由于对称以及坐标x,y正负的原因，可以一次计算两行
int v_sum = 0;
// 获取某行像素横坐标的最大范围，注意这里的图像块是圆形的！
int d = u_max[v];
// 在坐标范围内挨个像素遍历，实际是一次遍历2个
// 假设每次处理的两个点坐标，中心线下方为(x,y),中心线上方为(x,-y)
// 对于某次待处理的两个点：m_10 = Σ x*I(x,y) = x*I(x,y) + x*I(x,-y) = x*(I(x,y) + I(x,-y))
// 对于某次待处理的两个点：m_01 = Σ y*I(x,y) = y*I(x,y) - y*I(x,-y) = y*(I(x,y) - I(x,-y))
for (int u = -d; u <= d; ++u)
{
//得到需要进行加运算和减运算的像素灰度值
//val_plus：在中心线下方x=u时的的像素灰度值
//val_minus：在中心线上方x=u时的像素灰度值
int val_plus = center[u + v*step], val_minus = center[u - v*step];
//在v（y轴）上，2行所有像素灰度值之差
v_sum += (val_plus - val_minus);
//u轴（也就是x轴）方向上用u坐标加权和（u坐标也有正负符号），相当于同时计算两行
m_10 += u * (val_plus + val_minus);
}
//将这一行上的和按照y坐标加权
m_01 += v * v_sum;
}
//为了加快速度还使用了fastAtan2()函数，输出为[0,360)角度，精度为0.3°
return fastAtan2((float)m_01, (float)m_10);
}
computeDescriptors 计算orb描述子
- 遍历每个特征，计算其 fast特征的描述子：computeOrbDescriptor
static void computeDescriptors(const Mat& image, vector<KeyPoint>& keypoints, Mat& descriptors,
const vector<Point>& pattern)
{
// 描述子用 1个字节表示
descriptors = Mat::zeros((int)keypoints.size(), 32, CV_8UC1);
for (size_t i = 0; i < keypoints.size(); i++)
computeOrbDescriptor(keypoints[i], image, &pattern[0], descriptors.ptr((int)i));
}
static void computeOrbDescriptor(const KeyPoint &kpt,
const Mat &img, const Point *pattern,
uchar *desc) {
float angle = (float) kpt.angle * factorPI;
float a = (float) cos(angle), b = (float) sin(angle);
const uchar *center = &img.at<uchar>(cvRound(kpt.pt.y), cvRound(kpt.pt.x));
const int step = (int) img.step;
#define GET_VALUE(idx) \
center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + \
cvRound(pattern[idx].x*a - pattern[idx].y*b)]
for (int i = 0; i < 32; ++i, pattern += 16) {
int t0, t1, val;
t0 = GET_VALUE(0);
t1 = GET_VALUE(1);
val = t0 < t1;
t0 = GET_VALUE(2);
t1 = GET_VALUE(3);
val |= (t0 < t1) << 1;
t0 = GET_VALUE(4);
t1 = GET_VALUE(5);
val |= (t0 < t1) << 2;
t0 = GET_VALUE(6);
t1 = GET_VALUE(7);
val |= (t0 < t1) << 3;
t0 = GET_VALUE(8);
t1 = GET_VALUE(9);
val |= (t0 < t1) << 4;
t0 = GET_VALUE(10);
t1 = GET_VALUE(11);
val |= (t0 < t1) << 5;
t0 = GET_VALUE(12);
t1 = GET_VALUE(13);
val |= (t0 < t1) << 6;
t0 = GET_VALUE(14);
t1 = GET_VALUE(15);
val |= (t0 < t1) << 7;
desc[i] = (uchar) val;
}
#undef GET_VALUE
}
ORB 特征匹配
SearchByProjection 通过投影对局部地图点进行跟踪
简介：
-
@brief SearchByProjection 通过投影，对Local MapPoint进行跟踪
-
利用将相机坐标系下的Location MapPoints投影到图像坐标系，在其投影点附近根据描述子距离选取匹配，由此增加当前帧的MapPoints
参数：
- @param F 当前帧
- @param vpMapPoints 局部地图点
- @param th 搜索半径阈值。
- @param bFarPoints 是否地图点深度筛选
- @param thFarPoints 地图点深度筛选阈值
- @return 成功匹配的数量
步骤：
- 遍历所有地图点，若符合条件则找出匹配点
- 取出地图点，并对其进行评判，不符合要求则跳过
- 该地图点是不在视眼中或检测深度时深度过大或地图点质量不高
- 若地图点被 左图跟踪时，符合条件时得到匹配点
- 通过距离预测的金字塔层数，该层数相对于当前帧
- 确定搜索范围，若当前视角和平均视角夹角接近时，r取一个较小的值
- 如果需要进行更粗糙的搜索，则增大范围
r*=th - 通过投影点以及搜索窗口和预测的尺度进行搜索，找出附近的兴趣点
- 如果找到 兴趣点
- 得到该地图点描述子
- 遍历在该区域内找到了兴趣点，获得近关键点的最佳匹配和第二匹配
- 如果Frame中的该兴趣点已经有对应的MapPoint了,跳过
- 如果 跟踪误差评判较大时，跳过该兴趣点
- 取当前地图点的描述子，并计算描述子距离 DescriptorDistance
- 根据描述子寻找描述子距离 最小和次小的特征点(兴趣点)
- 若
最佳距离小于阈值，且第一第二有一定差异，则该匹配成功bestIdx- 也匹配右摄像头的立体观察
right++
- 也匹配右摄像头的立体观察
- 如果该地图点在右目中时
- 通过距离预测的金字塔层数，该层数相对于当前帧
- 同样确定搜索半径，找出该区域的兴趣点
- 同样得到最佳匹配和次佳匹配点
- 若
最佳距离小于阈值，且第一第二有一定差异，则该匹配成功bestIdx
- 取出地图点，并对其进行评判，不符合要求则跳过
- 返回匹配点个数
代码：
/**
* @brief SearchByProjection 通过投影，对Local MapPoint进行跟踪
* 利用将相机坐标系下的Location MapPoints投影到图像坐标系，在其投影点附近根据描述子距离选取匹配，由此增加当前帧的MapPoints
* @param F 当前帧
* @param vpMapPoints 局部地图点
* @param th 搜索半径阈值。
* @param bFarPoints 是否地图点深度筛选
* @param thFarPoints 地图点深度筛选阈值
* @return 成功匹配的数量
*/
int ORBmatcher::SearchByProjection(Frame &F, const vector<MapPoint*> &vpMapPoints
, const float th, const bool bFarPoints, const float thFarPoints)
{
int nmatches=0, left = 0, right = 0;
const bool bFactor = th!=1.0;
// 遍历所有地图点，若符合条件则找出匹配点
for(size_t iMP=0; iMP<vpMapPoints.size(); iMP++)
{
// 步骤1：取出地图点，并对其进行评判
MapPoint* pMP = vpMapPoints[iMP];
// 该地图点是不在视眼中 或 检测深度时深度过大 或 地图点质量不高，则 跳过
if(!pMP->mbTrackInView && !pMP->mbTrackInViewR)
continue;
if(bFarPoints && pMP->mTrackDepth>thFarPoints)
continue;
if(pMP->isBad())
continue;
// 若地图点被 左图跟踪时
if(pMP->mbTrackInView)
{
// 通过距离预测的金字塔层数，该层数相对于当前帧
const int &nPredictedLevel = pMP->mnTrackScaleLevel;
// The size of the window will depend on the viewing direction
// 确定搜索范围, 若当前视角和平均视角夹角接近时, r取一个较小的值
float r = RadiusByViewingCos(pMP->mTrackViewCos);
if(bFactor) // 如果需要进行更粗糙的搜索，则增大范围
r*=th;
// 通过投影点(投影到当前帧,见isInFrustum())以及搜索窗口和预测的尺度进行搜索, 找出附近的兴趣点
const vector<size_t> vIndices =
F.GetFeaturesInArea(pMP->mTrackProjX,pMP->mTrackProjY,r*F.mvScaleFactors[nPredictedLevel],nPredictedLevel-1,nPredictedLevel);
// 如果找到 兴趣点
if(!vIndices.empty()){
const cv::Mat MPdescriptor = pMP->GetDescriptor(); //得到该地图点描述子
int bestDist=256;
int bestLevel= -1;
int bestDist2=256;
int bestLevel2 = -1;
int bestIdx =-1 ;
// Get best and second matches with near keypoints
// 遍历在该区域内找到了兴趣点，获得近关键点的最佳匹配和第二匹配
for(vector<size_t>::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)
{
const float er = fabs(pMP->mTrackProjXR-F.mvuRight[idx]);
if(er>r*F.mvScaleFactors[nPredictedLevel])
continue;
}
const cv::Mat &d = F.mDescriptors.row(idx); // 取当前地图点的描述子
const int dist = DescriptorDistance(MPdescriptor,d); //求取描述子距离
// 根据描述子寻找描述子距离最小和次小的特征点(兴趣点)
if(dist<bestDist)
{
bestDist2=bestDist;
bestDist=dist;
bestLevel2 = bestLevel;
besdiantLevel = (F.Nleft == -1) ? F.mvKeysUn[idx].octave
: (idx < F.Nleft) ? F.mvKeys[idx].octave
: F.mvKeysRight[idx - F.Nleft].octave;
bestIdx=idx;
}
else if(dist<bestDist2)
{
bestLevel2 = (F.Nleft == -1) ? F.mvKeysUn[idx].octave
: (idx < F.Nleft) ? F.mvKeys[idx].octave
: F.mvKeysRight[idx - F.Nleft].octave;
bestDist2=dist;
}
} // finished 遍历在该区域内找到了兴趣点，获得近关键点的最佳匹配和第二匹配
// Apply ratio to second match (only if best and second are in the same scale level)
// 将比率应用于第二个匹配（仅当 best 和 second 处于同一比例级别时）
// 若`最佳距离`小于阈值，且第一第二有一定差异，则该匹配成功`bestIdx`
if(bestDist<=TH_HIGH)
{
if(bestLevel==bestLevel2 && bestDist>mfNNratio*bestDist2)
continue;
if(bestLevel!=bestLevel2 || bestDist<=mfNNratio*bestDist2){
F.mvpMapPoints[bestIdx]=pMP;
// 也匹配右摄像头的立体观察
if(F.Nleft != -1 && F.mvLeftToRightMatch[bestIdx] != -1){ //Also match with the stereo observation at right camera
F.mvpMapPoints[F.mvLeftToRightMatch[bestIdx] + F.Nleft] = pMP;
nmatches++;
right++;
}
nmatches++;
left++;
}
}
}
}
// 如果该地图点在右目中时，
if(F.Nleft != -1 && pMP->mbTrackInViewR){
// 通过距离预测的金字塔层数，该层数相对于当前帧
const int &nPredictedLevel = pMP->mnTrackScaleLevelR;
if(nPredictedLevel != -1){ // 层级存在时，
float r = RadiusByViewingCos(pMP->mTrackViewCosR);
// 同样确定搜索半径，找出该区域的兴趣点
const vector<size_t> vIndices =
F.GetFeaturesInArea(pMP->mTrackProjXR,pMP->mTrackProjYR,r*F.mvScaleFactors[nPredictedLevel],nPredictedLevel-1,nPredictedLevel,true);
if(vIndices.empty())
continue;
const cv::Mat MPdescriptor = pMP->GetDescriptor();
int bestDist=256;
int bestLevel= -1;
int bestDist2=256;
int bestLevel2 = -1;
int bestIdx =-1 ;
// Get best and second matches with near keypoints
// 同样得到最佳匹配和次佳匹配点
for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++)
{
const size_t idx = *vit;
if(F.mvpMapPoints[idx + F.Nleft])
if(F.mvpMapPoints[idx + F.Nleft]->Observations()>0)
continue;
const cv::Mat &d = F.mDescriptors.row(idx + F.Nleft);
const int dist = DescriptorDistance(MPdescriptor,d);
if(dist<bestDist)
{
bestDist2=bestDist;
bestDist=dist;
bestLevel2 = bestLevel;
bestLevel = F.mvKeysRight[idx].octave;
bestIdx=idx;
}
else if(dist<bestDist2)
{
bestLevel2 = F.mvKeysRight[idx].octave;
bestDist2=dist;
}
}
// Apply ratio to second match (only if best and second are in the same scale level)
// 若`最佳距离`小于阈值，且第一第二有一定差异，则该匹配成功`bestIdx`
if(bestDist<=TH_HIGH)
{
if(bestLevel==bestLevel2 && bestDist>mfNNratio*bestDist2)
continue;
if(F.Nleft != -1 && F.mvRightToLeftMatch[bestIdx] != -1){ //Also match with the stereo observation at right camera
F.mvpMapPoints[F.mvRightToLeftMatch[bestIdx]] = pMP;
nmatches++;
left++;
}
F.mvpMapPoints[bestIdx + F.Nleft]=pMP;
nmatches++;
right++;
}
}
}
}
return nmatches;
}
SearchByBoW 通过词袋对关键帧的特征点进行跟踪
简介：
- @brief SearchByBoW 通过词袋对关键帧和当前帧中的特征点进行快速匹配
- 通过词袋（bow）对关键帧（pKF）和当前帧（F）中的特征点进行快速匹配，不属于同一节点（node）的特征点直接跳过匹配，对属于同一节点（node0的特征点通过描述子距离进行匹配，根据匹配，用关键帧（pKF）中特征点对应的MapPoint更新F中特征点对应的MapPoints，通过距离阈值、比例阈值和角度投票进行剔除误匹配
参数：
- @param pKF 当前帧
- @param F 关键帧
- @param vpMapPointMatches 当前帧中MapPoints对应的匹配，NULL表示未匹配
- @return 成功匹配的数量
步骤：
-
从 关键帧中取出 匹配地图点
-
取 关键帧词袋的起始迭代器 + 当前帧词袋的起始迭代器
- 词袋：stl的map结构，key为wordId，value为tf id f中的tf
-
循环迭代器，直到关键帧词袋的迭代器指向其末端迭代器 或 当前帧词袋的迭代器 指向其末端迭代器
-
分别取出属于同一node的ORB特征点(只有属于同一node，才有可能是匹配点)
-
取 关键帧 和当前帧的 node value，两个vector
-
遍历 关键帧中属于该node的特征点
-
取出 关键帧中该特征对应的MapPoint的index
-
由index取当前地图点，若当前地图点空或者坏的直接continue
-
取 关键帧中该地图点对应的描述子
-
遍历当前帧中属于该node的特征点，找到了最佳匹配点，左图和右图一样
- 表明这个点已经被匹配过了，不再匹配，加快速度
- 取出F中该特征对应的描述子
- 计算 关键帧与当前帧描述子距离
- 找出最佳匹配和次佳匹配
-
根据阈值 和 角度投票剔除误匹配
最佳匹配距离(误差)小于阈值
-
最佳匹配比次佳匹配明显要好，那么最佳匹配才真正靠谱 。。两者有差异
-
更新特征点的MapPoint
-
如果检测 特征方向时、通过直方图统计得到最准确的角度变化值
每个特征点在提取描述子时的旋转主方向角度，如果图像旋转了，这个角度将发生改变，所有的特征点的角度变化应该是一致的
-
该特征点的角度变化值放入直方图中
rotHist统计
-
-
如果右图最佳匹配 低于阈值时，也进行同样的统计
- 与上面123步骤一样
-
-
-
-
否则 基于
id移动迭代器直到二者相等
-
-
根据方向剔除误匹配的点
- 计算rotHist中最大的三个的index
- 如果特征点的旋转角度变化量属于这三个组，则保留
- 将除了ind1 ind2 ind3以外的匹配点去掉
-
返回匹配个数
code：
/**
* @brief SearchByBoW 通过词袋对关键帧和当前帧中的特征点进行快速匹配
* 通过词袋（bow）对关键帧（pKF）和当前帧（F）中的特征点进行快速匹配，不属于同一节点（node）的特征点直接跳过匹配，
* 对属于同一节点（node0的特征点通过描述子距离进行匹配，根据匹配，用关键帧（pKF）中特征点对应的MapPoint更新F中特征点对应的MapPoints，
* 通过距离阈值、比例阈值和角度投票进行剔除误匹配
* @param pKF 关键帧
* @param F 当前帧
* @param vpMapPointMatches 当前帧中MapPoints对应的匹配，NULL表示未匹配
* @return 成功匹配的数量
*/
int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches)
{
// 从 关键帧中取出 匹配地图点
const vector<MapPoint*> vpMapPointsKF = pKF->GetMapPointMatches();
// 以 当前帧地图点个数 构建地图匹配点
vpMapPointMatches = vector<MapPoint*>(F.N,static_cast<MapPoint*>(NULL));
// stl的map结构，key为wordId，value为tfidf中的tf
const DBoW2::FeatureVector &vFeatVecKF = pKF->mFeatVec;
int nmatches=0; // 匹配点个数
vector<int> rotHist[HISTO_LENGTH];
for(int i=0;i<HISTO_LENGTH;i++)
rotHist[i].reserve(500);
const float factor = 1.0f/HISTO_LENGTH;
// We perform the matching over ORB that belong to the same vocabulary node (at a certain level)
// 我们对属于同一词汇节点（在某个级别）的 ORB 执行匹配
DBoW2::FeatureVector::const_iterator KFit = vFeatVecKF.begin();
DBoW2::FeatureVector::const_iterator Fit = F.mFeatVec.begin();
DBoW2::FeatureVector::const_iterator KFend = vFeatVecKF.end();
DBoW2::FeatureVector::const_iterator Fend = F.mFeatVec.end();
// 循环迭代器，直到关键帧词袋的迭代器指向其末端迭代器 或 当前帧词袋的迭代器 指向其末端迭代器
while(KFit != KFend && Fit != Fend)
{
// 分别取出属于同一node的ORB特征点(只有属于同一node，才有可能是匹配点)
if(KFit->first == Fit->first)
{
const vector<unsigned int> vIndicesKF = KFit->second;
const vector<unsigned int> vIndicesF = Fit->second;
// 遍历 关键帧中属于该node的特征点
for(size_t iKF=0; iKF<vIndicesKF.size(); iKF++)
{
// 取出KF中该特征对应的MapPoint的index
const unsigned int realIdxKF = vIndicesKF[iKF];
// 取当前地图点，若当前地图点空或者坏的直接continue
MapPoint* pMP = vpMapPointsKF[realIdxKF];
if(!pMP)
continue;
if(pMP->isBad())
continue;
// 取 关键帧中该地图点对应的描述子
const cv::Mat &dKF= pKF->mDescriptors.row(realIdxKF);
int bestDist1=256; // 最好的距离（最小距离）
int bestIdxF =-1 ;
int bestDist2=256; // 倒数第二好距离（倒数第二小距离）
int bestDist1R=256;
int bestIdxFR =-1 ;
int bestDist2R=256;
// 步骤3：遍历当前帧中属于该node的特征点，找到了最佳匹配点
for(size_t iF=0; iF<vIndicesF.size(); iF++)
{
if(F.Nleft == -1){
const unsigned int realIdxF = vIndicesF[iF];
// 表明这个点已经被匹配过了，不再匹配，加快速度
if(vpMapPointMatches[realIdxF])
continue;
// 取出F中该特征对应的描述子
const cv::Mat &dF = F.mDescriptors.row(realIdxF);
// 求描述子的距离
const int dist = DescriptorDistance(dKF,dF);
// 如果 dist < bestDist1 < bestDist2，更新bestDist1 bestDist2
if(dist<bestDist1)
{
bestDist2=bestDist1;
bestDist1=dist;
bestIdxF=realIdxF;
} // 否则 如果 bestDist1 < dist < bestDist2，更新bestDist2
else if(dist<bestDist2)
{
bestDist2=dist;
}
}
else{
// 非左图也一样支持，步骤跟左图一样
const unsigned int realIdxF = vIndicesF[iF];
if(vpMapPointMatches[realIdxF])
continue;
const cv::Mat &dF = F.mDescriptors.row(realIdxF);
const int dist = DescriptorDistance(dKF,dF);
if(realIdxF < F.Nleft && dist<bestDist1){
bestDist2=bestDist1;
bestDist1=dist;
bestIdxF=realIdxF;
}
else if(realIdxF < F.Nleft && dist<bestDist2){
bestDist2=dist;
}
if(realIdxF >= F.Nleft && dist<bestDist1R){
bestDist2R=bestDist1R;
bestDist1R=dist;
bestIdxFR=realIdxF;
}
else if(realIdxF >= F.Nleft && dist<bestDist2R){
bestDist2R=dist;
}
}
}
// 步骤4：根据阈值 和 角度投票剔除误匹配
if(bestDist1<=TH_LOW) // 匹配距离（误差）小于阈值
{
// 最佳匹配比次佳匹配明显要好，那么最佳匹配才真正靠谱 。。两者有差异
if(static_cast<float>(bestDist1)<mfNNratio*static_cast<float>(bestDist2))
{
// 步骤5：更新特征点的MapPoint
vpMapPointMatches[bestIdxF]=pMP;
const cv::KeyPoint &kp =
(!pKF->mpCamera2) ? pKF->mvKeysUn[realIdxKF] :
(realIdxKF >= pKF -> NLeft) ? pKF -> mvKeysRight[realIdxKF - pKF -> NLeft]
: pKF -> mvKeys[realIdxKF];
if(mbCheckOrientation)
{
// angle：每个特征点在提取描述子时的旋转主方向角度，如果图像旋转了，这个角度将发生改变
// 所有的特征点的角度变化应该是一致的，通过直方图统计得到最准确的角度变化值
cv::KeyPoint &Fkp =
(!pKF->mpCamera2 || F.Nleft == -1) ? F.mvKeys[bestIdxF] :
(bestIdxF >= F.Nleft) ? F.mvKeysRight[bestIdxF - F.Nleft]
: F.mvKeys[bestIdxF];
// 该特征点的角度变化值放入直方图中`rotHist`
float rot = kp.angle-Fkp.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(bestIdxF);
}
nmatches++;
}
// 如果右图最佳匹配 低于阈值时，也进行同样的统计
if(bestDist1R<=TH_LOW)
{
if(static_cast<float>(bestDist1R)<mfNNratio*static_cast<float>(bestDist2R) || true)
{
vpMapPointMatches[bestIdxFR]=pMP;
const cv::KeyPoint &kp =
(!pKF->mpCamera2) ? pKF->mvKeysUn[realIdxKF] :
(realIdxKF >= pKF -> NLeft) ? pKF -> mvKeysRight[realIdxKF - pKF -> NLeft]
: pKF -> mvKeys[realIdxKF];
if(mbCheckOrientation)
{
cv::KeyPoint &Fkp =
(!F.mpCamera2) ? F.mvKeys[bestIdxFR] :
(bestIdxFR >= F.Nleft) ? F.mvKeysRight[bestIdxFR - F.Nleft]
: F.mvKeys[bestIdxFR];
float rot = kp.angle-Fkp.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(bestIdxFR);
}
nmatches++;
}
}
}
}
KFit++;
Fit++;
}
else if(KFit->first < Fit->first)
{
KFit = vFeatVecKF.lower_bound(Fit->first);
}
else
{
Fit = F.mFeatVec.lower_bound(KFit->first);
}
}
// 根据方向剔除误匹配的点
if(mbCheckOrientation)
{
int ind1=-1;
int ind2=-1;
int ind3=-1;
// 计算rotHist中最大的三个的index
ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
for(int i=0; i<HISTO_LENGTH; i++)
{
// 如果特征点的旋转角度变化量属于这三个组，则保留
if(i==ind1 || i==ind2 || i==ind3)
continue;
// 将除了ind1 ind2 ind3以外的匹配点去掉
for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
{
vpMapPointMatches[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
nmatches--;
}
}
}
// 返回匹配个数
return nmatches;
}
R2000_laser_feature_param.laser_feature_const_param.parallel_line_mark_param.lines_delta_dist_error = 0.2
SearchByProjection 通过搜索区域与描述子跟踪特征点
简介：
- @brief 根据Sim3变换，将每个vpPoints投影到pKF上，并根据尺度确定一个搜索区域，根据该MapPoint的描述子与该区域内的特征点进行匹配，如果匹配误差小于TH_LOW即匹配成功，更新vpMatched
参数：
- @param pKF 关键帧
- @param Scw Sim3矩阵，即变换矩阵
- @param vpPoints 所有地图点
- @param vpMatched 匹配地图点
- @param th 阈值
- @return 成功匹配个数
步骤：
- 取当前关键帧外参，为后续校准做准备
- 分解参数Scw矩阵， 计算得到尺度
scw，Rcw，tcw： pKF坐标系下，世界坐标系到pKF的位移，方向由世界坐标系指向pKFOw：世界坐标系下，pKF到世界坐标系的位移（世界坐标系原点相对pKF的位置），方向由pKF指向世界坐标系
- 遍历所有的MapPoints
- 若 坏的MapPoints和已经匹配上的MapPoints，跳过
- 取该地图点的三维坐标(世界坐标系)，并将其转换到相机坐标系
- 若相机坐标系下，深度小于0时，跳过该地图点
- 将地图点投影到图像坐标系上
uv - 如果点不在图像上，则去掉
- 判断距离是否在尺度协方差范围内，不在则 continue
- 计算地图点到相机的长度，即尺度
- 若其长度不在协方差范围之内，则continue
- 视场角度:deg视角必须小于60度，否则continue
- 地图点基于尺寸和关键帧预测 层级，并确定根据尺度确定搜索半径
- 基于搜索半径，得到该区域内图片坐标系的特征indexs，为空则continue
- 取当前地图点的描述子
dMP - 遍历 上述区域地图内的地图点indexs，找到 描述子最近的
bestDist+bestIdx- 取关键帧中描述子，并进行距离判断，得到最小距离的描述子
- 若最佳距离小于阈值，则正面该地图点匹配成功
- 赋值匹配成功容器
- 匹配成功计数加一
- 返回 成功匹配的个数
Code:
/**
* @brief 根据Sim3变换，将每个vpPoints投影到pKF上，并根据尺度确定一个搜索区域，
* 根据该MapPoint的描述子与该区域内的特征点进行匹配，如果匹配误差小于TH_LOW即匹配成功，更新vpMatched
* @param pKF 关键帧
* @param Scw Sim3矩阵，即变换矩阵
* @param vpPoints 所有地图点
* @param vpMatched 匹配地图点
* @param th 阈值
* @return 成功匹配个数
*/
int ORBmatcher::SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const vector<MapPoint*> &vpPoints,
vector<MapPoint*> &vpMatched, int th, float ratioHamming)
{
// Get Calibration Parameters for later projection
// 取当前关键帧外参，为后续校准做准备
const float &fx = pKF->fx;
const float &fy = pKF->fy;
const float &cx = pKF->cx;
const float &cy = pKF->cy;
// Decompose Scw
// 分解参数Scw矩阵， 计算得到尺度`scw`，
cv::Mat sRcw = Scw.rowRange(0,3).colRange(0,3);
const float scw = sqrt(sRcw.row(0).dot(sRcw.row(0)));
cv::Mat Rcw = sRcw/scw;
cv::Mat tcw = Scw.rowRange(0,3).col(3)/scw; // pKF坐标系下，世界坐标系到pKF的位移，方向由世界坐标系指向pKF
cv::Mat Ow = -Rcw.t()*tcw; //世界坐标系下，pKF到世界坐标系的位移（世界坐标系原点相对pKF的位置），方向由pKF指向世界坐标系
// Set of MapPoints already found in the KeyFrame
// 使用set类型，并去除没有匹配的点，用于快速检索某个MapPoint是否有匹配
set<MapPoint*> spAlreadyFound(vpMatched.begin(), vpMatched.end());
spAlreadyFound.erase(static_cast<MapPoint*>(NULL));
int nmatches=0; //成功匹配个数
// For each Candidate MapPoint Project and Match
// 遍历所有的MapPoints
for(int iMP=0, iendMP=vpPoints.size(); iMP<iendMP; iMP++)
{
MapPoint* pMP = vpPoints[iMP];
// Discard Bad MapPoints and already found
// 丢弃坏的MapPoints和已经匹配上的MapPoints
if(pMP->isBad() || spAlreadyFound.count(pMP))
continue;
// Get 3D Coords. 取地图点三维坐标
cv::Mat p3Dw = pMP->GetWorldPos();
// Transform into Camera Coords. 转换到相机坐标系
cv::Mat p3Dc = Rcw*p3Dw+tcw;
// Depth must be positive 深度值小于0时跳过该地图点
if(p3Dc.at<float>(2)<0.0)
continue;
// Project into Image 将地图点投影到图像坐标系上 `uv`
const float x = p3Dc.at<float>(0);
const float y = p3Dc.at<float>(1);
const float z = p3Dc.at<float>(2);
const cv::Point2f uv = pKF->mpCamera->project(cv::Point3f(x,y,z));
// Point must be inside the image 如果点不在图像上，则去掉
if(!pKF->IsInImage(uv.x,uv.y))
continue;
// Depth must be inside the scale invariance region of the point
// 判断距离是否在尺度协方差范围内，不在则 continue
const float maxDistance = pMP->GetMaxDistanceInvariance();
const float minDistance = pMP->GetMinDistanceInvariance();
cv::Mat PO = p3Dw-Ow;
const float dist = cv::norm(PO);
if(dist<minDistance || dist>maxDistance)
continue;
// Viewing angle must be less than 60 deg
// 视场角度:deg视角必须小于60度
cv::Mat Pn = pMP->GetNormal();
if(PO.dot(Pn)<0.5*dist)
continue;
// 地图点基于尺寸和关键帧预测 层级，并确定根据尺度确定搜索半径
int nPredictedLevel = pMP->PredictScale(dist,pKF);
// Search in a radius
const float radius = th*pKF->mvScaleFactors[nPredictedLevel];
// 得到该区域内的地图点 indexs
const vector<size_t> vIndices = pKF->GetFeaturesInArea(uv.x,uv.y,radius);
if(vIndices.empty())
continue;
// Match to the most similar keypoint in the radius
// 匹配半径中最相似的关键点
const cv::Mat dMP = pMP->GetDescriptor(); // 取当前地图描述子
int bestDist = 256;
int bestIdx = -1;
// 遍历区域地图内的地图点index
for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++)
{
const size_t idx = *vit;
if(vpMatched[idx])
continue;
const int &kpLevel= pKF->mvKeysUn[idx].octave;
if(kpLevel<nPredictedLevel-1 || kpLevel>nPredictedLevel)
continue;
const cv::Mat &dKF = pKF->mDescriptors.row(idx);
const int dist = DescriptorDistance(dMP,dKF);
// 找到最小距离
if(dist<bestDist)
{
bestDist = dist;
bestIdx = idx;
}
}
if(bestDist<=TH_LOW*ratioHamming)
{
vpMatched[bestIdx]=pMP;
nmatches++;
}
}
return nmatches;
}
Fuse 关键帧与地图点进行融合
brief:
- 将MapPoints投影到关键帧pKF中，并判断是否有重复的MapPoints
- 1.如果MapPoint能匹配关键帧的特征点，并且该点有对应的MapPoint，那么将两个MapPoint合并（选择观测数多的）,
- 2.如果MapPoint能匹配关键帧的特征点，并且该点没有对应的MapPoint，那么为该点添加MapPoint
Param:
- @param pKF 相邻关键帧
- @param vpMapPoints 当前关键帧的地图点
- @param th 搜索半径阈值
- @param bRight 是否为右图
- @return 返回重复的地图点数
步骤：
-
得到当前关键帧的位姿+相机中心姿态+相机内参，主要bf
-
编译所有地图点
- 地图点是空、地图点是坏的、已经在关键帧中，则跳过 continue
- 将地图点转换到相机坐标系中，世界坐标系位姿+相机中心姿态
- 在相机坐标系下深度必须大于0，即在相机前方，否则跳过
- 将地图点投影到图像坐标系中，即计算
(
u
,
v
)
{(u,v)}
(u,v)
- 该地图点必须在图片上，否则跳过
- 地图点必须在图像关键的60°以内，否则跳过
- 基于当前距离 及 关键帧预测层级，并基于层级和阈值确定搜索半径
- 得到该半径区域内的 地图点 下标
- 若该区域地图点个数没有时，continue
- 取该地图点的描述子 dMp
- 遍历该半径区域内的所有地图点，找出最相似的关键帧点匹配
- 计算MapPoint投影的坐标与这个区域特征点的距离，如果偏差很大，直接跳过特征点匹配
- 其中特征点距离计算：图片上两个投影之欧式误差
- 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
- 找MapPoint在该区域最佳匹配的特征点
- 若最佳匹配距离小于阈值，如果已经有 MapPoint 替换，否则添加新的测量值
- 若没有直接添加
- 若有时，基于观测次数确定是否替换
- 返回 最佳匹配的个数
Code:
/** * @brief ORBmatcher::Fuse 将MapPoints投影到关键帧pKF中，并判断是否有重复的MapPoints, * 1.如果MapPoint能匹配关键帧的特征点，并且该点有对应的MapPoint，那么将两个MapPoint合并（选择观测数多的）, * 2.如果MapPoint能匹配关键帧的特征点，并且该点没有对应的MapPoint，那么为该点添加MapPoint * @param pKF 相邻关键帧 * @param vpMapPoints 当前关键帧的地图点 * @param th 搜索半径阈值 * @param bRight 是否为右图 * @return 返回重复的地图点数 */ int ORBmatcher::Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints, const float th, const bool bRight) { cv::Mat Rcw,tcw, Ow; GeometricCamera* pCamera; /// 基于是否为右图获取当前帧的位姿及相机中心 if(bRight){ Rcw = pKF->GetRightRotation(); tcw = pKF->GetRightTranslation(); Ow = pKF->GetRightCameraCenter(); pCamera = pKF->mpCamera2; } else{ Rcw = pKF->GetRotation(); tcw = pKF->GetTranslation(); Ow = pKF->GetCameraCenter(); pCamera = pKF->mpCamera; } /// 获取相机内参 const float &fx = pKF->fx; const float &fy = pKF->fy; const float &cx = pKF->cx; const float &cy = pKF->cy; const float &bf = pKF->mbf; int nFused=0; // 融合地图点个数 const int nMPs = vpMapPoints.size(); // For debbuging int count_notMP = 0, count_bad=0, count_isinKF = 0, count_negdepth = 0, count_notinim = 0, count_dist = 0, count_normal=0, count_notidx = 0, count_thcheck = 0; /// 遍历所有地图点 for(int i=0; i<nMPs; i++) { MapPoint* pMP = vpMapPoints[i]; if(!pMP) // 地图点是空的，跳过 { count_notMP++; continue; } if(pMP->isBad()) // 地图点是坏的，跳过 { count_bad++; continue; } else if(pMP->IsInKeyFrame(pKF)) // 已在关键帧中 { count_isinKF++; continue; } // 取地图点的世界坐标系位置 cv::Mat p3Dw = pMP->GetWorldPos(); cv::Mat p3Dc = Rcw*p3Dw + tcw; //求取MP在相机坐标系下的坐标 // Depth must be positive if(p3Dc.at<float>(2)<0.0f) // 地图点的深度必须大于0，即在相机前方 { count_negdepth++; continue; } // 计算得到MapPoint在图像上的投影坐标 uv const float invz = 1/p3Dc.at<float>(2); const float x = p3Dc.at<float>(0); const float y = p3Dc.at<float>(1); const float z = p3Dc.at<float>(2); const cv::Point2f uv = pCamera->project(cv::Point3f(x,y,z)); // Point must be inside the image if(!pKF->IsInImage(uv.x,uv.y)) // 若不在图像上，则continue { count_notinim++; continue; } const float ur = uv.x-bf*invz; // 深度阈值判断 const float maxDistance = pMP->GetMaxDistanceInvariance(); const float minDistance = pMP->GetMinDistanceInvariance(); cv::Mat PO = p3Dw-Ow; const float dist3D = cv::norm(PO); // Depth must be inside the scale pyramid of the image if(dist3D<minDistance || dist3D>maxDistance) { count_dist++; continue; } // 地图点必须在图像关键的60°以内 // Viewing angle must be less than 60 deg cv::Mat Pn = pMP->GetNormal(); if(PO.dot(Pn)<0.5*dist3D) { count_normal++; continue; } // 基于当前距离及 关键帧预测 层级 int nPredictedLevel = pMP->PredictScale(dist3D,pKF); // Search in a radius 基于阈值和层级确定搜索半径 const float radius = th*pKF->mvScaleFactors[nPredictedLevel]; // 得到该半径区域内的 地图点 下标 const vector<size_t> vIndices = pKF->GetFeaturesInArea(uv.x,uv.y,radius,bRight); if(vIndices.empty()) // 若该区域内地图点，则跳过 { count_notidx++; continue; } // Match to the most similar keypoint in the radius // 与半径中最相似的关键点匹配 const cv::Mat dMP = pMP->GetDescriptor(); int bestDist = 256; int bestIdx = -1; for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++) { size_t idx = *vit; const cv::KeyPoint &kp = (pKF -> NLeft == -1) ? pKF->mvKeysUn[idx] : (!bRight) ? pKF -> mvKeys[idx] : pKF -> mvKeysRight[idx]; const int &kpLevel= kp.octave; if(kpLevel<nPredictedLevel-1 || kpLevel>nPredictedLevel) continue; // 计算MapPoint投影的坐标与这个区域特征点的距离，如果偏差很大，直接跳过特征点匹配 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.x-kpx; const float ey = uv.y-kpy; const float er = ur-kpr; const float e2 = ex*ex+ey*ey+er*er; 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.x-kpx; const float ey = uv.y-kpy; const float e2 = ex*ex+ey*ey; //图片上的欧式距离 // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差） if(e2*pKF->mvInvLevelSigma2[kpLevel]>5.99) continue; } if(bRight) idx += pKF->NLeft; const cv::Mat &dKF = pKF->mDescriptors.row(idx); const int dist = DescriptorDistance(dMP,dKF); // 找MapPoint在该区域最佳匹配的特征点 if(dist<bestDist) { bestDist = dist; bestIdx = idx; } } // If there is already a MapPoint replace otherwise add new measurement // 若最佳匹配距离小于阈值，如果已经有 MapPoint 替换，否则添加新的测量值 if(bestDist<=TH_LOW) { MapPoint* pMPinKF = pKF->GetMapPoint(bestIdx); if(pMPinKF) { if(!pMPinKF->isBad()) { if(pMPinKF->Observations()>pMP->Observations()) // 哪个观察多选择哪个 pMP->Replace(pMPinKF); else pMPinKF->Replace(pMP); } } else { pMP->AddObservation(pKF,bestIdx); pKF->AddMapPoint(pMP,bestIdx); } nFused++; } else count_thcheck++; } return nFused; }
SearchBySim3 通过Sim3变换捕获之前漏掉的特征点，并更新匹配点
brief:
- 通过Sim3变换，确定pKF1的特征点在pKF2中的大致区域，同理，确定pKF2的特征点在pKF1中的大致区域，在该区域内通过描述子进行匹配捕获pKF1和pKF2之前漏匹配的特征点，更新vpMatches12（之前使用SearchByBoW进行特征点匹配时会有漏匹配）
Param:
- @param pKF1 关键帧1
- @param pKF2 关键帧2
- @param vpMatches12 关键帧12的匹配地图点
- @param s12 候选帧pKF到当前帧mpCurrentKF的变换尺度s
- @param R12 关键帧1到2的旋转变化
- @param t12 关键帧1到2的位置变化
- @param th 同一地图点阈值 7.5
- @return 匹配的点
步骤：
-
变量初始化
-
得到两关键帧，world到camera的变换
-
根据变换尺度得到 关键帧1,2的转换关系
-
取关键帧 1,2 的地图匹配点
- 用于记录该特征点是否被处理过
- 用于记录该特征点是否在pKF1中有匹配
-
用vpMatches12更新vbAlreadyMatched1和vbAlreadyMatched2
-
若该地图点非空，则 标记该特征点已经判断过
-
该地图点在 关键帧2 中的存在，则该特征点在pKF1中有匹配
-
通过Sim变换，确定pKF1的特征点在pKF2中的大致区域，
在该区域内通过描述子进行匹配捕获pKF1和pKF2之前漏匹配的特征点，更新vpMatches12
之前使用SearchByBoW进行特征点匹配时会有漏匹配
- 该特征点为空 或 已经在关键帧2中有匹配点了，直接跳过
- 把pKF1系下的MapPoint从world坐标系变换到camera1坐标系，再通过Sim3将该MapPoint从camera1变换到camera2坐标系
- 该地图点在 camera2 中深度必须大于0，否则跳过
- 投影到camera2图像平面，且其地图点必须在 图片上
- 基于相机内参计算 ( u , v ) {(u,v)} (u,v)，确定其是否在图片上
- 深度必须在尺度不变区域内
- 预测该MapPoint对应的特征点在图像金字塔哪一层，基于层数和阈值计算搜索半径
- 取出该搜索区域内的所有特征点下标，若为空跳过
- 遍历搜索区域内的所有特征点，与pMP进行描述子匹配，得到最佳匹配点
- 层级+描述子距离，取最佳距离
- 若最佳匹配距离小于阈值，确定其匹配点
-
通过Sim变换，确定pKF2的特征点在pKF1中的大致区域， 与3一样操作
在该区域内通过描述子进行匹配捕获pKF1和pKF2之前漏匹配的特征点，更新vpMatches12
之前使用SearchByBoW进行特征点匹配时会有漏匹配
-
检查确定匹配点。 历特征1中地图点，若其与特征2有匹配点，且下标一致，则更新匹配点，并设定已找到匹配点
-
返回找到的匹配点个数
代码：
/**
* @brief SearchBySim3 通过Sim3变换，确定pKF1的特征点在pKF2中的大致区域，同理，确定pKF2的特征点在pKF1中的大致区域
* 在该区域内通过描述子进行匹配捕获pKF1和pKF2之前漏匹配的特征点，更新vpMatches12（之前使用SearchByBoW进行特征点匹配时会有漏匹配）
* @param pKF1 关键帧1
* @param pKF2 关键帧2
* @param vpMatches12 关键帧12的匹配地图点
* @param s12
* @param R12 关键帧1到2的旋转变化
* @param t12 关键帧1到2的位置变化
* @param th 同一地图点阈值
* @return 匹配的点
*/
int ORBmatcher::SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint*> &vpMatches12,
const float &s12, const cv::Mat &R12, const cv::Mat &t12, const float th)
{
/// 步骤1：变量初始化
// 关键帧1的相机参数
const float &fx = pKF1->fx;
const float &fy = pKF1->fy;
const float &cx = pKF1->cx;
const float &cy = pKF1->cy;
// 从world到camera的变换
// Camera 1 from world
cv::Mat R1w = pKF1->GetRotation();
cv::Mat t1w = pKF1->GetTranslation();
//Camera 2 from world
cv::Mat R2w = pKF2->GetRotation();
cv::Mat t2w = pKF2->GetTranslation();
//Transformation between cameras
cv::Mat sR12 = s12*R12;
cv::Mat sR21 = (1.0/s12)*R12.t();
cv::Mat t21 = -sR21*t12;
// 取关键帧 1,2 的地图匹配点
const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();
const int N1 = vpMapPoints1.size();
const vector<MapPoint*> vpMapPoints2 = pKF2->GetMapPointMatches();
const int N2 = vpMapPoints2.size();
vector<bool> vbAlreadyMatched1(N1,false); // 用于记录该特征点是否被处理过
vector<bool> vbAlreadyMatched2(N2,false); // 用于记录该特征点是否在pKF1中有匹配
//步骤2：用vpMatches12更新vbAlreadyMatched1和vbAlreadyMatched2
for(int i=0; i<N1; i++)
{
MapPoint* pMP = vpMatches12[i];
if(pMP) // 该地图点非空
{
vbAlreadyMatched1[i]=true; // 该特征点已经判断过
int idx2 = get<0>(pMP->GetIndexInKeyFrame(pKF2));
if(idx2>=0 && idx2<N2)
vbAlreadyMatched2[idx2]=true; // 该特征点在pKF1中有匹配
}
}
vector<int> vnMatch1(N1,-1);
vector<int> vnMatch2(N2,-1);
// Transform from KF1 to KF2 and search
// 步骤3：通过Sim变换，确定pKF1的特征点在pKF2中的大致区域，
// 在该区域内通过描述子进行匹配捕获pKF1和pKF2之前漏匹配的特征点，更新vpMatches12
// （之前使用SearchByBoW进行特征点匹配时会有漏匹配）
for(int i1=0; i1<N1; i1++)
{
MapPoint* pMP = vpMapPoints1[i1];
if(!pMP || vbAlreadyMatched1[i1]) // 该特征点已经有匹配点了，直接跳过
continue;
if(pMP->isBad())
continue;
cv::Mat p3Dw = pMP->GetWorldPos();
cv::Mat p3Dc1 = R1w*p3Dw + t1w; // 把pKF1系下的MapPoint从world坐标系变换到camera1坐标系
cv::Mat p3Dc2 = sR21*p3Dc1 + t21; // 再通过Sim3将该MapPoint从camera1变换到camera2坐标系
// Depth must be positive 深度必须大于0
if(p3Dc2.at<float>(2)<0.0)
continue;
// 投影到camera2图像平面，且其地图点必须在 图片上
const float invz = 1.0/p3Dc2.at<float>(2);
const float x = p3Dc2.at<float>(0)*invz;
const float y = p3Dc2.at<float>(1)*invz;
const float u = fx*x+cx;
const float v = fy*y+cy;
// Point must be inside the image
if(!pKF2->IsInImage(u,v))
continue;
// 深度必须在尺度不变区域内
const float maxDistance = pMP->GetMaxDistanceInvariance();
const float minDistance = pMP->GetMinDistanceInvariance();
const float dist3D = cv::norm(p3Dc2);
// Depth must be inside the scale invariance region
if(dist3D<minDistance || dist3D>maxDistance )
continue;
// Compute predicted octave 预测该MapPoint对应的特征点在图像金字塔哪一层
const int nPredictedLevel = pMP->PredictScale(dist3D,pKF2);
// Search in a radius 基于层数和阈值计算搜索半径
const float radius = th*pKF2->mvScaleFactors[nPredictedLevel];
// 取出该搜索区域内的所有特征点下标，若为空跳过
const vector<size_t> vIndices = pKF2->GetFeaturesInArea(u,v,radius);
if(vIndices.empty())
continue;
// Match to the most similar keypoint in the radius 得到当前地图点的描述子
const cv::Mat dMP = pMP->GetDescriptor();
int bestDist = INT_MAX;
int bestIdx = -1;
// 遍历搜索区域内的所有特征点，与pMP进行描述子匹配，得到最佳匹配点
for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++)
{
const size_t idx = *vit;
const cv::KeyPoint &kp = pKF2->mvKeysUn[idx];
if(kp.octave<nPredictedLevel-1 || kp.octave>nPredictedLevel)
continue;
const cv::Mat &dKF = pKF2->mDescriptors.row(idx);
const int dist = DescriptorDistance(dMP,dKF);
if(dist<bestDist)
{
bestDist = dist;
bestIdx = idx;
}
}
// 若最佳匹配距离小于阈值，确定其匹配点
if(bestDist<=TH_HIGH)
{
vnMatch1[i1]=bestIdx;
}
}
// Transform from KF2 to KF2 and search
// 步骤3.2：通过Sim变换，确定pKF2的特征点在pKF1中的大致区域，
// 在该区域内通过描述子进行匹配捕获pKF1和pKF2之前漏匹配的特征点，更新vpMatches12
// （之前使用SearchByBoW进行特征点匹配时会有漏匹配）
for(int i2=0; i2<N2; i2++)
{
MapPoint* pMP = vpMapPoints2[i2];
if(!pMP || vbAlreadyMatched2[i2])
continue;
if(pMP->isBad())
continue;
cv::Mat p3Dw = pMP->GetWorldPos();
cv::Mat p3Dc2 = R2w*p3Dw + t2w;
cv::Mat p3Dc1 = sR12*p3Dc2 + t12;
// Depth must be positive
if(p3Dc1.at<float>(2)<0.0)
continue;
const float invz = 1.0/p3Dc1.at<float>(2);
const float x = p3Dc1.at<float>(0)*invz;
const float y = p3Dc1.at<float>(1)*invz;
const float u = fx*x+cx;
const float v = fy*y+cy;
// Point must be inside the image
if(!pKF1->IsInImage(u,v))
continue;
const float maxDistance = pMP->GetMaxDistanceInvariance();
const float minDistance = pMP->GetMinDistanceInvariance();
const float dist3D = cv::norm(p3Dc1);
// Depth must be inside the scale pyramid of the image
if(dist3D<minDistance || dist3D>maxDistance)
continue;
// Compute predicted octave
const int nPredictedLevel = pMP->PredictScale(dist3D,pKF1);
// Search in a radius of 2.5*sigma(ScaleLevel)
const float radius = th*pKF1->mvScaleFactors[nPredictedLevel];
const vector<size_t> vIndices = pKF1->GetFeaturesInArea(u,v,radius);
if(vIndices.empty())
continue;
// Match to the most similar keypoint in the radius
const cv::Mat dMP = pMP->GetDescriptor();
int bestDist = INT_MAX;
int bestIdx = -1;
for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++)
{
const size_t idx = *vit;
const cv::KeyPoint &kp = pKF1->mvKeysUn[idx];
if(kp.octave<nPredictedLevel-1 || kp.octave>nPredictedLevel)
continue;
const cv::Mat &dKF = pKF1->mDescriptors.row(idx);
const int dist = DescriptorDistance(dMP,dKF);
if(dist<bestDist)
{
bestDist = dist;
bestIdx = idx;
}
}
if(bestDist<=TH_HIGH)
{
vnMatch2[i2]=bestIdx;
}
}
// Check agreement 检查确定匹配点
int nFound = 0;
// 遍历特征1中地图点，若其与特征2有匹配点，且下标一致，则更新匹配点，并设定已找到匹配点
for(int i1=0; i1<N1; i1++)
{
int idx2 = vnMatch1[i1];
if(idx2>=0)
{
int idx1 = vnMatch2[idx2];
if(idx1==i1)
{
vpMatches12[i1] = vpMapPoints2[idx2];
nFound++;
}
}
}
return nFound;
}
SearchByProjection 通过投影，对上一帧的特征点进行跟踪
brief：
- 上一帧中包含了MapPoints，对这些MapPoints进行tracking，由此增加当前帧的MapPoints
- 将上一帧的MapPoints投影到当前帧(根据速度模型可以估计当前帧的Tcw)
- 在投影点附近根据描述子距离选取匹配，以及最终的方向投票机制进行剔除
Param：
- @param CurrentFrame 当前帧
- @param LastFrame 上一帧
- @param th 阈值
- @param bMono 是否为单目
- @return 成功匹配的数量
步骤：
- 处理前的数据准备
- 旋转直方图，为检查旋转的一致性做准备
- 得到当前帧和上一帧的 相机在世界坐标系的位姿
- 非单目情况，如果Z大于基线，则表示前进，如果Z小于基线，则表示后退
- 遍历上一关键帧地图点，对上一帧有效地图点进行跟踪
- 取 地图点的世界坐标系，并将其转换到相机坐标系
- 相机坐标系，其深度需大于0，否则continue
- 基于相机内参得到当前关键帧的图片坐标系
(
u
,
v
)
(u,v)
(u,v)
- 图片坐标系，该点在图片中，否则continue
- 确定查询半径，尺度越大，搜索范围越大
- 金字塔层数+搜索阈值共同确定
- 尺度越大,图像越小
- 当前进时，圆点的面积增大，在某个尺度m下它是一个特征点，由于面积增大，则需要在更高的尺度下才能检测出来，因此m>=n，对应前进的情况，nCurOctave>=nLastOctave。后退的情况可以类推
- 分前进后退得到搜索区域内的地图点
- 前进，则上一帧兴趣点在所在的尺度nLastOctave<=nCurOctave
- 后退，则上一帧兴趣点在所在的尺度0<=nCurOctave<=nLastOctave
- 在[nLastOctave-1, nLastOctave+1]中搜索
- 遍历满足条件的特征点，找到最佳匹配点
- 如果该特征点已经有对应的MapPoint了,则退出该次循环
- 双目和rgbd的情况，需要保证右图的点也在搜索半径以内
- 基于当前地图点描述子以及区域内地图点描述子确定
- 若最佳匹配小于阈值时，且检测各个地图点的方向一致性时，统计各个点的匹配角度误差
- 右图时，操作跟上面 4,5,6,7 一样
- 检测各个地图点的方向一致性
- 取出直方图中值最大的三个index
- 遍历方向差，若与三个都不同，则匹配个数减减，正常肯定在三个之上
- 返回匹配个数
Code：
/**
* @brief 通过投影，对上一帧的特征点进行跟踪
*
* 上一帧中包含了MapPoints，对这些MapPoints进行tracking，由此增加当前帧的MapPoints \n
* 1. 将上一帧的MapPoints投影到当前帧(根据速度模型可以估计当前帧的Tcw)
* 2. 在投影点附近根据描述子距离选取匹配，以及最终的方向投票机制进行剔除
* @param CurrentFrame 当前帧
* @param LastFrame 上一帧
* @param th 阈值
* @param bMono 是否为单目
* @return 成功匹配的数量
* @see SearchByBoW()
*/
int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono)
{
int nmatches = 0;
// Rotation Histogram (to check rotation consistency)
vector<int> rotHist[HISTO_LENGTH];
for(int i=0;i<HISTO_LENGTH;i++)
rotHist[i].reserve(500);
const float factor = HISTO_LENGTH/360.0f; //尺度
const cv::Mat Rcw = CurrentFrame.mTcw.rowRange(0,3).colRange(0,3);
const cv::Mat tcw = CurrentFrame.mTcw.rowRange(0,3).col(3);
const cv::Mat twc = -Rcw.t()*tcw; // twc(w)
const cv::Mat Rlw = LastFrame.mTcw.rowRange(0,3).colRange(0,3);
const cv::Mat tlw = LastFrame.mTcw.rowRange(0,3).col(3); // tlw(l)
// vector from LastFrame to CurrentFrame expressed in LastFrame
const cv::Mat tlc = Rlw*twc+tlw; // Rlw*twc(w) = twc(l), twc(l) + tlw(l) = tlc(l)
// 判断前进还是后退
const bool bForward = tlc.at<float>(2)>CurrentFrame.mb && !bMono; // 非单目情况，如果Z大于基线，则表示前进
const bool bBackward = -tlc.at<float>(2)>CurrentFrame.mb && !bMono; // 非单目情况，如果Z小于基线，则表示前进
for(int i=0; i<LastFrame.N; i++)
{
MapPoint* pMP = LastFrame.mvpMapPoints[i];
if(pMP)
{
if(!LastFrame.mvbOutlier[i])
{
// 对上一帧有效的MapPoints进行跟踪
// Project
cv::Mat x3Dw = pMP->GetWorldPos();
cv::Mat x3Dc = Rcw*x3Dw+tcw;
const float xc = x3Dc.at<float>(0);
const float yc = x3Dc.at<float>(1);
const float invzc = 1.0/x3Dc.at<float>(2);
if(invzc<0)
continue;
float u = CurrentFrame.fx*xc*invzc+CurrentFrame.cx;
float v = CurrentFrame.fy*yc*invzc+CurrentFrame.cy;
if(u<CurrentFrame.mnMinX || u>CurrentFrame.mnMaxX)
continue;
if(v<CurrentFrame.mnMinY || v>CurrentFrame.mnMaxY)
continue;
int nLastOctave = LastFrame.mvKeys[i].octave;
// Search in a window. Size depends on scale
float radius = th*CurrentFrame.mvScaleFactors[nLastOctave]; // 尺度越大，搜索范围越大
vector<size_t> vIndices2;
// NOTE 尺度越大,图像越小
// 以下可以这么理解，例如一个有一定面积的圆点，在某个尺度n下它是一个特征点
// 当前进时，圆点的面积增大，在某个尺度m下它是一个特征点，由于面积增大，则需要在更高的尺度下才能检测出来
// 因此m>=n，对应前进的情况，nCurOctave>=nLastOctave。后退的情况可以类推
if(bForward) // 前进,则上一帧兴趣点在所在的尺度nLastOctave<=nCurOctave
vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave);
else if(bBackward) // 后退,则上一帧兴趣点在所在的尺度0<=nCurOctave<=nLastOctave
vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, 0, nLastOctave);
else // 在[nLastOctave-1, nLastOctave+1]中搜索
vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave-1, nLastOctave+1);
if(vIndices2.empty())
continue;
const cv::Mat dMP = pMP->GetDescriptor();
int bestDist = 256;
int bestIdx2 = -1;
// 遍历满足条件的特征点
for(vector<size_t>::const_iterator vit=vIndices2.begin(), vend=vIndices2.end(); vit!=vend; vit++)
{
// 如果该特征点已经有对应的MapPoint了,则退出该次循环
const size_t i2 = *vit;
if(CurrentFrame.mvpMapPoints[i2])
if(CurrentFrame.mvpMapPoints[i2]->Observations()>0)
continue;
if(CurrentFrame.mvuRight[i2]>0)
{
// 双目和rgbd的情况，需要保证右图的点也在搜索半径以内
const float ur = u - CurrentFrame.mbf*invzc;
const float er = fabs(ur - CurrentFrame.mvuRight[i2]);
if(er>radius)
continue;
}
const cv::Mat &d = CurrentFrame.mDescriptors.row(i2);
const int dist = DescriptorDistance(dMP,d);
if(dist<bestDist)
{
bestDist=dist;
bestIdx2=i2;
}
}
// 详见SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches)函数步骤4
if(bestDist<=TH_HIGH)
{
CurrentFrame.mvpMapPoints[bestIdx2]=pMP; // 为当前帧添加MapPoint
nmatches++;
if(mbCheckOrientation)
{
float rot = LastFrame.mvKeysUn[i].angle-CurrentFrame.mvKeysUn[bestIdx2].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(bestIdx2);
}
}
}
}
}
//Apply rotation consistency
if(mbCheckOrientation)
{
int ind1=-1;
int ind2=-1;
int ind3=-1;
// 取出直方图中值最大的三个index
ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
// 遍历方向差，若与三个都不同，则匹配个数减减，正常肯定在三个之上
for(int i=0; i<HISTO_LENGTH; i++)
{
if(i!=ind1 && i!=ind2 && i!=ind3)
{
for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
{
CurrentFrame.mvpMapPoints[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
nmatches--;
}
}
}
}
return nmatches;
}
扫一扫
807
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
