├── ORB_Modify ├── data │ ├── 1.png │ └── 2.png ├── lib │ └── liborb.so ├── run ├── result │ ├── orb_modify.png │ └── ORB_matcher.png ├── build.sh ├── README.md ├── CMakeLists.txt ├── ORB.cc ├── include │ ├── ORB_modify.h │ ├── gms_matcher.h │ └── ORBextractor.h ├── TUM2.yaml ├── ORB_matcher.cc └── src │ ├── ORB_modify.cc │ ├── gms_matcher.cc │ └── ORBextractor.cc └── README.md /ORB_Modify/data/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DongyingYu/ORB_Feature/HEAD/ORB_Modify/data/1.png -------------------------------------------------------------------------------- /ORB_Modify/data/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DongyingYu/ORB_Feature/HEAD/ORB_Modify/data/2.png -------------------------------------------------------------------------------- /ORB_Modify/lib/liborb.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DongyingYu/ORB_Feature/HEAD/ORB_Modify/lib/liborb.so -------------------------------------------------------------------------------- /ORB_Modify/run: -------------------------------------------------------------------------------- 1 | ./bin/orb_modify TUM2.yaml data/1.png 2 | 3 | ./bin/orb_matcher TUM2.yaml data/1.png data/2.png 4 | -------------------------------------------------------------------------------- /ORB_Modify/result/orb_modify.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DongyingYu/ORB_Feature/HEAD/ORB_Modify/result/orb_modify.png -------------------------------------------------------------------------------- /ORB_Modify/build.sh: -------------------------------------------------------------------------------- 1 | echo "Configuring and building ORB_modify ..." 2 | 3 | mkdir build 4 | cd build 5 | cmake .. 6 | make -j4 -------------------------------------------------------------------------------- /ORB_Modify/result/ORB_matcher.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DongyingYu/ORB_Feature/HEAD/ORB_Modify/result/ORB_matcher.png -------------------------------------------------------------------------------- /ORB_Modify/README.md: -------------------------------------------------------------------------------- 1 | 1、说明 2 | 此程序是将ORB-SLAM2代码中的ORB特征提取部分拿出来单独测试，并结合GMS算法进行特征匹配。 3 | (https://github.com/raulmur/ORB_SLAM2) 4 | (https://github.com/JiawangBian/GMS-Feature-Matcher) 5 | 6 | 2、系统和依赖库： 7 | 使用ubunttu16.04和OpenCV3.3.1进行的代码测试 8 | 9 | 3、编译 10 | ./build.sh 11 | 12 | 4、运行 13 | 特征提取：./bin/orb_modify TUM2.yaml data/1.png 14 | 特征提取与匹配：./bin/orb_matcher TUM2.yaml data/1.png data/2.png 15 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ORB_Feature 2 | 3 | 1、说明 4 | 此程序是将ORB-SLAM2代码中的ORB特征提取部分拿出来单独测试，并结合GMS算法进行特征匹配。 5 | (https://github.com/raulmur/ORB_SLAM2) 6 | (https://github.com/JiawangBian/GMS-Feature-Matcher) 7 | 8 | 2、系统和依赖库： 9 | 使用ubunttu16.04和OpenCV3.3.1进行的代码测试 10 | 11 | 3、编译 12 | ./build.sh 13 | 14 | 4、运行 15 | 特征提取：./bin/orb_modify TUM2.yaml data/1.png 16 | 特征提取与匹配：./bin/orb_matcher TUM2.yaml data/1.png data/2.png 17 | -------------------------------------------------------------------------------- /ORB_Modify/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project( orb ) 3 | 4 | # OpenCV 5 | find_package( OpenCV REQUIRED ) 6 | 7 | include_directories( 8 | ${PROJECT_SOURCE_DIR}/include 9 | ${OpenCV_INCLUDE_DIRS} 10 | ) 11 | 12 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) 13 | 14 | add_library(${PROJECT_NAME} SHARED 15 | src/ORB_modify.cc 16 | src/ORBextractor.cc 17 | src/gms_matcher.cc 18 | ) 19 | 20 | target_link_libraries(${PROJECT_NAME} 21 | ${OpenCV_LIBS} 22 | ) 23 | 24 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin) 25 | 26 | add_executable( orb_modify ORB.cc ) 27 | target_link_libraries( orb_modify ${PROJECT_NAME} ) 28 | 29 | add_executable( orb_matcher ORB_matcher.cc ) 30 | target_link_libraries( orb_matcher ${PROJECT_NAME} ) -------------------------------------------------------------------------------- /ORB_Modify/ORB.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "ORB_modify.h" 4 | #include 5 | 6 | using namespace std; 7 | using namespace cv; 8 | 9 | int main ( int argc, char** argv ) 10 | { 11 | if ( argc != 3 ) 12 | { 13 | cout<<"usage: ./orb_modify path_to_settings path_to_image "< 4 | #include 5 | 6 | #include "ORBextractor.h" 7 | 8 | #include 9 | 10 | using namespace std; 11 | using namespace cv; 12 | using namespace ORB_SLAM2; 13 | 14 | class ORB_modify 15 | { 16 | public: 17 | ORB_modify(const string &strSettingPath); 18 | 19 | void ORB_feature(cv::Mat &im); 20 | void UndistortKeyPoints(); 21 | 22 | public: 23 | //ORB 24 | ORBextractor* mpORBextractor; 25 | 26 | //Calibration matrix 27 | cv::Mat mK; 28 | cv::Mat mDistCoef; 29 | float mbf; 30 | 31 | cv::Mat mImGray; 32 | 33 | int N; 34 | 35 | // Vector of keypoints (original for visualization) and undistorted (actually used by the system). 36 | std::vector mvKeys, mvKeysUn; 37 | 38 | // ORB descriptor, each row associated to a keypoint. 39 | cv::Mat mDescriptors; 40 | 41 | //Color order (true RGB, false BGR, ignored if grayscale) 42 | bool mbRGB; 43 | }; 44 | -------------------------------------------------------------------------------- /ORB_Modify/TUM2.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 520.908620 9 | Camera.fy: 521.007327 10 | Camera.cx: 325.141442 11 | Camera.cy: 249.701764 12 | 13 | Camera.k1: 0.231222 14 | Camera.k2: -0.784899 15 | Camera.p1: -0.003257 16 | Camera.p2: -0.000105 17 | Camera.k3: 0.917205 18 | 19 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 20 | Camera.RGB: 1 21 | 22 | #-------------------------------------------------------------------------------------------- 23 | # ORB Parameters 24 | #-------------------------------------------------------------------------------------------- 25 | 26 | # ORB Extractor: Number of features per image 27 | ORBextractor.nFeatures: 1000 28 | 29 | # ORB Extractor: Scale factor between levels in the scale pyramid 30 | ORBextractor.scaleFactor: 1.2 31 | 32 | # ORB Extractor: Number of levels in the scale pyramid 33 | ORBextractor.nLevels: 8 34 | 35 | # ORB Extractor: Fast threshold 36 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 37 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 38 | # You can lower these values if your images have low contrast 39 | ORBextractor.iniThFAST: 20 40 | ORBextractor.minThFAST: 7 -------------------------------------------------------------------------------- /ORB_Modify/ORB_matcher.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "gms_matcher.h" 5 | #include "ORB_modify.h" 6 | #include 7 | 8 | using namespace std; 9 | using namespace cv; 10 | 11 | int main ( int argc, char** argv ) 12 | { 13 | if ( argc != 4 ) 14 | { 15 | cout<<"usage: ./orb_matcher path_to_settings path_to_image1 path_to_image2"< matches_all, matches_gms; 37 | BFMatcher matcher(NORM_HAMMING); 38 | matcher.match(ORB_left.mDescriptors, ORB_right.mDescriptors, matches_all); 39 | 40 | // GMS filter 41 | std::vector vbInliers; 42 | gms_matcher gms(ORB_left.mvKeysUn, img1.size(), ORB_right.mvKeysUn, img2.size(), matches_all); 43 | 44 | int num_inliers = gms.GetInlierMask(vbInliers, false, false); 45 | cout << "Get total " << num_inliers << " matches." << endl; 46 | 47 | // collect matches 48 | for (size_t i = 0; i < vbInliers.size(); ++i) 49 | { 50 | if (vbInliers[i] == true) 51 | { 52 | matches_gms.push_back(matches_all[i]); 53 | } 54 | } 55 | 56 | // draw matching 57 | Mat show =gms.DrawInlier(img1, img2, ORB_left.mvKeysUn, ORB_right.mvKeysUn, matches_gms, 2); 58 | 59 | imwrite("./result/ORB_matcher.png", show); 60 | imshow("ORB_matcher", show); 61 | 62 | cv::waitKey(0); 63 | return 0; 64 | } -------------------------------------------------------------------------------- /ORB_Modify/include/gms_matcher.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace std; 9 | using namespace cv; 10 | 11 | #define THRESH_FACTOR 6 12 | 13 | // 8 possible rotation and each one is 3 X 3 14 | const int mRotationPatterns[8][9] = { 15 | 1,2,3, 16 | 4,5,6, 17 | 7,8,9, 18 | 19 | 4,1,2, 20 | 7,5,3, 21 | 8,9,6, 22 | 23 | 7,4,1, 24 | 8,5,2, 25 | 9,6,3, 26 | 27 | 8,7,4, 28 | 9,5,1, 29 | 6,3,2, 30 | 31 | 9,8,7, 32 | 6,5,4, 33 | 3,2,1, 34 | 35 | 6,9,8, 36 | 3,5,7, 37 | 2,1,4, 38 | 39 | 3,6,9, 40 | 2,5,8, 41 | 1,4,7, 42 | 43 | 2,3,6, 44 | 1,5,9, 45 | 4,7,8 46 | }; 47 | 48 | // 5 level scales 49 | const double mScaleRatios[5] = { 1.0, 1.0 / 2, 1.0 / sqrt(2.0), sqrt(2.0), 2.0 }; 50 | 51 | 52 | class gms_matcher 53 | { 54 | public: 55 | 56 | // OpenCV Keypoints & Correspond Image Size & Nearest Neighbor Matches 57 | gms_matcher(const vector &vkp1, const Size size1, const vector &vkp2, const Size size2, const vector &vDMatches); 58 | ~gms_matcher(); 59 | 60 | // Get Inlier Mask 61 | // Return number of inliers 62 | int GetInlierMask(vector &vbInliers, bool WithScale = false, bool WithRotation = false); 63 | 64 | Mat DrawInlier(Mat &src1, Mat &src2, vector &kpt1, vector &kpt2, vector &inlier, int type); 65 | 66 | private: 67 | 68 | // Normalize Key Points to Range(0 - 1) 69 | void NormalizePoints(const vector &kp, const Size &size, vector &npts); 70 | 71 | // Convert OpenCV DMatch to Match (pair) 72 | void ConvertMatches(const vector &vDMatches, vector > &vMatches); 73 | 74 | int GetGridIndexLeft(const Point2f &pt, int type); 75 | int GetGridIndexRight(const Point2f &pt); 76 | 77 | // Assign Matches to Cell Pairs 78 | void AssignMatchPairs(int GridType); 79 | 80 | // Verify Cell Pairs 81 | void VerifyCellPairs(int RotationType); 82 | 83 | // Get Neighbor 9 84 | vector GetNB9(const int idx, const Size& GridSize); 85 | 86 | void InitalizeNiehbors(Mat &neighbor, const Size& GridSize); 87 | void SetScale(int Scale); 88 | 89 | // Run 90 | int run(int RotationType); 91 | 92 | private: 93 | 94 | // Normalized Points 95 | vector mvP1, mvP2; 96 | 97 | // Matches 98 | vector > mvMatches; 99 | 100 | // Number of Matches 101 | size_t mNumberMatches; 102 | 103 | // Grid Size 104 | Size mGridSizeLeft, mGridSizeRight; 105 | int mGridNumberLeft; 106 | int mGridNumberRight; 107 | 108 | // x : left grid idx 109 | // y : right grid idx 110 | // value : how many matches from idx_left to idx_right 111 | Mat mMotionStatistics; 112 | 113 | // 114 | vector mNumberPointsInPerCellLeft; 115 | 116 | // Inldex : grid_idx_left 117 | // Value : grid_idx_right 118 | vector mCellPairs; 119 | 120 | // Every Matches has a cell-pair 121 | // first : grid_idx_left 122 | // second : grid_idx_right 123 | vector > mvMatchPairs; 124 | 125 | // Inlier Mask for output 126 | vector mvbInlierMask; 127 | 128 | // 129 | Mat mGridNeighborLeft; 130 | Mat mGridNeighborRight; 131 | }; -------------------------------------------------------------------------------- /ORB_Modify/include/ORBextractor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef ORBEXTRACTOR_H 22 | #define ORBEXTRACTOR_H 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | 29 | namespace ORB_SLAM2 30 | { 31 | 32 | class ExtractorNode 33 | { 34 | public: 35 | ExtractorNode():bNoMore(false){} 36 | 37 | void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4); 38 | 39 | std::vector vKeys; 40 | cv::Point2i UL, UR, BL, BR; 41 | std::list::iterator lit; 42 | bool bNoMore; 43 | }; 44 | 45 | class ORBextractor 46 | { 47 | public: 48 | 49 | enum {HARRIS_SCORE=0, FAST_SCORE=1 }; 50 | 51 | ORBextractor(int nfeatures, float scaleFactor, int nlevels, 52 | int iniThFAST, int minThFAST); 53 | 54 | ~ORBextractor(){} 55 | 56 | // Compute the ORB features and descriptors on an image. 57 | // ORB are dispersed on the image using an octree. 58 | // Mask is ignored in the current implementation. 59 | void operator()( cv::InputArray image, cv::InputArray mask, 60 | std::vector& keypoints, 61 | cv::OutputArray descriptors); 62 | 63 | int inline GetLevels(){ 64 | return nlevels;} 65 | 66 | float inline GetScaleFactor(){ 67 | return scaleFactor;} 68 | 69 | std::vector inline GetScaleFactors(){ 70 | return mvScaleFactor; 71 | } 72 | 73 | std::vector inline GetInverseScaleFactors(){ 74 | return mvInvScaleFactor; 75 | } 76 | 77 | std::vector inline GetScaleSigmaSquares(){ 78 | return mvLevelSigma2; 79 | } 80 | 81 | std::vector inline GetInverseScaleSigmaSquares(){ 82 | return mvInvLevelSigma2; 83 | } 84 | 85 | std::vector mvImagePyramid; 86 | 87 | protected: 88 | 89 | void ComputePyramid(cv::Mat image); 90 | void ComputeKeyPointsOctTree(std::vector >& allKeypoints); 91 | std::vector DistributeOctTree(const std::vector& vToDistributeKeys, const int &minX, 92 | const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level); 93 | 94 | void ComputeKeyPointsOld(std::vector >& allKeypoints); 95 | std::vector pattern; 96 | 97 | int nfeatures; 98 | double scaleFactor; 99 | int nlevels; 100 | int iniThFAST; 101 | int minThFAST; 102 | 103 | std::vector mnFeaturesPerLevel; 104 | 105 | std::vector umax; 106 | 107 | std::vector mvScaleFactor; 108 | std::vector mvInvScaleFactor; 109 | std::vector mvLevelSigma2; 110 | std::vector mvInvLevelSigma2; 111 | }; 112 | 113 | } //namespace ORB_SLAM 114 | 115 | #endif 116 | 117 | -------------------------------------------------------------------------------- /ORB_Modify/src/ORB_modify.cc: -------------------------------------------------------------------------------- 1 | #include "ORB_modify.h" 2 | 3 | ORB_modify::ORB_modify(const string &strSettingPath) 4 | { 5 | // Load camera parameters from settings file 6 | cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ); 7 | float fx = fSettings["Camera.fx"]; 8 | float fy = fSettings["Camera.fy"]; 9 | float cx = fSettings["Camera.cx"]; 10 | float cy = fSettings["Camera.cy"]; 11 | 12 | cv::Mat K = cv::Mat::eye(3,3,CV_32F); 13 | K.at(0,0) = fx; 14 | K.at(1,1) = fy; 15 | K.at(0,2) = cx; 16 | K.at(1,2) = cy; 17 | K.copyTo(mK); 18 | 19 | cv::Mat DistCoef(4,1,CV_32F); 20 | DistCoef.at(0) = fSettings["Camera.k1"]; 21 | DistCoef.at(1) = fSettings["Camera.k2"]; 22 | DistCoef.at(2) = fSettings["Camera.p1"]; 23 | DistCoef.at(3) = fSettings["Camera.p2"]; 24 | const float k3 = fSettings["Camera.k3"]; 25 | if(k3!=0) 26 | { 27 | DistCoef.resize(5); 28 | DistCoef.at(4) = k3; 29 | } 30 | DistCoef.copyTo(mDistCoef); 31 | 32 | mbf = fSettings["Camera.bf"]; 33 | 34 | cout << endl << "Camera Parameters: " << endl; 35 | cout << "- fx: " << fx << endl; 36 | cout << "- fy: " << fy << endl; 37 | cout << "- cx: " << cx << endl; 38 | cout << "- cy: " << cy << endl; 39 | cout << "- k1: " << DistCoef.at(0) << endl; 40 | cout << "- k2: " << DistCoef.at(1) << endl; 41 | if(DistCoef.rows==5) 42 | cout << "- k3: " << DistCoef.at(4) << endl; 43 | cout << "- p1: " << DistCoef.at(2) << endl; 44 | cout << "- p2: " << DistCoef.at(3) << endl; 45 | 46 | int nRGB = fSettings["Camera.RGB"]; 47 | mbRGB = nRGB; 48 | 49 | if(mbRGB) 50 | cout << "- color order: RGB (ignored if grayscale)" << endl; 51 | else 52 | cout << "- color order: BGR (ignored if grayscale)" << endl; 53 | 54 | // Load ORB parameters 55 | int nFeatures = fSettings["ORBextractor.nFeatures"]; 56 | float fScaleFactor = fSettings["ORBextractor.scaleFactor"]; 57 | int nLevels = fSettings["ORBextractor.nLevels"]; 58 | int fIniThFAST = fSettings["ORBextractor.iniThFAST"]; 59 | int fMinThFAST = fSettings["ORBextractor.minThFAST"]; 60 | 61 | mpORBextractor = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST); 62 | 63 | cout << endl << "ORB Extractor Parameters: " << endl; 64 | cout << "- Number of Features: " << nFeatures << endl; 65 | cout << "- Scale Levels: " << nLevels << endl; 66 | cout << "- Scale Factor: " << fScaleFactor << endl; 67 | cout << "- Initial Fast Threshold: " << fIniThFAST << endl; 68 | cout << "- Minimum Fast Threshold: " << fMinThFAST << endl; 69 | } 70 | 71 | void ORB_modify::ORB_feature(cv::Mat &im) 72 | { 73 | cv::Mat mImGray = im; 74 | cv::Mat Image; 75 | 76 | if(mImGray.channels()==3) 77 | { 78 | if(mbRGB) 79 | cvtColor(mImGray,mImGray,CV_RGB2GRAY); 80 | else 81 | cvtColor(mImGray,mImGray,CV_BGR2GRAY); 82 | } 83 | else if(mImGray.channels()==4) 84 | { 85 | if(mbRGB) 86 | cvtColor(mImGray,mImGray,CV_RGBA2GRAY); 87 | else 88 | cvtColor(mImGray,mImGray,CV_BGRA2GRAY); 89 | } 90 | 91 | (*mpORBextractor)(mImGray,cv::Mat(),mvKeys,mDescriptors); 92 | 93 | N = mvKeys.size(); 94 | if(mvKeys.empty()) 95 | { 96 | cout<<"no orb features?" <(0)==0.0) 110 | { 111 | mvKeysUn=mvKeys; 112 | return; 113 | } 114 | 115 | // Fill matrix with points 116 | cv::Mat mat(N,2,CV_32F); 117 | for(int i=0; i(i,0)=mvKeys[i].pt.x; 120 | mat.at(i,1)=mvKeys[i].pt.y; 121 | } 122 | 123 | // Undistort points 124 | mat=mat.reshape(2); 125 | cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK); 126 | mat=mat.reshape(1); 127 | 128 | // Fill undistorted keypoint vector 129 | mvKeysUn.resize(N); 130 | for(int i=0; i(i,0); 134 | kp.pt.y=mat.at(i,1); 135 | mvKeysUn[i]=kp; 136 | } 137 | } -------------------------------------------------------------------------------- /ORB_Modify/src/gms_matcher.cc: -------------------------------------------------------------------------------- 1 | #include "gms_matcher.h" 2 | 3 | // OpenCV Keypoints & Correspond Image Size & Nearest Neighbor Matches 4 | gms_matcher::gms_matcher(const vector &vkp1, const Size size1, const vector &vkp2, const Size size2, const vector &vDMatches) 5 | { 6 | // Input initialize 7 | NormalizePoints(vkp1, size1, mvP1); 8 | NormalizePoints(vkp2, size2, mvP2); 9 | mNumberMatches = vDMatches.size(); 10 | ConvertMatches(vDMatches, mvMatches); 11 | 12 | // Grid initialize 13 | mGridSizeLeft = Size(20, 20); 14 | mGridNumberLeft = mGridSizeLeft.width * mGridSizeLeft.height; 15 | 16 | // Initialize the neihbor of left grid 17 | mGridNeighborLeft = Mat::zeros(mGridNumberLeft, 9, CV_32SC1); 18 | InitalizeNiehbors(mGridNeighborLeft, mGridSizeLeft); 19 | }; 20 | 21 | gms_matcher::~gms_matcher() 22 | { 23 | 24 | } 25 | 26 | // Normalize Key Points to Range(0 - 1) 27 | void gms_matcher::NormalizePoints(const vector &kp, const Size &size, vector &npts) 28 | { 29 | const size_t numP = kp.size(); 30 | const int width = size.width; 31 | const int height = size.height; 32 | npts.resize(numP); 33 | 34 | for (size_t i = 0; i < numP; i++) 35 | { 36 | npts[i].x = kp[i].pt.x / width; 37 | npts[i].y = kp[i].pt.y / height; 38 | } 39 | } 40 | 41 | // Convert OpenCV DMatch to Match (pair) 42 | void gms_matcher::ConvertMatches(const vector &vDMatches, vector > &vMatches) 43 | { 44 | vMatches.resize(mNumberMatches); 45 | for (size_t i = 0; i < mNumberMatches; i++) 46 | { 47 | vMatches[i] = pair(vDMatches[i].queryIdx, vDMatches[i].trainIdx); 48 | } 49 | } 50 | 51 | int gms_matcher::GetGridIndexLeft(const Point2f &pt, int type) 52 | { 53 | int x = 0, y = 0; 54 | 55 | if (type == 1) 56 | { 57 | x = floor(pt.x * mGridSizeLeft.width); 58 | y = floor(pt.y * mGridSizeLeft.height); 59 | 60 | if (y >= mGridSizeLeft.height || x >= mGridSizeLeft.width) 61 | return -1; 62 | } 63 | 64 | if (type == 2) 65 | { 66 | x = floor(pt.x * mGridSizeLeft.width + 0.5); 67 | y = floor(pt.y * mGridSizeLeft.height); 68 | 69 | if (x >= mGridSizeLeft.width || x < 1) 70 | return -1; 71 | } 72 | 73 | if (type == 3) 74 | { 75 | x = floor(pt.x * mGridSizeLeft.width); 76 | y = floor(pt.y * mGridSizeLeft.height + 0.5); 77 | 78 | if (y >= mGridSizeLeft.height || y < 1) 79 | return -1; 80 | } 81 | 82 | if (type == 4) 83 | { 84 | x = floor(pt.x * mGridSizeLeft.width + 0.5); 85 | y = floor(pt.y * mGridSizeLeft.height + 0.5); 86 | 87 | if (y >= mGridSizeLeft.height || y < 1 || x >= mGridSizeLeft.width || x < 1) 88 | return -1; 89 | } 90 | 91 | return x + y * mGridSizeLeft.width; 92 | } 93 | 94 | int gms_matcher::GetGridIndexRight(const Point2f &pt) 95 | { 96 | int x = floor(pt.x * mGridSizeRight.width); 97 | int y = floor(pt.y * mGridSizeRight.height); 98 | 99 | return x + y * mGridSizeRight.width; 100 | } 101 | 102 | // Get Neighbor 9 103 | vector gms_matcher::GetNB9(const int idx, const Size& GridSize) 104 | { 105 | vector NB9(9, -1); 106 | 107 | int idx_x = idx % GridSize.width; 108 | int idx_y = idx / GridSize.width; 109 | 110 | for (int yi = -1; yi <= 1; yi++) 111 | { 112 | for (int xi = -1; xi <= 1; xi++) 113 | { 114 | int idx_xx = idx_x + xi; 115 | int idx_yy = idx_y + yi; 116 | 117 | if (idx_xx < 0 || idx_xx >= GridSize.width || idx_yy < 0 || idx_yy >= GridSize.height) 118 | continue; 119 | 120 | NB9[xi + 4 + yi * 3] = idx_xx + idx_yy * GridSize.width; 121 | } 122 | } 123 | return NB9; 124 | } 125 | 126 | void gms_matcher::InitalizeNiehbors(Mat &neighbor, const Size& GridSize) 127 | { 128 | for (int i = 0; i < neighbor.rows; i++) 129 | { 130 | vector NB9 = GetNB9(i, GridSize); 131 | int *data = neighbor.ptr(i); 132 | memcpy(data, &NB9[0], sizeof(int) * 9); 133 | } 134 | } 135 | 136 | void gms_matcher::SetScale(int Scale) 137 | { 138 | // Set Scale 139 | mGridSizeRight.width = mGridSizeLeft.width * mScaleRatios[Scale]; 140 | mGridSizeRight.height = mGridSizeLeft.height * mScaleRatios[Scale]; 141 | mGridNumberRight = mGridSizeRight.width * mGridSizeRight.height; 142 | 143 | // Initialize the neihbor of right grid 144 | mGridNeighborRight = Mat::zeros(mGridNumberRight, 9, CV_32SC1); 145 | InitalizeNiehbors(mGridNeighborRight, mGridSizeRight); 146 | } 147 | 148 | int gms_matcher::GetInlierMask(vector &vbInliers, bool WithScale, bool WithRotation) { 149 | 150 | int max_inlier = 0; 151 | 152 | if (!WithScale && !WithRotation) 153 | { 154 | SetScale(0); 155 | max_inlier = run(1); 156 | vbInliers = mvbInlierMask; 157 | return max_inlier; 158 | } 159 | 160 | if (WithRotation && WithScale) 161 | { 162 | for (int Scale = 0; Scale < 5; Scale++) 163 | { 164 | SetScale(Scale); 165 | for (int RotationType = 1; RotationType <= 8; RotationType++) 166 | { 167 | int num_inlier = run(RotationType); 168 | 169 | if (num_inlier > max_inlier) 170 | { 171 | vbInliers = mvbInlierMask; 172 | max_inlier = num_inlier; 173 | } 174 | } 175 | } 176 | return max_inlier; 177 | } 178 | 179 | if (WithRotation && !WithScale) 180 | { 181 | SetScale(0); 182 | for (int RotationType = 1; RotationType <= 8; RotationType++) 183 | { 184 | int num_inlier = run(RotationType); 185 | 186 | if (num_inlier > max_inlier) 187 | { 188 | vbInliers = mvbInlierMask; 189 | max_inlier = num_inlier; 190 | } 191 | } 192 | return max_inlier; 193 | } 194 | 195 | if (!WithRotation && WithScale) 196 | { 197 | for (int Scale = 0; Scale < 5; Scale++) 198 | { 199 | SetScale(Scale); 200 | 201 | int num_inlier = run(1); 202 | 203 | if (num_inlier > max_inlier) 204 | { 205 | vbInliers = mvbInlierMask; 206 | max_inlier = num_inlier; 207 | } 208 | 209 | } 210 | return max_inlier; 211 | } 212 | 213 | return max_inlier; 214 | } 215 | 216 | void gms_matcher::AssignMatchPairs(int GridType) { 217 | 218 | for (size_t i = 0; i < mNumberMatches; i++) 219 | { 220 | Point2f &lp = mvP1[mvMatches[i].first]; 221 | Point2f &rp = mvP2[mvMatches[i].second]; 222 | 223 | int lgidx = mvMatchPairs[i].first = GetGridIndexLeft(lp, GridType); 224 | int rgidx = -1; 225 | 226 | if (GridType == 1) 227 | { 228 | rgidx = mvMatchPairs[i].second = GetGridIndexRight(rp); 229 | } 230 | else 231 | { 232 | rgidx = mvMatchPairs[i].second; 233 | } 234 | 235 | if (lgidx < 0 || rgidx < 0) continue; 236 | 237 | mMotionStatistics.at(lgidx, rgidx)++; 238 | mNumberPointsInPerCellLeft[lgidx]++; 239 | } 240 | 241 | } 242 | 243 | void gms_matcher::VerifyCellPairs(int RotationType) { 244 | 245 | const int *CurrentRP = mRotationPatterns[RotationType - 1]; 246 | 247 | for (int i = 0; i < mGridNumberLeft; i++) 248 | { 249 | if (sum(mMotionStatistics.row(i))[0] == 0) 250 | { 251 | mCellPairs[i] = -1; 252 | continue; 253 | } 254 | 255 | int max_number = 0; 256 | for (int j = 0; j < mGridNumberRight; j++) 257 | { 258 | int *value = mMotionStatistics.ptr(i); 259 | if (value[j] > max_number) 260 | { 261 | mCellPairs[i] = j; 262 | max_number = value[j]; 263 | } 264 | } 265 | 266 | int idx_grid_rt = mCellPairs[i]; 267 | 268 | const int *NB9_lt = mGridNeighborLeft.ptr(i); 269 | const int *NB9_rt = mGridNeighborRight.ptr(idx_grid_rt); 270 | 271 | int score = 0; 272 | double thresh = 0; 273 | int numpair = 0; 274 | 275 | for (size_t j = 0; j < 9; j++) 276 | { 277 | int ll = NB9_lt[j]; 278 | int rr = NB9_rt[CurrentRP[j] - 1]; 279 | if (ll == -1 || rr == -1) continue; 280 | 281 | score += mMotionStatistics.at(ll, rr); 282 | thresh += mNumberPointsInPerCellLeft[ll]; 283 | numpair++; 284 | } 285 | 286 | thresh = THRESH_FACTOR * sqrt(thresh / numpair); 287 | 288 | if (score < thresh) 289 | mCellPairs[i] = -2; 290 | } 291 | } 292 | 293 | int gms_matcher::run(int RotationType) { 294 | 295 | mvbInlierMask.assign(mNumberMatches, false); 296 | 297 | // Initialize Motion Statisctics 298 | mMotionStatistics = Mat::zeros(mGridNumberLeft, mGridNumberRight, CV_32SC1); 299 | mvMatchPairs.assign(mNumberMatches, pair(0, 0)); 300 | 301 | for (int GridType = 1; GridType <= 4; GridType++) 302 | { 303 | // initialize 304 | mMotionStatistics.setTo(0); 305 | mCellPairs.assign(mGridNumberLeft, -1); 306 | mNumberPointsInPerCellLeft.assign(mGridNumberLeft, 0); 307 | 308 | AssignMatchPairs(GridType); 309 | VerifyCellPairs(RotationType); 310 | 311 | // Mark inliers 312 | for (size_t i = 0; i < mNumberMatches; i++) 313 | { 314 | if (mvMatchPairs[i].first >= 0) { 315 | if (mCellPairs[mvMatchPairs[i].first] == mvMatchPairs[i].second) 316 | { 317 | mvbInlierMask[i] = true; 318 | } 319 | } 320 | } 321 | } 322 | int num_inlier = sum(mvbInlierMask)[0]; 323 | return num_inlier; 324 | } 325 | 326 | Mat gms_matcher::DrawInlier(Mat &src1, Mat &src2, vector &kpt1, vector &kpt2, vector &inlier, int type) 327 | { 328 | const int height = max(src1.rows, src2.rows); 329 | const int width = src1.cols + src2.cols; 330 | Mat output(height, width, CV_8UC3, Scalar(0, 0, 0)); 331 | src1.copyTo(output(Rect(0, 0, src1.cols, src1.rows))); 332 | src2.copyTo(output(Rect(src1.cols, 0, src2.cols, src2.rows))); 333 | 334 | if (type == 1) 335 | { 336 | for (size_t i = 0; i < inlier.size(); i++) 337 | { 338 | Point2f left = kpt1[inlier[i].queryIdx].pt; 339 | Point2f right = (kpt2[inlier[i].trainIdx].pt + Point2f((float)src1.cols, 0.f)); 340 | line(output, left, right, Scalar(0, 255, 255)); 341 | } 342 | } 343 | else if (type == 2) 344 | { 345 | for (size_t i = 0; i < inlier.size(); i++) 346 | { 347 | Point2f left = kpt1[inlier[i].queryIdx].pt; 348 | Point2f right = (kpt2[inlier[i].trainIdx].pt + Point2f((float)src1.cols, 0.f)); 349 | line(output, left, right, Scalar(255, 0, 0)); 350 | } 351 | 352 | for (size_t i = 0; i < inlier.size(); i++) 353 | { 354 | Point2f left = kpt1[inlier[i].queryIdx].pt; 355 | Point2f right = (kpt2[inlier[i].trainIdx].pt + Point2f((float)src1.cols, 0.f)); 356 | circle(output, left, 1, Scalar(0, 255, 255), 2); 357 | circle(output, right, 1, Scalar(0, 255, 0), 2); 358 | } 359 | } 360 | 361 | return output; 362 | } -------------------------------------------------------------------------------- /ORB_Modify/src/ORBextractor.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * This file is based on the file orb.cpp from the OpenCV library (see BSD license below). 4 | * 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 6 | * For more information see 7 | * 8 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * ORB-SLAM2 is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with ORB-SLAM2. If not, see . 20 | */ 21 | /** 22 | * Software License Agreement (BSD License) 23 | * 24 | * Copyright (c) 2009, Willow Garage, Inc. 25 | * All rights reserved. 26 | * 27 | * Redistribution and use in source and binary forms, with or without 28 | * modification, are permitted provided that the following conditions 29 | * are met: 30 | * 31 | * * Redistributions of source code must retain the above copyright 32 | * notice, this list of conditions and the following disclaimer. 33 | * * Redistributions in binary form must reproduce the above 34 | * copyright notice, this list of conditions and the following 35 | * disclaimer in the documentation and/or other materials provided 36 | * with the distribution. 37 | * * Neither the name of the Willow Garage nor the names of its 38 | * contributors may be used to endorse or promote products derived 39 | * from this software without specific prior written permission. 40 | * 41 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 42 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 43 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 44 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 45 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 46 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 47 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 48 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 49 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 50 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 51 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 52 | * POSSIBILITY OF SUCH DAMAGE. 53 | * 54 | */ 55 | 56 | 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | 63 | #include "ORBextractor.h" 64 | 65 | 66 | using namespace cv; 67 | using namespace std; 68 | 69 | namespace ORB_SLAM2 70 | { 71 | 72 | const int PATCH_SIZE = 31; 73 | const int HALF_PATCH_SIZE = 15; 74 | const int EDGE_THRESHOLD = 19; 75 | 76 | 77 | static float IC_Angle(const Mat& image, Point2f pt, const vector & u_max) 78 | { 79 | int m_01 = 0, m_10 = 0; 80 | 81 | const uchar* center = &image.at (cvRound(pt.y), cvRound(pt.x)); 82 | 83 | // Treat the center line differently, v=0 84 | for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u) 85 | m_10 += u * center[u]; 86 | 87 | // Go line by line in the circuI853lar patch 88 | int step = (int)image.step1(); 89 | for (int v = 1; v <= HALF_PATCH_SIZE; ++v) 90 | { 91 | // Proceed over the two lines 92 | int v_sum = 0; 93 | int d = u_max[v]; 94 | for (int u = -d; u <= d; ++u) 95 | { 96 | int val_plus = center[u + v*step], val_minus = center[u - v*step]; 97 | v_sum += (val_plus - val_minus); 98 | m_10 += u * (val_plus + val_minus); 99 | } 100 | m_01 += v * v_sum; 101 | } 102 | 103 | return fastAtan2((float)m_01, (float)m_10); 104 | } 105 | 106 | 107 | const float factorPI = (float)(CV_PI/180.f); 108 | static void computeOrbDescriptor(const KeyPoint& kpt, 109 | const Mat& img, const Point* pattern, 110 | uchar* desc) 111 | { 112 | float angle = (float)kpt.angle*factorPI; 113 | float a = (float)cos(angle), b = (float)sin(angle); 114 | 115 | const uchar* center = &img.at(cvRound(kpt.pt.y), cvRound(kpt.pt.x)); 116 | const int step = (int)img.step; 117 | 118 | #define GET_VALUE(idx) \ 119 | center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + \ 120 | cvRound(pattern[idx].x*a - pattern[idx].y*b)] 121 | 122 | 123 | for (int i = 0; i < 32; ++i, pattern += 16) 124 | { 125 | int t0, t1, val; 126 | t0 = GET_VALUE(0); t1 = GET_VALUE(1); 127 | val = t0 < t1; 128 | t0 = GET_VALUE(2); t1 = GET_VALUE(3); 129 | val |= (t0 < t1) << 1; 130 | t0 = GET_VALUE(4); t1 = GET_VALUE(5); 131 | val |= (t0 < t1) << 2; 132 | t0 = GET_VALUE(6); t1 = GET_VALUE(7); 133 | val |= (t0 < t1) << 3; 134 | t0 = GET_VALUE(8); t1 = GET_VALUE(9); 135 | val |= (t0 < t1) << 4; 136 | t0 = GET_VALUE(10); t1 = GET_VALUE(11); 137 | val |= (t0 < t1) << 5; 138 | t0 = GET_VALUE(12); t1 = GET_VALUE(13); 139 | val |= (t0 < t1) << 6; 140 | t0 = GET_VALUE(14); t1 = GET_VALUE(15); 141 | val |= (t0 < t1) << 7; 142 | 143 | desc[i] = (uchar)val; 144 | } 145 | 146 | #undef GET_VALUE 147 | } 148 | 149 | 150 | static int bit_pattern_31_[256*4] = 151 | { 152 | 8,-3, 9,5/*mean (0), correlation (0)*/, 153 | 4,2, 7,-12/*mean (1.12461e-05), correlation (0.0437584)*/, 154 | -11,9, -8,2/*mean (3.37382e-05), correlation (0.0617409)*/, 155 | 7,-12, 12,-13/*mean (5.62303e-05), correlation (0.0636977)*/, 156 | 2,-13, 2,12/*mean (0.000134953), correlation (0.085099)*/, 157 | 1,-7, 1,6/*mean (0.000528565), correlation (0.0857175)*/, 158 | -2,-10, -2,-4/*mean (0.0188821), correlation (0.0985774)*/, 159 | -13,-13, -11,-8/*mean (0.0363135), correlation (0.0899616)*/, 160 | -13,-3, -12,-9/*mean (0.121806), correlation (0.099849)*/, 161 | 10,4, 11,9/*mean (0.122065), correlation (0.093285)*/, 162 | -13,-8, -8,-9/*mean (0.162787), correlation (0.0942748)*/, 163 | -11,7, -9,12/*mean (0.21561), correlation (0.0974438)*/, 164 | 7,7, 12,6/*mean (0.160583), correlation (0.130064)*/, 165 | -4,-5, -3,0/*mean (0.228171), correlation (0.132998)*/, 166 | -13,2, -12,-3/*mean (0.00997526), correlation (0.145926)*/, 167 | -9,0, -7,5/*mean (0.198234), correlation (0.143636)*/, 168 | 12,-6, 12,-1/*mean (0.0676226), correlation (0.16689)*/, 169 | -3,6, -2,12/*mean (0.166847), correlation (0.171682)*/, 170 | -6,-13, -4,-8/*mean (0.101215), correlation (0.179716)*/, 171 | 11,-13, 12,-8/*mean (0.200641), correlation (0.192279)*/, 172 | 4,7, 5,1/*mean (0.205106), correlation (0.186848)*/, 173 | 5,-3, 10,-3/*mean (0.234908), correlation (0.192319)*/, 174 | 3,-7, 6,12/*mean (0.0709964), correlation (0.210872)*/, 175 | -8,-7, -6,-2/*mean (0.0939834), correlation (0.212589)*/, 176 | -2,11, -1,-10/*mean (0.127778), correlation (0.20866)*/, 177 | -13,12, -8,10/*mean (0.14783), correlation (0.206356)*/, 178 | -7,3, -5,-3/*mean (0.182141), correlation (0.198942)*/, 179 | -4,2, -3,7/*mean (0.188237), correlation (0.21384)*/, 180 | -10,-12, -6,11/*mean (0.14865), correlation (0.23571)*/, 181 | 5,-12, 6,-7/*mean (0.222312), correlation (0.23324)*/, 182 | 5,-6, 7,-1/*mean (0.229082), correlation (0.23389)*/, 183 | 1,0, 4,-5/*mean (0.241577), correlation (0.215286)*/, 184 | 9,11, 11,-13/*mean (0.00338507), correlation (0.251373)*/, 185 | 4,7, 4,12/*mean (0.131005), correlation (0.257622)*/, 186 | 2,-1, 4,4/*mean (0.152755), correlation (0.255205)*/, 187 | -4,-12, -2,7/*mean (0.182771), correlation (0.244867)*/, 188 | -8,-5, -7,-10/*mean (0.186898), correlation (0.23901)*/, 189 | 4,11, 9,12/*mean (0.226226), correlation (0.258255)*/, 190 | 0,-8, 1,-13/*mean (0.0897886), correlation (0.274827)*/, 191 | -13,-2, -8,2/*mean (0.148774), correlation (0.28065)*/, 192 | -3,-2, -2,3/*mean (0.153048), correlation (0.283063)*/, 193 | -6,9, -4,-9/*mean (0.169523), correlation (0.278248)*/, 194 | 8,12, 10,7/*mean (0.225337), correlation (0.282851)*/, 195 | 0,9, 1,3/*mean (0.226687), correlation (0.278734)*/, 196 | 7,-5, 11,-10/*mean (0.00693882), correlation (0.305161)*/, 197 | -13,-6, -11,0/*mean (0.0227283), correlation (0.300181)*/, 198 | 10,7, 12,1/*mean (0.125517), correlation (0.31089)*/, 199 | -6,-3, -6,12/*mean (0.131748), correlation (0.312779)*/, 200 | 10,-9, 12,-4/*mean (0.144827), correlation (0.292797)*/, 201 | -13,8, -8,-12/*mean (0.149202), correlation (0.308918)*/, 202 | -13,0, -8,-4/*mean (0.160909), correlation (0.310013)*/, 203 | 3,3, 7,8/*mean (0.177755), correlation (0.309394)*/, 204 | 5,7, 10,-7/*mean (0.212337), correlation (0.310315)*/, 205 | -1,7, 1,-12/*mean (0.214429), correlation (0.311933)*/, 206 | 3,-10, 5,6/*mean (0.235807), correlation (0.313104)*/, 207 | 2,-4, 3,-10/*mean (0.00494827), correlation (0.344948)*/, 208 | -13,0, -13,5/*mean (0.0549145), correlation (0.344675)*/, 209 | -13,-7, -12,12/*mean (0.103385), correlation (0.342715)*/, 210 | -13,3, -11,8/*mean (0.134222), correlation (0.322922)*/, 211 | -7,12, -4,7/*mean (0.153284), correlation (0.337061)*/, 212 | 6,-10, 12,8/*mean (0.154881), correlation (0.329257)*/, 213 | -9,-1, -7,-6/*mean (0.200967), correlation (0.33312)*/, 214 | -2,-5, 0,12/*mean (0.201518), correlation (0.340635)*/, 215 | -12,5, -7,5/*mean (0.207805), correlation (0.335631)*/, 216 | 3,-10, 8,-13/*mean (0.224438), correlation (0.34504)*/, 217 | -7,-7, -4,5/*mean (0.239361), correlation (0.338053)*/, 218 | -3,-2, -1,-7/*mean (0.240744), correlation (0.344322)*/, 219 | 2,9, 5,-11/*mean (0.242949), correlation (0.34145)*/, 220 | -11,-13, -5,-13/*mean (0.244028), correlation (0.336861)*/, 221 | -1,6, 0,-1/*mean (0.247571), correlation (0.343684)*/, 222 | 5,-3, 5,2/*mean (0.000697256), correlation (0.357265)*/, 223 | -4,-13, -4,12/*mean (0.00213675), correlation (0.373827)*/, 224 | -9,-6, -9,6/*mean (0.0126856), correlation (0.373938)*/, 225 | -12,-10, -8,-4/*mean (0.0152497), correlation (0.364237)*/, 226 | 10,2, 12,-3/*mean (0.0299933), correlation (0.345292)*/, 227 | 7,12, 12,12/*mean (0.0307242), correlation (0.366299)*/, 228 | -7,-13, -6,5/*mean (0.0534975), correlation (0.368357)*/, 229 | -4,9, -3,4/*mean (0.099865), correlation (0.372276)*/, 230 | 7,-1, 12,2/*mean (0.117083), correlation (0.364529)*/, 231 | -7,6, -5,1/*mean (0.126125), correlation (0.369606)*/, 232 | -13,11, -12,5/*mean (0.130364), correlation (0.358502)*/, 233 | -3,7, -2,-6/*mean (0.131691), correlation (0.375531)*/, 234 | 7,-8, 12,-7/*mean (0.160166), correlation (0.379508)*/, 235 | -13,-7, -11,-12/*mean (0.167848), correlation (0.353343)*/, 236 | 1,-3, 12,12/*mean (0.183378), correlation (0.371916)*/, 237 | 2,-6, 3,0/*mean (0.228711), correlation (0.371761)*/, 238 | -4,3, -2,-13/*mean (0.247211), correlation (0.364063)*/, 239 | -1,-13, 1,9/*mean (0.249325), correlation (0.378139)*/, 240 | 7,1, 8,-6/*mean (0.000652272), correlation (0.411682)*/, 241 | 1,-1, 3,12/*mean (0.00248538), correlation (0.392988)*/, 242 | 9,1, 12,6/*mean (0.0206815), correlation (0.386106)*/, 243 | -1,-9, -1,3/*mean (0.0364485), correlation (0.410752)*/, 244 | -13,-13, -10,5/*mean (0.0376068), correlation (0.398374)*/, 245 | 7,7, 10,12/*mean (0.0424202), correlation (0.405663)*/, 246 | 12,-5, 12,9/*mean (0.0942645), correlation (0.410422)*/, 247 | 6,3, 7,11/*mean (0.1074), correlation (0.413224)*/, 248 | 5,-13, 6,10/*mean (0.109256), correlation (0.408646)*/, 249 | 2,-12, 2,3/*mean (0.131691), correlation (0.416076)*/, 250 | 3,8, 4,-6/*mean (0.165081), correlation (0.417569)*/, 251 | 2,6, 12,-13/*mean (0.171874), correlation (0.408471)*/, 252 | 9,-12, 10,3/*mean (0.175146), correlation (0.41296)*/, 253 | -8,4, -7,9/*mean (0.183682), correlation (0.402956)*/, 254 | -11,12, -4,-6/*mean (0.184672), correlation (0.416125)*/, 255 | 1,12, 2,-8/*mean (0.191487), correlation (0.386696)*/, 256 | 6,-9, 7,-4/*mean (0.192668), correlation (0.394771)*/, 257 | 2,3, 3,-2/*mean (0.200157), correlation (0.408303)*/, 258 | 6,3, 11,0/*mean (0.204588), correlation (0.411762)*/, 259 | 3,-3, 8,-8/*mean (0.205904), correlation (0.416294)*/, 260 | 7,8, 9,3/*mean (0.213237), correlation (0.409306)*/, 261 | -11,-5, -6,-4/*mean (0.243444), correlation (0.395069)*/, 262 | -10,11, -5,10/*mean (0.247672), correlation (0.413392)*/, 263 | -5,-8, -3,12/*mean (0.24774), correlation (0.411416)*/, 264 | -10,5, -9,0/*mean (0.00213675), correlation (0.454003)*/, 265 | 8,-1, 12,-6/*mean (0.0293635), correlation (0.455368)*/, 266 | 4,-6, 6,-11/*mean (0.0404971), correlation (0.457393)*/, 267 | -10,12, -8,7/*mean (0.0481107), correlation (0.448364)*/, 268 | 4,-2, 6,7/*mean (0.050641), correlation (0.455019)*/, 269 | -2,0, -2,12/*mean (0.0525978), correlation (0.44338)*/, 270 | -5,-8, -5,2/*mean (0.0629667), correlation (0.457096)*/, 271 | 7,-6, 10,12/*mean (0.0653846), correlation (0.445623)*/, 272 | -9,-13, -8,-8/*mean (0.0858749), correlation (0.449789)*/, 273 | -5,-13, -5,-2/*mean (0.122402), correlation (0.450201)*/, 274 | 8,-8, 9,-13/*mean (0.125416), correlation (0.453224)*/, 275 | -9,-11, -9,0/*mean (0.130128), correlation (0.458724)*/, 276 | 1,-8, 1,-2/*mean (0.132467), correlation (0.440133)*/, 277 | 7,-4, 9,1/*mean (0.132692), correlation (0.454)*/, 278 | -2,1, -1,-4/*mean (0.135695), correlation (0.455739)*/, 279 | 11,-6, 12,-11/*mean (0.142904), correlation (0.446114)*/, 280 | -12,-9, -6,4/*mean (0.146165), correlation (0.451473)*/, 281 | 3,7, 7,12/*mean (0.147627), correlation (0.456643)*/, 282 | 5,5, 10,8/*mean (0.152901), correlation (0.455036)*/, 283 | 0,-4, 2,8/*mean (0.167083), correlation (0.459315)*/, 284 | -9,12, -5,-13/*mean (0.173234), correlation (0.454706)*/, 285 | 0,7, 2,12/*mean (0.18312), correlation (0.433855)*/, 286 | -1,2, 1,7/*mean (0.185504), correlation (0.443838)*/, 287 | 5,11, 7,-9/*mean (0.185706), correlation (0.451123)*/, 288 | 3,5, 6,-8/*mean (0.188968), correlation (0.455808)*/, 289 | -13,-4, -8,9/*mean (0.191667), correlation (0.459128)*/, 290 | -5,9, -3,-3/*mean (0.193196), correlation (0.458364)*/, 291 | -4,-7, -3,-12/*mean (0.196536), correlation (0.455782)*/, 292 | 6,5, 8,0/*mean (0.1972), correlation (0.450481)*/, 293 | -7,6, -6,12/*mean (0.199438), correlation (0.458156)*/, 294 | -13,6, -5,-2/*mean (0.211224), correlation (0.449548)*/, 295 | 1,-10, 3,10/*mean (0.211718), correlation (0.440606)*/, 296 | 4,1, 8,-4/*mean (0.213034), correlation (0.443177)*/, 297 | -2,-2, 2,-13/*mean (0.234334), correlation (0.455304)*/, 298 | 2,-12, 12,12/*mean (0.235684), correlation (0.443436)*/, 299 | -2,-13, 0,-6/*mean (0.237674), correlation (0.452525)*/, 300 | 4,1, 9,3/*mean (0.23962), correlation (0.444824)*/, 301 | -6,-10, -3,-5/*mean (0.248459), correlation (0.439621)*/, 302 | -3,-13, -1,1/*mean (0.249505), correlation (0.456666)*/, 303 | 7,5, 12,-11/*mean (0.00119208), correlation (0.495466)*/, 304 | 4,-2, 5,-7/*mean (0.00372245), correlation (0.484214)*/, 305 | -13,9, -9,-5/*mean (0.00741116), correlation (0.499854)*/, 306 | 7,1, 8,6/*mean (0.0208952), correlation (0.499773)*/, 307 | 7,-8, 7,6/*mean (0.0220085), correlation (0.501609)*/, 308 | -7,-4, -7,1/*mean (0.0233806), correlation (0.496568)*/, 309 | -8,11, -7,-8/*mean (0.0236505), correlation (0.489719)*/, 310 | -13,6, -12,-8/*mean (0.0268781), correlation (0.503487)*/, 311 | 2,4, 3,9/*mean (0.0323324), correlation (0.501938)*/, 312 | 10,-5, 12,3/*mean (0.0399235), correlation (0.494029)*/, 313 | -6,-5, -6,7/*mean (0.0420153), correlation (0.486579)*/, 314 | 8,-3, 9,-8/*mean (0.0548021), correlation (0.484237)*/, 315 | 2,-12, 2,8/*mean (0.0616622), correlation (0.496642)*/, 316 | -11,-2, -10,3/*mean (0.0627755), correlation (0.498563)*/, 317 | -12,-13, -7,-9/*mean (0.0829622), correlation (0.495491)*/, 318 | -11,0, -10,-5/*mean (0.0843342), correlation (0.487146)*/, 319 | 5,-3, 11,8/*mean (0.0929937), correlation (0.502315)*/, 320 | -2,-13, -1,12/*mean (0.113327), correlation (0.48941)*/, 321 | -1,-8, 0,9/*mean (0.132119), correlation (0.467268)*/, 322 | -13,-11, -12,-5/*mean (0.136269), correlation (0.498771)*/, 323 | -10,-2, -10,11/*mean (0.142173), correlation (0.498714)*/, 324 | -3,9, -2,-13/*mean (0.144141), correlation (0.491973)*/, 325 | 2,-3, 3,2/*mean (0.14892), correlation (0.500782)*/, 326 | -9,-13, -4,0/*mean (0.150371), correlation (0.498211)*/, 327 | -4,6, -3,-10/*mean (0.152159), correlation (0.495547)*/, 328 | -4,12, -2,-7/*mean (0.156152), correlation (0.496925)*/, 329 | -6,-11, -4,9/*mean (0.15749), correlation (0.499222)*/, 330 | 6,-3, 6,11/*mean (0.159211), correlation (0.503821)*/, 331 | -13,11, -5,5/*mean (0.162427), correlation (0.501907)*/, 332 | 11,11, 12,6/*mean (0.16652), correlation (0.497632)*/, 333 | 7,-5, 12,-2/*mean (0.169141), correlation (0.484474)*/, 334 | -1,12, 0,7/*mean (0.169456), correlation (0.495339)*/, 335 | -4,-8, -3,-2/*mean (0.171457), correlation (0.487251)*/, 336 | -7,1, -6,7/*mean (0.175), correlation (0.500024)*/, 337 | -13,-12, -8,-13/*mean (0.175866), correlation (0.497523)*/, 338 | -7,-2, -6,-8/*mean (0.178273), correlation (0.501854)*/, 339 | -8,5, -6,-9/*mean (0.181107), correlation (0.494888)*/, 340 | -5,-1, -4,5/*mean (0.190227), correlation (0.482557)*/, 341 | -13,7, -8,10/*mean (0.196739), correlation (0.496503)*/, 342 | 1,5, 5,-13/*mean (0.19973), correlation (0.499759)*/, 343 | 1,0, 10,-13/*mean (0.204465), correlation (0.49873)*/, 344 | 9,12, 10,-1/*mean (0.209334), correlation (0.49063)*/, 345 | 5,-8, 10,-9/*mean (0.211134), correlation (0.503011)*/, 346 | -1,11, 1,-13/*mean (0.212), correlation (0.499414)*/, 347 | -9,-3, -6,2/*mean (0.212168), correlation (0.480739)*/, 348 | -1,-10, 1,12/*mean (0.212731), correlation (0.502523)*/, 349 | -13,1, -8,-10/*mean (0.21327), correlation (0.489786)*/, 350 | 8,-11, 10,-6/*mean (0.214159), correlation (0.488246)*/, 351 | 2,-13, 3,-6/*mean (0.216993), correlation (0.50287)*/, 352 | 7,-13, 12,-9/*mean (0.223639), correlation (0.470502)*/, 353 | -10,-10, -5,-7/*mean (0.224089), correlation (0.500852)*/, 354 | -10,-8, -8,-13/*mean (0.228666), correlation (0.502629)*/, 355 | 4,-6, 8,5/*mean (0.22906), correlation (0.498305)*/, 356 | 3,12, 8,-13/*mean (0.233378), correlation (0.503825)*/, 357 | -4,2, -3,-3/*mean (0.234323), correlation (0.476692)*/, 358 | 5,-13, 10,-12/*mean (0.236392), correlation (0.475462)*/, 359 | 4,-13, 5,-1/*mean (0.236842), correlation (0.504132)*/, 360 | -9,9, -4,3/*mean (0.236977), correlation (0.497739)*/, 361 | 0,3, 3,-9/*mean (0.24314), correlation (0.499398)*/, 362 | -12,1, -6,1/*mean (0.243297), correlation (0.489447)*/, 363 | 3,2, 4,-8/*mean (0.00155196), correlation (0.553496)*/, 364 | -10,-10, -10,9/*mean (0.00239541), correlation (0.54297)*/, 365 | 8,-13, 12,12/*mean (0.0034413), correlation (0.544361)*/, 366 | -8,-12, -6,-5/*mean (0.003565), correlation (0.551225)*/, 367 | 2,2, 3,7/*mean (0.00835583), correlation (0.55285)*/, 368 | 10,6, 11,-8/*mean (0.00885065), correlation (0.540913)*/, 369 | 6,8, 8,-12/*mean (0.0101552), correlation (0.551085)*/, 370 | -7,10, -6,5/*mean (0.0102227), correlation (0.533635)*/, 371 | -3,-9, -3,9/*mean (0.0110211), correlation (0.543121)*/, 372 | -1,-13, -1,5/*mean (0.0113473), correlation (0.550173)*/, 373 | -3,-7, -3,4/*mean (0.0140913), correlation (0.554774)*/, 374 | -8,-2, -8,3/*mean (0.017049), correlation (0.55461)*/, 375 | 4,2, 12,12/*mean (0.01778), correlation (0.546921)*/, 376 | 2,-5, 3,11/*mean (0.0224022), correlation (0.549667)*/, 377 | 6,-9, 11,-13/*mean (0.029161), correlation (0.546295)*/, 378 | 3,-1, 7,12/*mean (0.0303081), correlation (0.548599)*/, 379 | 11,-1, 12,4/*mean (0.0355151), correlation (0.523943)*/, 380 | -3,0, -3,6/*mean (0.0417904), correlation (0.543395)*/, 381 | 4,-11, 4,12/*mean (0.0487292), correlation (0.542818)*/, 382 | 2,-4, 2,1/*mean (0.0575124), correlation (0.554888)*/, 383 | -10,-6, -8,1/*mean (0.0594242), correlation (0.544026)*/, 384 | -13,7, -11,1/*mean (0.0597391), correlation (0.550524)*/, 385 | -13,12, -11,-13/*mean (0.0608974), correlation (0.55383)*/, 386 | 6,0, 11,-13/*mean (0.065126), correlation (0.552006)*/, 387 | 0,-1, 1,4/*mean (0.074224), correlation (0.546372)*/, 388 | -13,3, -9,-2/*mean (0.0808592), correlation (0.554875)*/, 389 | -9,8, -6,-3/*mean (0.0883378), correlation (0.551178)*/, 390 | -13,-6, -8,-2/*mean (0.0901035), correlation (0.548446)*/, 391 | 5,-9, 8,10/*mean (0.0949843), correlation (0.554694)*/, 392 | 2,7, 3,-9/*mean (0.0994152), correlation (0.550979)*/, 393 | -1,-6, -1,-1/*mean (0.10045), correlation (0.552714)*/, 394 | 9,5, 11,-2/*mean (0.100686), correlation (0.552594)*/, 395 | 11,-3, 12,-8/*mean (0.101091), correlation (0.532394)*/, 396 | 3,0, 3,5/*mean (0.101147), correlation (0.525576)*/, 397 | -1,4, 0,10/*mean (0.105263), correlation (0.531498)*/, 398 | 3,-6, 4,5/*mean (0.110785), correlation (0.540491)*/, 399 | -13,0, -10,5/*mean (0.112798), correlation (0.536582)*/, 400 | 5,8, 12,11/*mean (0.114181), correlation (0.555793)*/, 401 | 8,9, 9,-6/*mean (0.117431), correlation (0.553763)*/, 402 | 7,-4, 8,-12/*mean (0.118522), correlation (0.553452)*/, 403 | -10,4, -10,9/*mean (0.12094), correlation (0.554785)*/, 404 | 7,3, 12,4/*mean (0.122582), correlation (0.555825)*/, 405 | 9,-7, 10,-2/*mean (0.124978), correlation (0.549846)*/, 406 | 7,0, 12,-2/*mean (0.127002), correlation (0.537452)*/, 407 | -1,-6, 0,-11/*mean (0.127148), correlation (0.547401)*/ 408 | }; 409 | 410 | ORBextractor::ORBextractor(int _nfeatures, float _scaleFactor, int _nlevels, 411 | int _iniThFAST, int _minThFAST): 412 | nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels), 413 | iniThFAST(_iniThFAST), minThFAST(_minThFAST) 414 | { 415 | mvScaleFactor.resize(nlevels); 416 | mvLevelSigma2.resize(nlevels); 417 | mvScaleFactor[0]=1.0f; 418 | mvLevelSigma2[0]=1.0f; 419 | for(int i=1; i= vmin; --v) 464 | { 465 | while (umax[v0] == umax[v0 + 1]) 466 | ++v0; 467 | umax[v] = v0; 468 | ++v0; 469 | } 470 | } 471 | 472 | static void computeOrientation(const Mat& image, vector& keypoints, const vector& umax) 473 | { 474 | for (vector::iterator keypoint = keypoints.begin(), 475 | keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint) 476 | { 477 | keypoint->angle = IC_Angle(image, keypoint->pt, umax); 478 | } 479 | } 480 | 481 | void ExtractorNode::DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4) 482 | { 483 | const int halfX = ceil(static_cast(UR.x-UL.x)/2); 484 | const int halfY = ceil(static_cast(BR.y-UL.y)/2); 485 | 486 | //Define boundaries of childs 487 | n1.UL = UL; 488 | n1.UR = cv::Point2i(UL.x+halfX,UL.y); 489 | n1.BL = cv::Point2i(UL.x,UL.y+halfY); 490 | n1.BR = cv::Point2i(UL.x+halfX,UL.y+halfY); 491 | n1.vKeys.reserve(vKeys.size()); 492 | 493 | n2.UL = n1.UR; 494 | n2.UR = UR; 495 | n2.BL = n1.BR; 496 | n2.BR = cv::Point2i(UR.x,UL.y+halfY); 497 | n2.vKeys.reserve(vKeys.size()); 498 | 499 | n3.UL = n1.BL; 500 | n3.UR = n1.BR; 501 | n3.BL = BL; 502 | n3.BR = cv::Point2i(n1.BR.x,BL.y); 503 | n3.vKeys.reserve(vKeys.size()); 504 | 505 | n4.UL = n3.UR; 506 | n4.UR = n2.BR; 507 | n4.BL = n3.BR; 508 | n4.BR = BR; 509 | n4.vKeys.reserve(vKeys.size()); 510 | 511 | //Associate points to childs 512 | for(size_t i=0;i ORBextractor::DistributeOctTree(const vector& vToDistributeKeys, const int &minX, 540 | const int &maxX, const int &minY, const int &maxY, const int &N, const int &level) 541 | { 542 | // Compute how many initial nodes 543 | const int nIni = round(static_cast(maxX-minX)/(maxY-minY)); 544 | 545 | const float hX = static_cast(maxX-minX)/nIni; 546 | 547 | list lNodes; 548 | 549 | vector vpIniNodes; 550 | vpIniNodes.resize(nIni); 551 | 552 | for(int i=0; i(i),0); 556 | ni.UR = cv::Point2i(hX*static_cast(i+1),0); 557 | ni.BL = cv::Point2i(ni.UL.x,maxY-minY); 558 | ni.BR = cv::Point2i(ni.UR.x,maxY-minY); 559 | ni.vKeys.reserve(vToDistributeKeys.size()); 560 | 561 | lNodes.push_back(ni); 562 | vpIniNodes[i] = &lNodes.back(); 563 | } 564 | 565 | //Associate points to childs 566 | for(size_t i=0;ivKeys.push_back(kp); 570 | } 571 | 572 | list::iterator lit = lNodes.begin(); 573 | 574 | while(lit!=lNodes.end()) 575 | { 576 | if(lit->vKeys.size()==1) 577 | { 578 | lit->bNoMore=true; 579 | lit++; 580 | } 581 | else if(lit->vKeys.empty()) 582 | lit = lNodes.erase(lit); 583 | else 584 | lit++; 585 | } 586 | 587 | bool bFinish = false; 588 | 589 | int iteration = 0; 590 | 591 | vector > vSizeAndPointerToNode; 592 | vSizeAndPointerToNode.reserve(lNodes.size()*4); 593 | 594 | while(!bFinish) 595 | { 596 | iteration++; 597 | 598 | int prevSize = lNodes.size(); 599 | 600 | lit = lNodes.begin(); 601 | 602 | int nToExpand = 0; 603 | 604 | vSizeAndPointerToNode.clear(); 605 | 606 | while(lit!=lNodes.end()) 607 | { 608 | if(lit->bNoMore) 609 | { 610 | // If node only contains one point do not subdivide and continue 611 | lit++; 612 | continue; 613 | } 614 | else 615 | { 616 | // If more than one point, subdivide 617 | ExtractorNode n1,n2,n3,n4; 618 | lit->DivideNode(n1,n2,n3,n4); 619 | 620 | // Add childs if they contain points 621 | if(n1.vKeys.size()>0) 622 | { 623 | lNodes.push_front(n1); 624 | if(n1.vKeys.size()>1) 625 | { 626 | nToExpand++; 627 | vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front())); 628 | lNodes.front().lit = lNodes.begin(); 629 | } 630 | } 631 | if(n2.vKeys.size()>0) 632 | { 633 | lNodes.push_front(n2); 634 | if(n2.vKeys.size()>1) 635 | { 636 | nToExpand++; 637 | vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front())); 638 | lNodes.front().lit = lNodes.begin(); 639 | } 640 | } 641 | if(n3.vKeys.size()>0) 642 | { 643 | lNodes.push_front(n3); 644 | if(n3.vKeys.size()>1) 645 | { 646 | nToExpand++; 647 | vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front())); 648 | lNodes.front().lit = lNodes.begin(); 649 | } 650 | } 651 | if(n4.vKeys.size()>0) 652 | { 653 | lNodes.push_front(n4); 654 | if(n4.vKeys.size()>1) 655 | { 656 | nToExpand++; 657 | vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front())); 658 | lNodes.front().lit = lNodes.begin(); 659 | } 660 | } 661 | 662 | lit=lNodes.erase(lit); 663 | continue; 664 | } 665 | } 666 | 667 | // Finish if there are more nodes than required features 668 | // or all nodes contain just one point 669 | if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize) 670 | { 671 | bFinish = true; 672 | } 673 | else if(((int)lNodes.size()+nToExpand*3)>N) 674 | { 675 | 676 | while(!bFinish) 677 | { 678 | 679 | prevSize = lNodes.size(); 680 | 681 | vector > vPrevSizeAndPointerToNode = vSizeAndPointerToNode; 682 | vSizeAndPointerToNode.clear(); 683 | 684 | sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end()); 685 | for(int j=vPrevSizeAndPointerToNode.size()-1;j>=0;j--) 686 | { 687 | ExtractorNode n1,n2,n3,n4; 688 | vPrevSizeAndPointerToNode[j].second->DivideNode(n1,n2,n3,n4); 689 | 690 | // Add childs if they contain points 691 | if(n1.vKeys.size()>0) 692 | { 693 | lNodes.push_front(n1); 694 | if(n1.vKeys.size()>1) 695 | { 696 | vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front())); 697 | lNodes.front().lit = lNodes.begin(); 698 | } 699 | } 700 | if(n2.vKeys.size()>0) 701 | { 702 | lNodes.push_front(n2); 703 | if(n2.vKeys.size()>1) 704 | { 705 | vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front())); 706 | lNodes.front().lit = lNodes.begin(); 707 | } 708 | } 709 | if(n3.vKeys.size()>0) 710 | { 711 | lNodes.push_front(n3); 712 | if(n3.vKeys.size()>1) 713 | { 714 | vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front())); 715 | lNodes.front().lit = lNodes.begin(); 716 | } 717 | } 718 | if(n4.vKeys.size()>0) 719 | { 720 | lNodes.push_front(n4); 721 | if(n4.vKeys.size()>1) 722 | { 723 | vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front())); 724 | lNodes.front().lit = lNodes.begin(); 725 | } 726 | } 727 | 728 | lNodes.erase(vPrevSizeAndPointerToNode[j].second->lit); 729 | 730 | if((int)lNodes.size()>=N) 731 | break; 732 | } 733 | 734 | if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize) 735 | bFinish = true; 736 | 737 | } 738 | } 739 | } 740 | 741 | // Retain the best point in each node 742 | vector vResultKeys; 743 | vResultKeys.reserve(nfeatures); 744 | for(list::iterator lit=lNodes.begin(); lit!=lNodes.end(); lit++) 745 | { 746 | vector &vNodeKeys = lit->vKeys; 747 | cv::KeyPoint* pKP = &vNodeKeys[0]; 748 | float maxResponse = pKP->response; 749 | 750 | for(size_t k=1;kmaxResponse) 753 | { 754 | pKP = &vNodeKeys[k]; 755 | maxResponse = vNodeKeys[k].response; 756 | } 757 | } 758 | 759 | vResultKeys.push_back(*pKP); 760 | } 761 | 762 | return vResultKeys; 763 | } 764 | 765 | void ORBextractor::ComputeKeyPointsOctTree(vector >& allKeypoints) 766 | { 767 | allKeypoints.resize(nlevels); 768 | 769 | const float W = 30; 770 | 771 | for (int level = 0; level < nlevels; ++level) 772 | { 773 | const int minBorderX = EDGE_THRESHOLD-3; 774 | const int minBorderY = minBorderX; 775 | const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3; 776 | const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3; 777 | 778 | vector vToDistributeKeys; 779 | vToDistributeKeys.reserve(nfeatures*10); 780 | 781 | const float width = (maxBorderX-minBorderX); 782 | const float height = (maxBorderY-minBorderY); 783 | 784 | const int nCols = width/W; 785 | const int nRows = height/W; 786 | const int wCell = ceil(width/nCols); 787 | const int hCell = ceil(height/nRows); 788 | 789 | for(int i=0; i=maxBorderY-3) 795 | continue; 796 | if(maxY>maxBorderY) 797 | maxY = maxBorderY; 798 | 799 | for(int j=0; j=maxBorderX-6) 804 | continue; 805 | if(maxX>maxBorderX) 806 | maxX = maxBorderX; 807 | 808 | vector vKeysCell; 809 | FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), 810 | vKeysCell,iniThFAST,true); 811 | 812 | if(vKeysCell.empty()) 813 | { 814 | FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), 815 | vKeysCell,minThFAST,true); 816 | } 817 | 818 | if(!vKeysCell.empty()) 819 | { 820 | for(vector::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++) 821 | { 822 | (*vit).pt.x+=j*wCell; 823 | (*vit).pt.y+=i*hCell; 824 | vToDistributeKeys.push_back(*vit); 825 | } 826 | } 827 | 828 | } 829 | } 830 | 831 | vector & keypoints = allKeypoints[level]; 832 | keypoints.reserve(nfeatures); 833 | 834 | keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX, 835 | minBorderY, maxBorderY,mnFeaturesPerLevel[level], level); 836 | 837 | const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level]; 838 | 839 | // Add border to coordinates and scale information 840 | const int nkps = keypoints.size(); 841 | for(int i=0; i > &allKeypoints) 856 | { 857 | allKeypoints.resize(nlevels); 858 | 859 | float imageRatio = (float)mvImagePyramid[0].cols/mvImagePyramid[0].rows; 860 | 861 | for (int level = 0; level < nlevels; ++level) 862 | { 863 | const int nDesiredFeatures = mnFeaturesPerLevel[level]; 864 | 865 | const int levelCols = sqrt((float)nDesiredFeatures/(5*imageRatio)); 866 | const int levelRows = imageRatio*levelCols; 867 | 868 | const int minBorderX = EDGE_THRESHOLD; 869 | const int minBorderY = minBorderX; 870 | const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD; 871 | const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD; 872 | 873 | const int W = maxBorderX - minBorderX; 874 | const int H = maxBorderY - minBorderY; 875 | const int cellW = ceil((float)W/levelCols); 876 | const int cellH = ceil((float)H/levelRows); 877 | 878 | const int nCells = levelRows*levelCols; 879 | const int nfeaturesCell = ceil((float)nDesiredFeatures/nCells); 880 | 881 | vector > > cellKeyPoints(levelRows, vector >(levelCols)); 882 | 883 | vector > nToRetain(levelRows,vector(levelCols,0)); 884 | vector > nTotal(levelRows,vector(levelCols,0)); 885 | vector > bNoMore(levelRows,vector(levelCols,false)); 886 | vector iniXCol(levelCols); 887 | vector iniYRow(levelRows); 888 | int nNoMore = 0; 889 | int nToDistribute = 0; 890 | 891 | 892 | float hY = cellH + 6; 893 | 894 | for(int i=0; infeaturesCell) 949 | { 950 | nToRetain[i][j] = nfeaturesCell; 951 | bNoMore[i][j] = false; 952 | } 953 | else 954 | { 955 | nToRetain[i][j] = nKeys; 956 | nToDistribute += nfeaturesCell-nKeys; 957 | bNoMore[i][j] = true; 958 | nNoMore++; 959 | } 960 | 961 | } 962 | } 963 | 964 | 965 | // Retain by score 966 | 967 | while(nToDistribute>0 && nNoMorenNewFeaturesCell) 979 | { 980 | nToRetain[i][j] = nNewFeaturesCell; 981 | bNoMore[i][j] = false; 982 | } 983 | else 984 | { 985 | nToRetain[i][j] = nTotal[i][j]; 986 | nToDistribute += nNewFeaturesCell-nTotal[i][j]; 987 | bNoMore[i][j] = true; 988 | nNoMore++; 989 | } 990 | } 991 | } 992 | } 993 | } 994 | 995 | vector & keypoints = allKeypoints[level]; 996 | keypoints.reserve(nDesiredFeatures*2); 997 | 998 | const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level]; 999 | 1000 | // Retain by score and transform coordinates 1001 | for(int i=0; i &keysCell = cellKeyPoints[i][j]; 1006 | KeyPointsFilter::retainBest(keysCell,nToRetain[i][j]); 1007 | if((int)keysCell.size()>nToRetain[i][j]) 1008 | keysCell.resize(nToRetain[i][j]); 1009 | 1010 | 1011 | for(size_t k=0, kend=keysCell.size(); knDesiredFeatures) 1023 | { 1024 | KeyPointsFilter::retainBest(keypoints,nDesiredFeatures); 1025 | keypoints.resize(nDesiredFeatures); 1026 | } 1027 | } 1028 | 1029 | // and compute orientations 1030 | for (int level = 0; level < nlevels; ++level) 1031 | computeOrientation(mvImagePyramid[level], allKeypoints[level], umax); 1032 | } 1033 | 1034 | static void computeDescriptors(const Mat& image, vector& keypoints, Mat& descriptors, 1035 | const vector& pattern) 1036 | { 1037 | descriptors = Mat::zeros((int)keypoints.size(), 32, CV_8UC1); 1038 | 1039 | for (size_t i = 0; i < keypoints.size(); i++) 1040 | computeOrbDescriptor(keypoints[i], image, &pattern[0], descriptors.ptr((int)i)); 1041 | } 1042 | 1043 | void ORBextractor::operator()( InputArray _image, InputArray _mask, vector& _keypoints, 1044 | OutputArray _descriptors) 1045 | { 1046 | if(_image.empty()) 1047 | return; 1048 | 1049 | Mat image = _image.getMat(); 1050 | assert(image.type() == CV_8UC1 ); 1051 | 1052 | // Pre-compute the scale pyramid 1053 | ComputePyramid(image); 1054 | 1055 | vector < vector > allKeypoints; 1056 | ComputeKeyPointsOctTree(allKeypoints); 1057 | //ComputeKeyPointsOld(allKeypoints); 1058 | 1059 | Mat descriptors; 1060 | 1061 | int nkeypoints = 0; 1062 | for (int level = 0; level < nlevels; ++level) 1063 | nkeypoints += (int)allKeypoints[level].size(); 1064 | if( nkeypoints == 0 ) 1065 | _descriptors.release(); 1066 | else 1067 | { 1068 | _descriptors.create(nkeypoints, 32, CV_8U); 1069 | descriptors = _descriptors.getMat(); 1070 | } 1071 | 1072 | _keypoints.clear(); 1073 | _keypoints.reserve(nkeypoints); 1074 | 1075 | int offset = 0; 1076 | for (int level = 0; level < nlevels; ++level) 1077 | { 1078 | vector& keypoints = allKeypoints[level]; 1079 | int nkeypointsLevel = (int)keypoints.size(); 1080 | 1081 | if(nkeypointsLevel==0) 1082 | continue; 1083 | 1084 | // preprocess the resized image 1085 | Mat workingMat = mvImagePyramid[level].clone(); 1086 | GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101); 1087 | 1088 | // Compute the descriptors 1089 | Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel); 1090 | computeDescriptors(workingMat, keypoints, desc, pattern); 1091 | 1092 | offset += nkeypointsLevel; 1093 | 1094 | // Scale keypoint coordinates 1095 | if (level != 0) 1096 | { 1097 | float scale = mvScaleFactor[level]; //getScale(level, firstLevel, scaleFactor); 1098 | for (vector::iterator keypoint = keypoints.begin(), 1099 | keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint) 1100 | keypoint->pt *= scale; 1101 | } 1102 | // And add the keypoints to the output 1103 | _keypoints.insert(_keypoints.end(), keypoints.begin(), keypoints.end()); 1104 | } 1105 | } 1106 | 1107 | void ORBextractor::ComputePyramid(cv::Mat image) 1108 | { 1109 | for (int level = 0; level < nlevels; ++level) 1110 | { 1111 | float scale = mvInvScaleFactor[level]; 1112 | Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale)); 1113 | Size wholeSize(sz.width + EDGE_THRESHOLD*2, sz.height + EDGE_THRESHOLD*2); 1114 | Mat temp(wholeSize, image.type()), masktemp; 1115 | mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height)); 1116 | 1117 | // Compute the resized image 1118 | if( level != 0 ) 1119 | { 1120 | resize(mvImagePyramid[level-1], mvImagePyramid[level], sz, 0, 0, INTER_LINEAR); 1121 | 1122 | copyMakeBorder(mvImagePyramid[level], temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, 1123 | BORDER_REFLECT_101+BORDER_ISOLATED); 1124 | } 1125 | else 1126 | { 1127 | copyMakeBorder(image, temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, 1128 | BORDER_REFLECT_101); 1129 | } 1130 | } 1131 | 1132 | } 1133 | 1134 | } //namespace ORB_SLAM 1135 | --------------------------------------------------------------------------------