ORBSLAM学习笔记7(ORBextractor)
void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoints)
图像提取特征点的范围
图像的矩
圆patch
static float IC_Angle(const Mat& image, Point2f pt, const vector<int> & u_max)
这个函数主要就是计算这个圆patch的图像矩,计算方向向量
ORBextractor.h
/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza)
* For more information see
*
* ORB-SLAM2 is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM2 is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM2. If not, see
#ifndef ORBEXTRACTOR_H
#define ORBEXTRACTOR_H
#include
#include ORBextractor.cc
/**
* This file is part of ORB-SLAM2.
* This file is based on the file orb.cpp from the OpenCV library (see BSD license below).
*
* Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza)
* For more information see
*
* ORB-SLAM2 is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM2 is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM2. If not, see
/**
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*/
#include >& allKeypoints: 金字塔每层有多少个特征点
* 1.这里首先将图像分成若干个cell,然后在每个cell里面提取FAST点;
* 2.
*/
void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoints)
{
allKeypoints.resize(nlevels);
// about 30 pixel every cell
//划分cell大小30个像素
const float W = 30;
///handle every level image
//处理每层的图像
for (int level = 0; level < nlevels; ++level)
{
/// limit x ,y range at this level, number 3 mean 3 pixel large than image, is FAST radius
//限制提取特征点的范围,数字3是FAST描述子的半径
//留出边的空间EDGE_THRESHOLD
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;
vector<cv::KeyPoint> vToDistributeKeys;
vToDistributeKeys.reserve(nfeatures*10);
const float width = (maxBorderX-minBorderX);
const float height = (maxBorderY-minBorderY);
// divided into cells
//取整,分成几个cell
const int nCols = width/W;
const int nRows = height/W;
// compute real cell size
//实际的网格尺寸
const int wCell = ceil(width/nCols);
const int hCell = ceil(height/nRows);
//extract FAST features in every cells
//遍历行
for(int i=0; i<nRows; i++)
{
/// y range [iniy, maxY]
const float iniY =minBorderY+i*hCell;
/// 6 means 2*FAST radius,cell之间重叠6个像素,刚好是FAST特征点的半径的两倍
float maxY = iniY+hCell+6;
/// in case surpass image border
//不足3个像素了,就放弃这行
if(iniY>=maxBorderY-3)
continue;
if(maxY>maxBorderY)
maxY = maxBorderY;
//遍历列
for(int j=0; j<nCols; j++)
{
const float iniX =minBorderX+j*wCell;
float maxX = iniX+wCell+6;
//不足6个像素了,就放弃这列像素
if(iniX>=maxBorderX-6)
continue;
if(maxX>maxBorderX)
maxX = maxBorderX;
//提取FAST角点
vector<cv::KeyPoint> vKeysCell;
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
vKeysCell,iniThFAST,true);
// if cannot detect fast features with iniThFAST then redo with miniThFAST threshold
if(vKeysCell.empty())
{
//FAST: input image, output feature, threshold,
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
vKeysCell,minThFAST,true);
}
//push keypoint into vToDistributeKeys
if(!vKeysCell.empty())
{
/**
* 因为特征点是在cell中提取的,因此这里需要计算实际的特征点位置,
* 这里得到坐标的还是相对于(minBorderX, minBorderY)这个点的
*/
for(vector<cv::KeyPoint>::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++)
{
(*vit).pt.x+=j*wCell;
(*vit).pt.y+=i*hCell;
vToDistributeKeys.push_back(*vit);
}
}
}
}
vector<KeyPoint> & keypoints = allKeypoints[level];
keypoints.reserve(nfeatures);
/// select best keypoints
///将提取的特征点分布在四叉树节点里面,然后在每个节点里面选择响应值最高的
keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX,
minBorderY, maxBorderY,mnFeaturesPerLevel[level], level);
//scale PATH_SIZE
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++)
{
//get keypoints's really coordinate in image
keypoints[i].pt.x+=minBorderX;// feature x coordinate
keypoints[i].pt.y+=minBorderY; // feature y coordinate
keypoints[i].octave=level; // feature at which level
keypoints[i].size = scaledPatchSize; // feature patch size used for compute feature descriptor
}
}//for level
// compute orientations
//计算图像的方向
for (int level = 0; level < nlevels; ++level)
computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);
}
void ORBextractor::ComputeKeyPointsOld(std::vector<std::vector<KeyPoint> > &allKeypoints)
{
allKeypoints.resize(nlevels);
float imageRatio = (float)mvImagePyramid[0].cols/mvImagePyramid[0].rows;
for (int level = 0; level < nlevels; ++level)
{
const int nDesiredFeatures = mnFeaturesPerLevel[level];
const int levelCols = sqrt((float)nDesiredFeatures/(5*imageRatio));
const int levelRows = imageRatio*levelCols;
const int minBorderX = EDGE_THRESHOLD;
const int minBorderY = minBorderX;
const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD;
const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD;
const int W = maxBorderX - minBorderX;
const int H = maxBorderY - minBorderY;
const int cellW = ceil((float)W/levelCols);
const int cellH = ceil((float)H/levelRows);
const int nCells = levelRows*levelCols;
const int nfeaturesCell = ceil((float)nDesiredFeatures/nCells);
vector<vector<vector<KeyPoint> > > cellKeyPoints(levelRows, vector<vector<KeyPoint> >(levelCols));
vector<vector<int> > nToRetain(levelRows,vector<int>(levelCols,0));
vector<vector<int> > nTotal(levelRows,vector<int>(levelCols,0));
vector<vector<bool> > bNoMore(levelRows,vector<bool>(levelCols,false));
vector<int> iniXCol(levelCols);
vector<int> iniYRow(levelRows);
int nNoMore = 0;
int nToDistribute = 0;
float hY = cellH + 6;
for(int i=0; i<levelRows; i++)
{
const float iniY = minBorderY + i*cellH - 3;
iniYRow[i] = iniY;
if(i == levelRows-1)
{
hY = maxBorderY+3-iniY;
if(hY<=0)
continue;
}
float hX = cellW + 6;
for(int j=0; j<levelCols; j++)
{
float iniX;
if(i==0)
{
iniX = minBorderX + j*cellW - 3;
iniXCol[j] = iniX;
}
else
{
iniX = iniXCol[j];
}
if(j == levelCols-1)
{
hX = maxBorderX+3-iniX;
if(hX<=0)
continue;
}
Mat cellImage = mvImagePyramid[level].rowRange(iniY,iniY+hY).colRange(iniX,iniX+hX);
cellKeyPoints[i][j].reserve(nfeaturesCell*5);
FAST(cellImage,cellKeyPoints[i][j],iniThFAST,true);
if(cellKeyPoints[i][j].size()<=3)
{
cellKeyPoints[i][j].clear();
FAST(cellImage,cellKeyPoints[i][j],minThFAST,true);
}
const int nKeys = cellKeyPoints[i][j].size();
nTotal[i][j] = nKeys;
if(nKeys>nfeaturesCell)
{
nToRetain[i][j] = nfeaturesCell;
bNoMore[i][j] = false;
}
else
{
nToRetain[i][j] = nKeys;
nToDistribute += nfeaturesCell-nKeys;
bNoMore[i][j] = true;
nNoMore++;
}
}
}
// Retain by score
while(nToDistribute>0 && nNoMore<nCells)
{
int nNewFeaturesCell = nfeaturesCell + ceil((float)nToDistribute/(nCells-nNoMore));
nToDistribute = 0;
for(int i=0; i<levelRows; i++)
{
for(int j=0; j<levelCols; j++)
{
if(!bNoMore[i][j])
{
if(nTotal[i][j]>nNewFeaturesCell)
{
nToRetain[i][j] = nNewFeaturesCell;
bNoMore[i][j] = false;
}
else
{
nToRetain[i][j] = nTotal[i][j];
nToDistribute += nNewFeaturesCell-nTotal[i][j];
bNoMore[i][j] = true;
nNoMore++;
}
}
}
}
}
vector<KeyPoint> & keypoints = allKeypoints[level];
keypoints.reserve(nDesiredFeatures*2);
const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level];
// Retain by score and transform coordinates
for(int i=0; i<levelRows; i++)
{
for(int j=0; j<levelCols; j++)
{
vector<KeyPoint> &keysCell = cellKeyPoints[i][j];
KeyPointsFilter::retainBest(keysCell,nToRetain[i][j]);
if((int)keysCell.size()>nToRetain[i][j])
keysCell.resize(nToRetain[i][j]);
for(size_t k=0, kend=keysCell.size(); k<kend; k++)
{
keysCell[k].pt.x+=iniXCol[j];
keysCell[k].pt.y+=iniYRow[i];
keysCell[k].octave=level;
keysCell[k].size = scaledPatchSize;
keypoints.push_back(keysCell[k]);
}
}
}
if((int)keypoints.size()>nDesiredFeatures)
{
KeyPointsFilter::retainBest(keypoints,nDesiredFeatures);
keypoints.resize(nDesiredFeatures);
}
}
// and compute orientations
for (int level = 0; level < nlevels; ++level)
computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);
}
static void computeDescriptors(const Mat& image, vector<KeyPoint>& keypoints, Mat& descriptors,
const vector<Point>& pattern)
{
/// every keypoint has a 256 = 32 * 8 bit descriptor
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));
}
/**
* 重载()操作符,提取图像特征点
*/
void ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints,
OutputArray _descriptors)
{
if(_image.empty())
return;
Mat image = _image.getMat();
assert(image.type() == CV_8UC1 );
// Pre-compute the scale pyramid
//计算图像金字塔
ComputePyramid(image);
//提取每层的特征点
vector < vector<KeyPoint> > allKeypoints;
ComputeKeyPointsOctTree(allKeypoints);
//ComputeKeyPointsOld(allKeypoints);
Mat descriptors;
/// compute all keypoints number
//统计特征点的总数
int nkeypoints = 0;
for (int level = 0; level < nlevels; ++level)
nkeypoints += (int)allKeypoints[level].size();
if( nkeypoints == 0 )
_descriptors.release();
else
{
_descriptors.create(nkeypoints, 32, CV_8U);
descriptors = _descriptors.getMat();
}
_keypoints.clear();
_keypoints.reserve(nkeypoints);
//金字塔逐层计算描述子
int offset = 0;
for (int level = 0; level < nlevels; ++level)
{
vector<KeyPoint>& keypoints = allKeypoints[level];
int nkeypointsLevel = (int)keypoints.size();
if(nkeypointsLevel==0)
continue;
// preprocess the resized image
Mat workingMat = mvImagePyramid[level].clone();
/// GaussianBlur: src, dst, kernel size, sigmaX, sigmaY, borderType
//高斯降噪
GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);
// Compute the descriptors
//计算描述子
Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel);
computeDescriptors(workingMat, keypoints, desc, pattern);
offset += nkeypointsLevel;
// Scale keypoint coordinates
//缩放描述子的坐标
if (level != 0)
{
float scale = mvScaleFactor[level]; //getScale(level, firstLevel, scaleFactor);
for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),
keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
keypoint->pt *= scale;
}
// And add the keypoints to the output
//输出到_keypoints
_keypoints.insert(_keypoints.end(), keypoints.begin(), keypoints.end());
}
}
/*计算图像金字塔 */
void ORBextractor::ComputePyramid(cv::Mat image)
{
for (int level = 0; level < nlevels; ++level)
{
//获取该层的缩放比例
float scale = mvInvScaleFactor[level];
/* scale both cols and rows */
//计算缩放图像的行和列的大小
Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale));
//在图像边上留一些空白的像素
Size wholeSize(sz.width + EDGE_THRESHOLD*2, sz.height + EDGE_THRESHOLD*2);
//新建空的图像
Mat temp(wholeSize, image.type()), masktemp;
mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height));
// Compute the resized image
if( level != 0 )
{
///resize: src, dst, size, fx, fy, inter_linear
//缩小尺寸存入mvImagePyramid[level]
resize(mvImagePyramid[level-1], mvImagePyramid[level], sz, 0, 0, INTER_LINEAR);
/**void cv::copyMakeBorder ( InputArray src,
OutputArray dst,
int top,
int bottom,
int left,
int right,
int borderType,
const Scalar & value = Scalar() )
扩充图像边缘
*/
//这里输出到temp无意义
copyMakeBorder(mvImagePyramid[level], temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
BORDER_REFLECT_101+BORDER_ISOLATED);
}
else
{
//这里输出到temp无意义
copyMakeBorder(image, temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
BORDER_REFLECT_101);
}
}
}
} //namespace ORB_SLAM