65 #include <opencv2/core/core.hpp>
66 #include <opencv2/highgui/highgui.hpp>
67 #include <opencv2/features2d/features2d.hpp>
68 #include <opencv2/imgproc/imgproc.hpp>
83 static float IC_Angle(
const Mat& image, Point2f pt,
const vector<int> & u_max,
int halfPatchSize)
85 int m_01 = 0, m_10 = 0;
87 const uchar* center = &image.at<
uchar> (cvRound(pt.y), cvRound(pt.x));
90 for (
int u = -halfPatchSize; u <= halfPatchSize; ++u)
91 m_10 += u * center[u];
95 for (
int v = 1;
v <= halfPatchSize; ++
v)
100 for (
int u = -
d; u <=
d; ++u)
102 int val_plus = center[u +
v*
step], val_minus = center[u -
v*
step];
103 v_sum += (val_plus - val_minus);
104 m_10 += u * (val_plus + val_minus);
109 return fastAtan2((
float)m_01, (
float)m_10);
115 const Mat&
img,
const Point* pattern,
121 const uchar* center = &
img.at<
uchar>(cvRound(kpt.pt.y), cvRound(kpt.pt.x));
125 center[cvRound(pattern[idx].
x*
b + pattern[idx].
y*
a)*
step + \
126 cvRound(pattern[idx].
x*
a - pattern[idx].
y*
b)]
129 for (
int i = 0;
i < 32; ++
i, pattern += 16)
135 val |= (t0 < t1) << 1;
137 val |= (t0 < t1) << 2;
139 val |= (t0 < t1) << 3;
141 val |= (t0 < t1) << 4;
143 val |= (t0 < t1) << 5;
145 val |= (t0 < t1) << 6;
147 val |= (t0 < t1) << 7;
416 ORBextractor::ORBextractor(
int _nfeatures,
float _scaleFactor,
int _nlevels,
417 int _iniThFAST,
int _minThFAST,
int _patchSize,
int _edgeThreshold):
418 nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels),
419 iniThFAST(_iniThFAST), minThFAST(_minThFAST), patchSize(_patchSize), edgeThreshold(_edgeThreshold)
444 float nDesiredFeaturesPerScale =
nfeatures*(1 - factor)/(1 - (
float)
pow((
double)factor, (
double)
nlevels));
451 nDesiredFeaturesPerScale *= factor;
455 const int npoints = 512;
457 std::copy(pattern0, pattern0 + npoints, std::back_inserter(
pattern));
466 for (
v = 0;
v <= vmax; ++
v)
479 static void computeOrientation(
const Mat& image, vector<KeyPoint>& keypoints,
const vector<int>& umax,
int halfPatchSize)
481 for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),
482 keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
484 keypoint->angle =
IC_Angle(image, keypoint->pt, umax, halfPatchSize);
490 const int halfX =
ceil(
static_cast<float>(
UR.x-
UL.x)/2);
491 const int halfY =
ceil(
static_cast<float>(
BR.y-
UL.y)/2);
495 n1.UR = cv::Point2i(
UL.x+halfX,
UL.y);
496 n1.BL = cv::Point2i(
UL.x,
UL.y+halfY);
497 n1.BR = cv::Point2i(
UL.x+halfX,
UL.y+halfY);
498 n1.vKeys.reserve(
vKeys.size());
503 n2.BR = cv::Point2i(
UR.x,
UL.y+halfY);
504 n2.vKeys.reserve(
vKeys.size());
509 n3.
BR = cv::Point2i(
n1.BR.x,
BL.y);
521 const cv::KeyPoint &kp =
vKeys[
i];
525 n1.vKeys.push_back(kp);
527 n3.
vKeys.push_back(kp);
529 else if(kp.pt.y<
n1.BR.y)
530 n2.vKeys.push_back(kp);
532 n4.
vKeys.push_back(kp);
535 if(
n1.vKeys.size()==1)
537 if(
n2.vKeys.size()==1)
539 if(n3.
vKeys.size()==1)
541 if(n4.
vKeys.size()==1)
547 const int &maxX,
const int &minY,
const int &maxY,
const int &N,
const int &level)
550 const int nIni =
round(
static_cast<float>(maxX-minX)/(maxY-minY));
552 const float hX =
static_cast<float>(maxX-minX)/nIni;
556 vector<ExtractorNode*> vpIniNodes;
557 vpIniNodes.resize(nIni);
559 for(
int i=0;
i<nIni;
i++)
562 ni.
UL = cv::Point2i(hX*
static_cast<float>(
i),0);
563 ni.
UR = cv::Point2i(hX*
static_cast<float>(
i+1),0);
564 ni.
BL = cv::Point2i(ni.
UL.x,maxY-minY);
565 ni.
BR = cv::Point2i(ni.
UR.x,maxY-minY);
566 ni.
vKeys.reserve(vToDistributeKeys.size());
568 lNodes.push_back(ni);
569 vpIniNodes[
i] = &lNodes.back();
573 for(
size_t i=0;
i<vToDistributeKeys.size();
i++)
575 const cv::KeyPoint &kp = vToDistributeKeys[
i];
576 vpIniNodes[kp.pt.x/hX]->vKeys.push_back(kp);
581 while(lit!=lNodes.
end())
583 if(lit->vKeys.
size()==1)
588 else if(lit->vKeys.
empty())
589 lit = lNodes.erase(lit);
594 bool bFinish =
false;
598 vector<pair<int,ExtractorNode*> > vSizeAndPointerToNode;
599 vSizeAndPointerToNode.reserve(lNodes.
size()*4);
605 int prevSize = lNodes.
size();
607 lit = lNodes.
begin();
611 vSizeAndPointerToNode.clear();
613 while(lit!=lNodes.
end())
625 lit->DivideNode(
n1,
n2,n3,n4);
628 if(
n1.vKeys.size()>0)
630 lNodes.push_front(
n1);
631 if(
n1.vKeys.size()>1)
634 vSizeAndPointerToNode.push_back(make_pair(
n1.vKeys.size(),&lNodes.front()));
635 lNodes.front().lit = lNodes.
begin();
638 if(
n2.vKeys.size()>0)
640 lNodes.push_front(
n2);
641 if(
n2.vKeys.size()>1)
644 vSizeAndPointerToNode.push_back(make_pair(
n2.vKeys.size(),&lNodes.front()));
645 lNodes.front().lit = lNodes.
begin();
648 if(n3.
vKeys.size()>0)
650 lNodes.push_front(n3);
651 if(n3.
vKeys.size()>1)
654 vSizeAndPointerToNode.push_back(make_pair(n3.
vKeys.size(),&lNodes.front()));
655 lNodes.front().lit = lNodes.
begin();
658 if(n4.
vKeys.size()>0)
660 lNodes.push_front(n4);
661 if(n4.
vKeys.size()>1)
664 vSizeAndPointerToNode.push_back(make_pair(n4.
vKeys.size(),&lNodes.front()));
665 lNodes.front().lit = lNodes.
begin();
669 lit=lNodes.erase(lit);
676 if((
int)lNodes.
size()>=
N || (
int)lNodes.
size()==prevSize)
680 else if(((
int)lNodes.
size()+nToExpand*3)>
N)
686 prevSize = lNodes.
size();
688 vector<pair<int,ExtractorNode*> > vPrevSizeAndPointerToNode = vSizeAndPointerToNode;
689 vSizeAndPointerToNode.clear();
691 sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end());
692 for(
int j=vPrevSizeAndPointerToNode.size()-1;
j>=0;
j--)
695 vPrevSizeAndPointerToNode[
j].second->DivideNode(
n1,
n2,n3,n4);
698 if(
n1.vKeys.size()>0)
700 lNodes.push_front(
n1);
701 if(
n1.vKeys.size()>1)
703 vSizeAndPointerToNode.push_back(make_pair(
n1.vKeys.size(),&lNodes.front()));
704 lNodes.front().lit = lNodes.
begin();
707 if(
n2.vKeys.size()>0)
709 lNodes.push_front(
n2);
710 if(
n2.vKeys.size()>1)
712 vSizeAndPointerToNode.push_back(make_pair(
n2.vKeys.size(),&lNodes.front()));
713 lNodes.front().lit = lNodes.
begin();
716 if(n3.
vKeys.size()>0)
718 lNodes.push_front(n3);
719 if(n3.
vKeys.size()>1)
721 vSizeAndPointerToNode.push_back(make_pair(n3.
vKeys.size(),&lNodes.front()));
722 lNodes.front().lit = lNodes.
begin();
725 if(n4.
vKeys.size()>0)
727 lNodes.push_front(n4);
728 if(n4.
vKeys.size()>1)
730 vSizeAndPointerToNode.push_back(make_pair(n4.
vKeys.size(),&lNodes.front()));
731 lNodes.front().lit = lNodes.
begin();
735 lNodes.erase(vPrevSizeAndPointerToNode[
j].
second->lit);
737 if((
int)lNodes.
size()>=
N)
741 if((
int)lNodes.
size()>=
N || (
int)lNodes.
size()==prevSize)
749 vector<cv::KeyPoint> vResultKeys;
753 vector<cv::KeyPoint> &vNodeKeys = lit->vKeys;
754 cv::KeyPoint* pKP = &vNodeKeys[0];
755 float maxResponse = pKP->response;
757 for(
size_t k=1;k<vNodeKeys.size();k++)
759 if(vNodeKeys[k].response>maxResponse)
762 maxResponse = vNodeKeys[k].response;
766 vResultKeys.push_back(*pKP);
781 const int minBorderY = minBorderX;
785 vector<cv::KeyPoint> vToDistributeKeys;
788 const float width = (maxBorderX-minBorderX);
789 const float height = (maxBorderY-minBorderY);
791 const int nCols = width/
W;
792 const int nRows = height/
W;
793 const int wCell =
ceil(width/nCols);
794 const int hCell =
ceil(height/nRows);
796 for(
int i=0;
i<nRows;
i++)
798 const float iniY =minBorderY+
i*hCell;
799 float maxY = iniY+hCell+6;
801 if(iniY>=maxBorderY-3)
806 for(
int j=0;
j<nCols;
j++)
808 const float iniX =minBorderX+
j*wCell;
809 float maxX = iniX+wCell+6;
810 if(iniX>=maxBorderX-6)
815 vector<cv::KeyPoint> vKeysCell;
819 if(vKeysCell.empty())
825 if(!vKeysCell.empty())
827 for(vector<cv::KeyPoint>::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++)
829 (*vit).pt.x+=
j*wCell;
830 (*vit).pt.y+=
i*hCell;
831 vToDistributeKeys.push_back(*vit);
838 vector<KeyPoint> & keypoints = allKeypoints[
level];
847 const int nkps = keypoints.size();
848 for(
int i=0;
i<nkps ;
i++)
850 keypoints[
i].pt.x+=minBorderX;
851 keypoints[
i].pt.y+=minBorderY;
852 keypoints[
i].octave=
level;
853 keypoints[
i].size = scaledPatchSize;
872 const int levelCols =
sqrt((
float)nDesiredFeatures/(5*imageRatio));
873 const int levelRows = imageRatio*levelCols;
876 const int minBorderY = minBorderX;
880 const int W = maxBorderX - minBorderX;
881 const int H = maxBorderY - minBorderY;
882 const int cellW =
ceil((
float)
W/levelCols);
883 const int cellH =
ceil((
float)
H/levelRows);
885 const int nCells = levelRows*levelCols;
886 const int nfeaturesCell =
ceil((
float)nDesiredFeatures/nCells);
888 vector<vector<vector<KeyPoint> > > cellKeyPoints(levelRows, vector<vector<KeyPoint> >(levelCols));
890 vector<vector<int> > nToRetain(levelRows,vector<int>(levelCols,0));
891 vector<vector<int> > nTotal(levelRows,vector<int>(levelCols,0));
892 vector<vector<bool> > bNoMore(levelRows,vector<bool>(levelCols,
false));
893 vector<int> iniXCol(levelCols);
894 vector<int> iniYRow(levelRows);
896 int nToDistribute = 0;
899 float hY = cellH + 6;
901 for(
int i=0;
i<levelRows;
i++)
903 const float iniY = minBorderY +
i*cellH - 3;
908 hY = maxBorderY+3-iniY;
913 float hX = cellW + 6;
915 for(
int j=0;
j<levelCols;
j++)
921 iniX = minBorderX +
j*cellW - 3;
932 hX = maxBorderX+3-iniX;
940 cellKeyPoints[
i][
j].reserve(nfeaturesCell*5);
944 if(cellKeyPoints[
i][
j].
size()<=3)
946 cellKeyPoints[
i][
j].clear();
952 const int nKeys = cellKeyPoints[
i][
j].size();
953 nTotal[
i][
j] = nKeys;
955 if(nKeys>nfeaturesCell)
957 nToRetain[
i][
j] = nfeaturesCell;
958 bNoMore[
i][
j] =
false;
962 nToRetain[
i][
j] = nKeys;
963 nToDistribute += nfeaturesCell-nKeys;
964 bNoMore[
i][
j] =
true;
974 while(nToDistribute>0 && nNoMore<nCells)
976 int nNewFeaturesCell = nfeaturesCell +
ceil((
float)nToDistribute/(nCells-nNoMore));
979 for(
int i=0;
i<levelRows;
i++)
981 for(
int j=0;
j<levelCols;
j++)
985 if(nTotal[
i][
j]>nNewFeaturesCell)
987 nToRetain[
i][
j] = nNewFeaturesCell;
988 bNoMore[
i][
j] =
false;
992 nToRetain[
i][
j] = nTotal[
i][
j];
993 nToDistribute += nNewFeaturesCell-nTotal[
i][
j];
994 bNoMore[
i][
j] =
true;
1002 vector<KeyPoint> & keypoints = allKeypoints[
level];
1003 keypoints.reserve(nDesiredFeatures*2);
1008 for(
int i=0;
i<levelRows;
i++)
1010 for(
int j=0;
j<levelCols;
j++)
1012 vector<KeyPoint> &keysCell = cellKeyPoints[
i][
j];
1013 KeyPointsFilter::retainBest(keysCell,nToRetain[
i][
j]);
1014 if((
int)keysCell.size()>nToRetain[
i][
j])
1015 keysCell.resize(nToRetain[
i][
j]);
1018 for(
size_t k=0, kend=keysCell.size(); k<kend; k++)
1020 keysCell[k].pt.x+=iniXCol[
j];
1021 keysCell[k].pt.y+=iniYRow[
i];
1022 keysCell[k].octave=
level;
1023 keysCell[k].size = scaledPatchSize;
1024 keypoints.push_back(keysCell[k]);
1029 if((
int)keypoints.size()>nDesiredFeatures)
1031 KeyPointsFilter::retainBest(keypoints,nDesiredFeatures);
1032 keypoints.resize(nDesiredFeatures);
1042 const vector<Point>& pattern)
1044 descriptors = Mat::zeros((
int)keypoints.size(), 32, CV_8UC1);
1046 for (
size_t i = 0;
i < keypoints.size();
i++)
1051 OutputArray _descriptors)
1056 Mat image = _image.getMat();
1057 assert(image.type() == CV_8UC1 );
1062 vector < vector<KeyPoint> > allKeypoints;
1070 nkeypoints += (
int)allKeypoints[
level].size();
1071 if( nkeypoints == 0 )
1072 _descriptors.release();
1075 _descriptors.create(nkeypoints, 32, CV_8U);
1076 descriptors = _descriptors.getMat();
1080 _keypoints.reserve(nkeypoints);
1085 vector<KeyPoint>& keypoints = allKeypoints[
level];
1086 int nkeypointsLevel = (
int)keypoints.
size();
1088 if(nkeypointsLevel==0)
1093 GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);
1099 offset += nkeypointsLevel;
1105 for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),
1106 keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
1107 keypoint->pt *=
scale;
1110 _keypoints.insert(_keypoints.end(), keypoints.begin(), keypoints.end());
1119 Size sz(cvRound((
float)image.cols*
scale), cvRound((
float)image.rows*
scale));
1121 Mat temp(wholeSize, image.type()), masktemp;
1130 BORDER_REFLECT_101+BORDER_ISOLATED);
1135 BORDER_REFLECT_101);