├── utils ├── orb_track.gif ├── sp_track_origin_size.gif ├── sp_track_resized_1_3rd.gif └── pytorch_model_convert.py ├── package.xml ├── LICENSE ├── .gitignore ├── README.md ├── include ├── FeatureTrackerBase.h ├── FeatureDetectorBase.h ├── ORBFeatureMatcher.h ├── SuperPointFeatureMatcher.h ├── ORBFeatureDetector.h ├── SuperPointFeatureTracker.h ├── SuperPointFeatureDetector.h ├── FeatureDescriptor.h └── ORBFeatureTracker.h ├── CMakeLists.txt └── src ├── ORBFeatureMatcher.cc ├── main_track.cc ├── SuperPointFeatureMatcher.cc ├── SuperPointFeatureTracker.cc ├── ORBFeatureTracker.cc ├── ORBFeatureDetector.cc └── SuperPointFeatureDetector.cc /utils/orb_track.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/b51/FeatureTracker/HEAD/utils/orb_track.gif -------------------------------------------------------------------------------- /utils/sp_track_origin_size.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/b51/FeatureTracker/HEAD/utils/sp_track_origin_size.gif -------------------------------------------------------------------------------- /utils/sp_track_resized_1_3rd.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/b51/FeatureTracker/HEAD/utils/sp_track_resized_1_3rd.gif -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | FeatureTracker 4 | 0.0.0 5 | The FeatureTracker package 6 | 7 | b51 8 | 9 | MIT 10 | 11 | catkin 12 | 13 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 b51, mail: b51live@gmail.com 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | ####################### MacOS gitignore ###################### 2 | # General 3 | .DS_Store 4 | .AppleDouble 5 | .LSOverride 6 | 7 | # Icon must end with two \r 8 | Icon 9 | 10 | # Thumbnails 11 | ._* 12 | 13 | # Files that might appear in the root of a volume 14 | .DocumentRevisions-V100 15 | .fseventsd 16 | .Spotlight-V100 17 | .TemporaryItems 18 | .Trashes 19 | .VolumeIcon.icns 20 | .com.apple.timemachine.donotpresent 21 | 22 | # Directories potentially created on remote AFP share 23 | .AppleDB 24 | .AppleDesktop 25 | Network Trash Folder 26 | Temporary Items 27 | .apdisk 28 | 29 | ####################### C++ gitignore ######################## 30 | # Prerequisites 31 | *.d 32 | 33 | # Compiled Object files 34 | *.slo 35 | *.lo 36 | *.o 37 | *.obj 38 | 39 | # Precompiled Headers 40 | *.gch 41 | *.pch 42 | 43 | # Compiled Dynamic libraries 44 | *.so 45 | *.dylib 46 | *.dll 47 | 48 | # Fortran module files 49 | *.mod 50 | *.smod 51 | 52 | # Compiled Static libraries 53 | *.lai 54 | *.la 55 | *.a 56 | *.lib 57 | 58 | # Executables 59 | *.exe 60 | *.out 61 | *.app 62 | 63 | ####################### Python gitignore ##################### 64 | # Byte-compiled / optimized / DLL files 65 | __pycache__/ 66 | *.py[cod] 67 | *$py.class 68 | 69 | # C extensions 70 | #*.so 71 | 72 | # Distribution / packaging 73 | .Python 74 | build/ 75 | develop-eggs/ 76 | eggs/ 77 | .eggs/ 78 | lib/ 79 | lib64/ 80 | *.egg-info/ 81 | .installed.cfg 82 | *.egg 83 | 84 | *.swp 85 | *.pt 86 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## FeatureTracker 2 | 3 | ### What this repository for 4 | Detect, match and track different type features 5 | 6 | **Current type of feture points** 7 | - [X] ORB feature points 8 | - [X] SuperPoint feature points 9 | 10 | ### Usage 11 | **PreInstall** 12 | 13 | install libpytorch: [Install from source with Anaconda](https://pytorch.org/get-started/locally/) 14 | Set libpytorch cmake path at CMakeLists.txt 15 | 16 | **Dependence Info** 17 | 18 | pytorch: 1.1.0 19 | 20 | cuda/cudatoolkit: 9.0 21 | 22 | **Build** 23 | ```bash 24 | $ cd FeatureTracker 25 | $ mkdir build 26 | $ cd build && cmake .. 27 | $ make -j4 28 | ``` 29 | 30 | **Run** 31 | ***ORB feature points*** 32 | ```bash 33 | $ ./FeatureTracker --image_dir /path/to/images_dir --type orb [--image_suffix .jpg] 34 | ``` 35 | ***SuperPoint feature points*** 36 | ```bash 37 | Download superpoint model 38 | $ cd utils 39 | $ wget https://github.com/MagicLeapResearch/SuperPointPretrainedNetwork/raw/master/superpoint_v1.pth 40 | $ python pytorch_model_convert.py superpoint_v1.pth 41 | $ cd ../build 42 | $ ./FeatureTracker --image_dir /path/to/image_dir --model_path ../utils/super_point_scripted_model.pt --type sp [--image_suffix .jpg --H 480 --W 640] 43 | ``` 44 | 45 | Kitti: 46 | 47 | **ORB Tracker** 48 | 49 |

50 | 51 | 52 | **SuperPoint Tracker** 53 | 54 | Resized image to 414, 125 (width/3, height/3) 55 |

56 | 57 | Original size image 58 |

59 | -------------------------------------------------------------------------------- /include/FeatureTrackerBase.h: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: FeatureTrackerBase.h 6 | * 7 | * Created On: Sat 12 Oct 2019 08:23:14 AM UTC 8 | * Licensed under The MIT License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | 12 | #ifndef FEATURE_TRACKER_FEATURE_TRACKER_BASE_H_ 13 | #define FEATURE_TRACKER_FEATURE_TRACKER_BASE_H_ 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | class FeatureTrackerBase { 20 | public: 21 | FeatureTrackerBase(){}; 22 | 23 | virtual ~FeatureTrackerBase(){}; 24 | 25 | virtual void Init(int width, int height, int max_number_of_features) = 0; 26 | 27 | virtual void Track(const cv::Mat& image, 28 | Eigen::Matrix2Xf* current_measurements, 29 | Eigen::Matrix2Xf* previous_measurements, 30 | std::vector* feature_ids) = 0; 31 | 32 | virtual bool IsInitialized() const = 0; 33 | 34 | inline virtual int GetWidth() const = 0; 35 | 36 | inline virtual int GetHeight() const = 0; 37 | 38 | virtual void GetFeatureLocations( 39 | std::vector* points) const = 0; 40 | 41 | virtual const std::vector& GetNewTrackIds() const = 0; 42 | 43 | virtual int GetNumberOfFeaturesMatched() const = 0; 44 | 45 | virtual void Display() = 0; 46 | 47 | private: 48 | FeatureTrackerBase(const FeatureTrackerBase& ft) = delete; 49 | FeatureTrackerBase& operator=(const FeatureTrackerBase& ft) = delete; 50 | }; 51 | #endif 52 | -------------------------------------------------------------------------------- /include/FeatureDetectorBase.h: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: FeatureDetectorBase.h 6 | * 7 | * Created On: Thu 27 Jun 2019 03:29:38 PM CST 8 | * Licensed under The MIT License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | 12 | #ifndef FEATURE_TRACKER_FEATURE_DETECTOR_BASE_H_ 13 | #define FEATURE_TRACKER_FEATURE_DETECTOR_BASE_H_ 14 | 15 | #include 16 | #include 17 | 18 | #include "FeatureDescriptor.h" 19 | 20 | class FeatureDetectorBase { 21 | public: 22 | FeatureDetectorBase(){}; 23 | virtual ~FeatureDetectorBase(){}; 24 | 25 | virtual void Init(int width, int height, int max_number_of_features) = 0; 26 | 27 | virtual void Detect(const cv::Mat& image, 28 | Eigen::Matrix2Xd* current_measurements, 29 | std::vector* current_feature_orientations, 30 | std::vector* current_feature_scales, 31 | FeatureDescriptoru* current_feature_descriptors) {} 32 | 33 | virtual void Detect(const cv::Mat& image, 34 | Eigen::Matrix2Xf* current_measurements, 35 | FeatureDescriptorf* current_feature_descriptors) {} 36 | 37 | virtual bool IsInitialized() const = 0; 38 | virtual int GetHeight() const = 0; 39 | virtual int GetWidth() const = 0; 40 | virtual int GetMaxNumberOfFeatures() const = 0; 41 | 42 | private: 43 | FeatureDetectorBase(const FeatureDetectorBase& fd) = delete; 44 | FeatureDetectorBase& operator=(const FeatureDetectorBase& fd) = delete; 45 | }; 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /include/ORBFeatureMatcher.h: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: ORBFeatureMatcher.h 6 | * 7 | * Created On: Thu 26 Sep 2019 03:29:02 PM CST 8 | * Licensed under The MIT License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | 12 | #ifndef FEATURE_TRACKER_ORB_FEATURE_MATCHER_H_ 13 | #define FEATURE_TRACKER_ORB_FEATURE_MATCHER_H_ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | class ORBFeatureMatcher { 23 | public: 24 | typedef cv::NormTypes DistType; 25 | 26 | ORBFeatureMatcher(); 27 | 28 | ~ORBFeatureMatcher() = default; 29 | 30 | void Init(); 31 | 32 | void Init(DistType dist_type, float refine_dist); 33 | 34 | bool IsInitialized() const { return is_initialized_; } 35 | 36 | ORBFeatureMatcher(const ORBFeatureMatcher& d); 37 | 38 | ORBFeatureMatcher& operator=(const ORBFeatureMatcher& d); 39 | 40 | int Match(const cv::Mat& descriptors_img1, const cv::Mat& descriptors_img2, 41 | std::vector* output_matches); 42 | 43 | inline DistType GetDistType() const { return dist_type_; } 44 | 45 | inline float GetRefineDist() const { return refine_dist_; } 46 | 47 | void SetDistType(DistType dist_type); 48 | 49 | inline void SetRefineDist(float refine_dist) { refine_dist_ = refine_dist; } 50 | 51 | std::vector& GetMatches() { return matches_; } 52 | 53 | std::vector& GetRefinedMatches() { return refined_matches_; } 54 | 55 | private: 56 | cv::Ptr matcher_; 57 | DistType dist_type_; 58 | float refine_dist_; 59 | bool is_initialized_; 60 | std::vector matches_; 61 | std::vector refined_matches_; 62 | }; 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(FeatureTracker) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_definitions(-std=c++11) 6 | 7 | ######################################################################### 8 | if (NOT CMAKE_BUILD_TYPE) 9 | set(CMAKE_BUILD_TYPE "Release") 10 | message(STATUS "No build type selected, default to ${CMAKE_BUILD_TYPE}") 11 | endif() 12 | 13 | if(CMAKE_BUILD_TYPE STREQUAL "Release") 14 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -fPIC -march=native") 15 | else(CMAKE_BUILD_TYPE STREQUAL "Debug") 16 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -fPIC") 17 | endif(CMAKE_BUILD_TYPE STREQUAL "Release") 18 | ######################################################################### 19 | 20 | #find_package(catkin REQUIRED) 21 | find_package(OpenCV REQUIRED) 22 | find_package(Eigen3 REQUIRED) 23 | find_package(OpenMP REQUIRED) 24 | 25 | find_package(PythonLibs 3.7 REQUIRED) 26 | execute_process(COMMAND 27 | python -c "from distutils.sysconfig import get_python_lib; print(get_python_lib())" 28 | OUTPUT_VARIABLE 29 | PYTHON_SITE_PACKAGES 30 | OUTPUT_STRIP_TRAILING_WHITESPACE 31 | ) 32 | list(APPEND CMAKE_PREFIX_PATH ${PYTHON_SITE_PACKAGES}/torch/share/cmake) 33 | #list(APPEND CMAKE_PREFIX_PATH "~/.local/lib/python3.6/site-packages/torch/share/cmake") 34 | find_package(Torch REQUIRED) 35 | 36 | #catkin_package( 37 | # INCLUDE_DIRS include 38 | # #CATKIN_DEPENDS 39 | #) 40 | 41 | include_directories( 42 | include 43 | ${OPENCV_INCLUDE_DIRS} 44 | ${EIGEN3_INCLUDE_DIRS} 45 | ) 46 | 47 | ## Declare a C++ executable 48 | add_executable(${PROJECT_NAME} 49 | src/main_track.cc 50 | src/ORBFeatureDetector.cc 51 | src/ORBFeatureMatcher.cc 52 | src/ORBFeatureTracker.cc 53 | src/SuperPointFeatureDetector.cc 54 | src/SuperPointFeatureMatcher.cc 55 | src/SuperPointFeatureTracker.cc 56 | ) 57 | 58 | ## Specify libraries to link a library or executable target against 59 | target_link_libraries(${PROJECT_NAME} PUBLIC 60 | OpenMP::OpenMP_CXX 61 | ${TORCH_LIBRARIES} 62 | ${OpenCV_LIBS} 63 | glog 64 | gflags 65 | ) 66 | -------------------------------------------------------------------------------- /include/SuperPointFeatureMatcher.h: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: SuperPointFeatureMatcher.h 6 | * 7 | * Created On: Thu 10 Oct 2019 07:48:30 AM UTC 8 | * Licensed under The MIT License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | 12 | #ifndef FEATURE_TRACKER_SUPER_POINT_FEATURE_MATCHER_H_ 13 | #define FEATURE_TRACKER_SUPER_POINT_FEATURE_MATCHER_H_ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include "FeatureDescriptor.h" 23 | 24 | class SuperPointFeatureMatcher { 25 | public: 26 | /* Feature matched indices. */ 27 | struct FeatureMatch { 28 | int index1; 29 | int index2; 30 | float distance; 31 | FeatureMatch(int idx1, int idx2, float dist) 32 | : index1(idx1), index2(idx2), distance(dist){}; 33 | FeatureMatch() 34 | : index1(-1), index2(-1), distance(std::numeric_limits::max()){}; 35 | }; 36 | 37 | SuperPointFeatureMatcher(); 38 | 39 | ~SuperPointFeatureMatcher() = default; 40 | 41 | void Init(); 42 | 43 | void Init(float nn_thresh); 44 | 45 | bool IsInitialized() const { return is_initialized_; } 46 | 47 | SuperPointFeatureMatcher(const SuperPointFeatureMatcher& d); 48 | 49 | SuperPointFeatureMatcher& operator=(const SuperPointFeatureMatcher& d); 50 | 51 | int Match(const FeatureDescriptorf& descriptors_img1, 52 | const FeatureDescriptorf& descriptors_img2, 53 | Eigen::Matrix3Xf* output_matches); 54 | 55 | inline float GetRefineDist() const { return refine_dist_; } 56 | 57 | std::vector& GetMatches() { return matches_; } 58 | 59 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 60 | 61 | private: 62 | int Match(const FeatureDescriptorf& descriptors_img1, 63 | const FeatureDescriptorf& descriptors_img2); 64 | float refine_dist_; 65 | bool is_initialized_; 66 | std::vector matches_; 67 | }; 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /include/ORBFeatureDetector.h: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: ORBFeatureDetector.h 6 | * 7 | * Created On: Wed 25 Sep 2019 04:26:40 PM CST 8 | * Licensed under The MIT License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | 12 | #ifndef FEATURE_TRACKER_ORB_FEATURE_DETECTOR_H_ 13 | #define FEATURE_TRACKER_ORB_FEATURE_DETECTOR_H_ 14 | 15 | #include "FeatureDetectorBase.h" 16 | 17 | #include 18 | #include 19 | 20 | class ORBFeatureDetector : public FeatureDetectorBase { 21 | public: 22 | ORBFeatureDetector(); 23 | ~ORBFeatureDetector() = default; 24 | 25 | virtual void Init(int width, int height, int max_number_of_features); 26 | 27 | void Init(int width, int height, int max_number_of_features, 28 | float scale_factor, int number_of_levels, int edge_dist, 29 | int patch_size, bool refine_corners); 30 | 31 | virtual void Detect(const cv::Mat& image, 32 | Eigen::Matrix2Xf* current_measurements, 33 | std::vector* current_feature_orientations, 34 | std::vector* current_feature_scales, 35 | FeatureDescriptoru* current_feature_descriptors); 36 | 37 | int Detect(const cv::Mat& image, std::vector* feature_locations, 38 | cv::Mat* descriptors); 39 | 40 | inline virtual bool IsInitialized() const { return is_initialized_; } 41 | 42 | inline virtual int GetWidth() const { return width_; } 43 | 44 | inline virtual int GetHeight() const { return height_; } 45 | 46 | inline virtual int GetMaxNumberOfFeatures() const { 47 | return max_number_of_features_; 48 | } 49 | 50 | inline float GetScaleFactor() const { return scale_factor_; } 51 | 52 | inline int GetNumberOfLevels() const { return number_of_levels_; } 53 | 54 | inline int GetEdgeDist() const { return edge_dist_; } 55 | 56 | inline int GetPatchSize() const { return patch_size_; } 57 | 58 | inline bool GetRefineCorners() const { return refine_corners_; } 59 | 60 | void SetMask(const cv::Mat& mask); 61 | 62 | void SetNumberOfLevels(int number_of_levels); 63 | 64 | const cv::Mat& GetCvImage() const { return image_; } 65 | 66 | ORBFeatureDetector(const ORBFeatureDetector& d); 67 | 68 | ORBFeatureDetector& operator=(const ORBFeatureDetector& d); 69 | 70 | void Refine(const cv::Mat& image, 71 | std::vector* feature_locations); 72 | 73 | private: 74 | int width_; 75 | int height_; 76 | 77 | int max_number_of_features_; 78 | 79 | float scale_factor_; 80 | int number_of_levels_; 81 | int edge_dist_; 82 | int patch_size_; 83 | bool refine_corners_; 84 | float downsample_scale_; 85 | bool is_initialized_; 86 | 87 | cv::Mat image_; 88 | cv::Mat mask_; 89 | 90 | cv::Ptr detector_; 91 | }; 92 | 93 | #endif 94 | -------------------------------------------------------------------------------- /include/SuperPointFeatureTracker.h: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: SuperPointFeatureTracker.h 6 | * 7 | * Created On: Fri 11 Oct 2019 07:52:55 AM UTC 8 | * Licensed under The MIT License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | 12 | #ifndef FEATURE_TRACKER_SUPER_POINT_FEATURE_TRACKER_H_ 13 | #define FEATURE_TRACKER_SUPER_POINT_FEATURE_TRACKER_H_ 14 | 15 | #include "FeatureTrackerBase.h" 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "FeatureDescriptor.h" 24 | #include "FeatureDetectorBase.h" 25 | #include "SuperPointFeatureDetector.h" 26 | #include "SuperPointFeatureMatcher.h" 27 | 28 | class SuperPointFeatureTracker : public FeatureTrackerBase { 29 | public: 30 | SuperPointFeatureTracker(); 31 | 32 | ~SuperPointFeatureTracker() = default; 33 | 34 | virtual void Init(int width, int height, int max_number_of_features); 35 | 36 | virtual void Track(const cv::Mat& image, 37 | Eigen::Matrix2Xf* current_measurements, 38 | Eigen::Matrix2Xf* previous_measurements, 39 | std::vector* feature_ids); 40 | 41 | void Init(int width, int height, int max_number_of_features, 42 | std::string model_path, float conf_thresh, int nms_dist, 43 | float nn_thresh); 44 | 45 | 46 | virtual bool IsInitialized() const { return is_initialized_; } 47 | 48 | virtual int GetWidth() const { return width_; } 49 | 50 | virtual int GetHeight() const { return height_; } 51 | 52 | virtual void GetFeatureLocations(std::vector* points) const; 53 | 54 | virtual const std::vector& GetNewTrackIds() const { 55 | return previous_track_ids_; 56 | } 57 | 58 | virtual int GetNumberOfFeaturesMatched() const { return matches_.cols(); } 59 | 60 | void GetFeatureDescriptor(FeatureDescriptorf* descriptors) { 61 | descriptors->Copy(current_descriptors_); 62 | } 63 | 64 | virtual void Display(); 65 | 66 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 67 | 68 | private: 69 | void LocalInit(int width, int height); 70 | 71 | std::shared_ptr detector_; 72 | std::shared_ptr matcher_; 73 | 74 | int width_; 75 | int height_; 76 | int current_track_id_; 77 | bool is_initialized_; 78 | 79 | Eigen::Matrix2Xf previous_feature_locations_; 80 | Eigen::Matrix2Xf current_feature_locations_; 81 | /** 82 | * matches_[0]: current_image matched keypoints index 83 | * [1]: previous_image matched keypoints index 84 | * [2]: distance 85 | */ 86 | Eigen::Matrix3Xf matches_; 87 | 88 | std::vector previous_track_ids_; 89 | std::vector current_track_ids_; 90 | 91 | FeatureDescriptorf previous_descriptors_; 92 | FeatureDescriptorf current_descriptors_; 93 | 94 | cv::Mat previous_image_; 95 | cv::Mat current_image_; 96 | }; 97 | 98 | #endif 99 | -------------------------------------------------------------------------------- /src/ORBFeatureMatcher.cc: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: ORBFeatureMatcher.cc 6 | * 7 | * Created On: Thu 26 Sep 2019 03:27:02 PM CST 8 | * Licensed under The MIT License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | 12 | #include "ORBFeatureMatcher.h" 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | DEFINE_int32(dist_type, 4, "1=NORM_L1, 2=NORM_L2, 4=NORM_HAMMING."); 20 | DEFINE_double(refine_dist, 64.0, "Reject distance > refine_dist matches."); 21 | 22 | ORBFeatureMatcher::ORBFeatureMatcher() 23 | : dist_type_(DistType::NORM_HAMMING), 24 | refine_dist_(0), 25 | is_initialized_(false) {} 26 | 27 | void ORBFeatureMatcher::Init() { 28 | Init(static_cast(FLAGS_dist_type), FLAGS_refine_dist); 29 | } 30 | 31 | void ORBFeatureMatcher::Init(DistType dist_type, float refine_dist) { 32 | CHECK_GT(refine_dist, 0.); 33 | dist_type_ = dist_type; 34 | refine_dist_ = refine_dist; 35 | matches_.reserve(2000); 36 | refined_matches_.reserve(2000); 37 | 38 | matcher_ = cv::BFMatcher::create(dist_type_); 39 | 40 | is_initialized_ = true; 41 | } 42 | 43 | ORBFeatureMatcher::ORBFeatureMatcher(const ORBFeatureMatcher& d) { 44 | if (d.IsInitialized()) { 45 | Init(d.GetDistType(), d.GetRefineDist()); 46 | } 47 | } 48 | 49 | ORBFeatureMatcher& ORBFeatureMatcher::operator=(const ORBFeatureMatcher& d) { 50 | if (this == &d) { 51 | return *this; 52 | } 53 | if (d.IsInitialized()) { 54 | Init(d.GetDistType(), d.GetRefineDist()); 55 | } 56 | return *this; 57 | } 58 | 59 | int ORBFeatureMatcher::Match(const cv::Mat& descriptors_img1, 60 | const cv::Mat& descriptors_img2, 61 | std::vector* output_matches) { 62 | if (descriptors_img1.empty() or descriptors_img2.empty()) return -1; 63 | 64 | matches_.resize(0); 65 | matcher_->match(descriptors_img1, descriptors_img2, matches_); 66 | 67 | std::unordered_set query_indicies; 68 | std::unordered_set train_indicies; 69 | 70 | VLOG(4) << "Number of features in image 1: " << descriptors_img1.rows 71 | << ", in image 2: " << descriptors_img2.rows; 72 | 73 | std::unordered_map train_index_to_index_map; 74 | refined_matches_.resize(0); 75 | for (size_t i = 0; i < matches_.size(); i++) { 76 | if (matches_[i].distance < refine_dist_) { 77 | const int& key = matches_[i].trainIdx; 78 | if (train_index_to_index_map.count(key) == 0) { 79 | train_index_to_index_map[key] = refined_matches_.size(); 80 | refined_matches_.push_back(matches_[i]); 81 | } else if (refined_matches_[train_index_to_index_map[key]].distance < 82 | matches_[i].distance) { 83 | refined_matches_[train_index_to_index_map[key]] = matches_[i]; 84 | } 85 | } 86 | } 87 | VLOG(4) << "Number of refined matches: " << refined_matches_.size(); 88 | if (refined_matches_.size() == 0) 89 | LOG(WARNING) << "Couldn't find refined matches"; 90 | 91 | *output_matches = refined_matches_; 92 | return refined_matches_.size(); 93 | } 94 | -------------------------------------------------------------------------------- /utils/pytorch_model_convert.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ######################################################################### 3 | # 4 | # Author: b51 5 | # Mail: b51live@gmail.com 6 | # FileName: pytorch_model_convert.py 7 | # 8 | # Created On: Mon 30 Sep 2019 09:32:30 AM UTC 9 | # Licensed under The MIT License [see LICENSE for details] 10 | # 11 | ######################################################################### 12 | 13 | import torch 14 | import sys 15 | 16 | 17 | class SuperPointNet(torch.nn.Module): 18 | """ Pytorch definition of SuperPoint Network. """ 19 | 20 | def __init__(self): 21 | super(SuperPointNet, self).__init__() 22 | self.relu = torch.nn.ReLU(inplace=True) 23 | self.pool = torch.nn.MaxPool2d(kernel_size=2, stride=2) 24 | c1, c2, c3, c4, c5, d1 = 64, 64, 128, 128, 256, 256 25 | # Shared Encoder. 26 | self.conv1a = torch.nn.Conv2d(1, c1, kernel_size=3, stride=1, padding=1) 27 | self.conv1b = torch.nn.Conv2d(c1, c1, kernel_size=3, stride=1, padding=1) 28 | self.conv2a = torch.nn.Conv2d(c1, c2, kernel_size=3, stride=1, padding=1) 29 | self.conv2b = torch.nn.Conv2d(c2, c2, kernel_size=3, stride=1, padding=1) 30 | self.conv3a = torch.nn.Conv2d(c2, c3, kernel_size=3, stride=1, padding=1) 31 | self.conv3b = torch.nn.Conv2d(c3, c3, kernel_size=3, stride=1, padding=1) 32 | self.conv4a = torch.nn.Conv2d(c3, c4, kernel_size=3, stride=1, padding=1) 33 | self.conv4b = torch.nn.Conv2d(c4, c4, kernel_size=3, stride=1, padding=1) 34 | # Detector Head. 35 | self.convPa = torch.nn.Conv2d(c4, c5, kernel_size=3, stride=1, padding=1) 36 | self.convPb = torch.nn.Conv2d(c5, 65, kernel_size=1, stride=1, padding=0) 37 | # Descriptor Head. 38 | self.convDa = torch.nn.Conv2d(c4, c5, kernel_size=3, stride=1, padding=1) 39 | self.convDb = torch.nn.Conv2d(c5, d1, kernel_size=1, stride=1, padding=0) 40 | 41 | def forward(self, x): 42 | """ Forward pass that jointly computes unprocessed point and descriptor 43 | tensors. 44 | Input 45 | x: Image pytorch tensor shaped N x 1 x H x W. 46 | Output 47 | semi: Output point pytorch tensor shaped N x 65 x H/8 x W/8. 48 | desc: Output descriptor pytorch tensor shaped N x 256 x H/8 x W/8. 49 | """ 50 | # Shared Encoder. 51 | x = self.relu(self.conv1a(x)) 52 | x = self.relu(self.conv1b(x)) 53 | x = self.pool(x) 54 | x = self.relu(self.conv2a(x)) 55 | x = self.relu(self.conv2b(x)) 56 | x = self.pool(x) 57 | x = self.relu(self.conv3a(x)) 58 | x = self.relu(self.conv3b(x)) 59 | x = self.pool(x) 60 | x = self.relu(self.conv4a(x)) 61 | x = self.relu(self.conv4b(x)) 62 | # Detector Head. 63 | cPa = self.relu(self.convPa(x)) 64 | semi = self.convPb(cPa) 65 | # Descriptor Head. 66 | cDa = self.relu(self.convDa(x)) 67 | desc = self.convDb(cDa) 68 | dn = torch.norm(desc, p=2, dim=1) # Compute the norm. 69 | desc = desc.div(torch.unsqueeze(dn, 1)) # Divide by norm to normalize. 70 | return semi, desc 71 | 72 | 73 | def main(argv): 74 | super_point_net = SuperPointNet() 75 | super_point_net.load_state_dict(torch.load( 76 | argv[1], map_location=lambda storage, loc: storage)) 77 | #super_point_net.eval() 78 | #script_module = torch.jit.script(super_point_net) 79 | #script_module.save("super_point_scripted_model.pt") 80 | example = torch.rand(1, 1, 120, 160) 81 | traced_script_module = torch.jit.trace(super_point_net, example) 82 | traced_script_module.save("super_point_scripted_model.pt") 83 | 84 | 85 | if __name__ == "__main__": 86 | main(sys.argv) 87 | -------------------------------------------------------------------------------- /include/SuperPointFeatureDetector.h: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: SuperPointFeatureDetector.h 6 | * 7 | * Created On: Thu 26 Sep 2019 04:07:29 AM UTC 8 | * Licensed under The MIT License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | 12 | #ifndef FEATURE_TRACKER_SUPER_POINT_FEATURE_DETECTOR_H_ 13 | #define FEATURE_TRACKER_SUPER_POINT_FEATURE_DETECTOR_H_ 14 | 15 | #include "FeatureDetectorBase.h" 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | class SuperPointFeatureDetector : public FeatureDetectorBase { 24 | public: 25 | SuperPointFeatureDetector(); 26 | ~SuperPointFeatureDetector() = default; 27 | 28 | virtual void Init(int width, int height, int max_number_of_features); 29 | 30 | void Init(int width, int height, int max_number_of_features, 31 | std::string model_path, float conf_thresh, int nms_dist); 32 | 33 | virtual void Detect(const cv::Mat& image, 34 | Eigen::Matrix2Xf* current_measurements, 35 | FeatureDescriptorf* current_feature_descriptors); 36 | 37 | inline virtual bool IsInitialized() const { return is_initialized_; } 38 | 39 | inline virtual int GetWidth() const { return width_; } 40 | 41 | inline virtual int GetHeight() const { return height_; } 42 | 43 | inline virtual int GetMaxNumberOfFeatures() const { 44 | return max_number_of_features_; 45 | } 46 | 47 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 48 | 49 | private: 50 | /** 51 | * Detect input 52 | * image: gray image normed to [0.0, 1.0] 53 | * return: 54 | * feature_locations: 3 X N feature points, [x, y, confidence]^T X N 55 | * descriptors: normed 256 X N descriptors 56 | */ 57 | void Detect(const cv::Mat& image, Eigen::Matrix3Xf& filtered_pts, 58 | torch::Tensor& descriptors); 59 | 60 | /** 61 | * Run a faster approximate Non-Max-Suppression on Eigen corners shaped: 62 | * 3xN [x_i,y_i,conf_i]^T 63 | * 64 | * Algo summary: Create a grid sized height x width. Assign each corner 65 | * location a 1, rest are zeros. Iterate through all the 1's and convert 66 | * them either to -1 or 0. Suppress points by setting nearby values to 0. 67 | * 68 | * Grid Value Legend: 69 | * -1 : Kept. 70 | * 0 : Empty or suppressed. 71 | * 1 : To be processed (converted to either kept or supressed). 72 | * 73 | * NOTE: The NMS first rounds points to integers, so NMS distance might not 74 | * be exactly dist_thresh. It also assumes points are within image boundaries. 75 | * 76 | * Inputs 77 | * in_corners - 3 x N Eigen Matrix with corners [x_i, y_i, confidence_i]^T. 78 | * width - Image width. 79 | * height - Image height. 80 | * dist_thresh - Distance to suppress, measured as an infinty norm distance. 81 | * Returns 82 | * filtered_pts - 3 x N Eigen Matrix with surviving corners. 83 | */ 84 | void NMSFast(const Eigen::Matrix3Xf& pts, int width, int height, 85 | int dist_thresh, Eigen::Matrix3Xf& filtered_pts); 86 | 87 | int width_; 88 | int height_; 89 | int max_number_of_features_; 90 | 91 | std::string model_path_; 92 | float conf_thresh_; 93 | int nms_dist_; 94 | bool is_initialized_; 95 | 96 | std::shared_ptr module_; 97 | cv::Mat image_; 98 | }; 99 | 100 | #endif 101 | -------------------------------------------------------------------------------- /include/FeatureDescriptor.h: -------------------------------------------------------------------------------- 1 | #ifndef BINARY_FEATURE_STORE_H_ 2 | #define BINARY_FEATURE_STORE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | static const int kDefaultReservedSpace = 2000; 11 | 12 | template 13 | class FeatureDescriptor { 14 | public: 15 | FeatureDescriptor() : num_features_(0), descriptor_size_(0) {} 16 | 17 | FeatureDescriptor(int descriptor_size, 18 | int reserved_space = kDefaultReservedSpace) 19 | : num_features_(0), descriptor_size_(descriptor_size) { 20 | descriptor_data_.reserve(reserved_space * descriptor_size_); 21 | } 22 | 23 | // Set the number of bytes per descriptor. Any existing data is invalidated. 24 | // The number of features is always zero following this call; the 25 | // reserved_space argument only reserves space so that future resize() calls 26 | // avoid reallocation. 27 | void Configure(int descriptor_size, 28 | int reserved_space = kDefaultReservedSpace) { 29 | descriptor_size_ = descriptor_size; 30 | Resize(0); 31 | descriptor_data_.reserve(reserved_space * descriptor_size_); 32 | } 33 | 34 | // Change the number of features. If n > num_features_ then this function 35 | // may trigger a re-allocation. If n < num_features_ then this function 36 | // never re-allocates. In either case the previous data is always retained. 37 | void Resize(int n) { 38 | num_features_ = n; 39 | descriptor_data_.resize(n * descriptor_size_); 40 | } 41 | 42 | void Clear() { 43 | num_features_ = 0; 44 | descriptor_data_.clear(); 45 | } 46 | 47 | inline uint32_t Size() const { return num_features_; } 48 | 49 | inline bool Empty() const { return num_features_ == 0; } 50 | 51 | inline uint32_t DescriptorSize() const { return descriptor_size_; } 52 | 53 | inline ScalarType* descriptor(uint32_t index) { 54 | return &descriptor_data_[index * descriptor_size_]; 55 | } 56 | 57 | inline const ScalarType* descriptor(uint32_t index) const { 58 | return &descriptor_data_[index * descriptor_size_]; 59 | } 60 | 61 | inline void SetDescriptorToZero(uint32_t index) { 62 | memset(descriptor(index), 0, descriptor_size_); 63 | } 64 | 65 | inline void SetDescriptorRandom(uint32_t index, int seed) { 66 | std::mt19937 generator(seed); 67 | ScalarType* data = descriptor(index); 68 | for (size_t i = 0; i < descriptor_size_; ++i) { 69 | data[i] = generator() % 256; 70 | } 71 | } 72 | 73 | inline ScalarType* descriptor_data() { return descriptor_data_.data(); } 74 | inline const ScalarType* descriptor_data() const { 75 | return descriptor_data_.data(); 76 | } 77 | 78 | void Swap(FeatureDescriptor* other) { 79 | CHECK_NE(other, this); 80 | CHECK_NOTNULL(other); 81 | std::swap(num_features_, other->num_features_); 82 | std::swap(descriptor_size_, other->descriptor_size_); 83 | std::swap(descriptor_data_, other->descriptor_data_); 84 | } 85 | 86 | // Copy the feature store from another. The preferred method of moving data is 87 | // with swap. 88 | void Copy(const FeatureDescriptor& other) { 89 | num_features_ = other.num_features_; 90 | descriptor_size_ = other.descriptor_size_; 91 | descriptor_data_ = other.descriptor_data_; 92 | } 93 | 94 | private: 95 | // Prevent accidental use of the copy constructor since it is expensive. 96 | // The preferred way to move feature stores around is with swap(). 97 | FeatureDescriptor(const FeatureDescriptor&) = delete; 98 | FeatureDescriptor& operator=(const FeatureDescriptor&) = delete; 99 | 100 | // The number of features in the store. 101 | uint32_t num_features_; 102 | 103 | // Number of bytes per descriptor. 104 | uint32_t descriptor_size_; 105 | 106 | std::vector descriptor_data_; 107 | }; 108 | 109 | using FeatureDescriptoru = FeatureDescriptor; 110 | using FeatureDescriptorf = FeatureDescriptor; 111 | 112 | #endif 113 | -------------------------------------------------------------------------------- /src/main_track.cc: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: main_track.cc 6 | * 7 | * Created On: Sun 13 Oct 2019 11:00:50 AM CST 8 | * Licensed under The MIT License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include "FeatureTrackerBase.h" 20 | #include "ORBFeatureTracker.h" 21 | #include "SuperPointFeatureTracker.h" 22 | 23 | DEFINE_string( 24 | image_dir, 25 | "/home/b51live/Sources/SuperPointPretrainedNetwork/assets", 26 | "Where to find images"); 27 | DEFINE_string(image_suffix, ".jpg", "Suffix of images, default: .jpg"); 28 | DEFINE_int32(max_number_of_features, 500, "max number of detected features"); 29 | 30 | DEFINE_string(type, "sp", "Feature detector type[orb, sp], default: orb"); 31 | 32 | void LoadImages(const std::string& dir_name, 33 | std::vector& image_filenames) { 34 | std::vector files; 35 | cv::glob(dir_name, files); 36 | for (auto file : files) { 37 | std::string image_file(file); 38 | size_t index = image_file.rfind("."); 39 | if (index != std::string::npos) { 40 | std::string suffix = image_file.substr(index); 41 | if (suffix.compare(FLAGS_image_suffix) == 0) { 42 | image_filenames.emplace_back(image_file); 43 | } 44 | } 45 | } // end of for loop 46 | } 47 | 48 | int main(int argc, char* argv[]) { 49 | google::InitGoogleLogging(argv[0]); 50 | google::ParseCommandLineFlags(&argc, &argv, true); 51 | FLAGS_logtostderr = true; 52 | FLAGS_colorlogtostderr = true; 53 | 54 | std::vector image_filenames; 55 | std::string string_file = FLAGS_image_dir; // + "/rgb.txt"; 56 | LoadImages(string_file, image_filenames); 57 | 58 | std::cout << "Usage: " << argv[0] 59 | << " --image_dir path/to/images --image_suffix .jpg/.png --model" 60 | " path/to/model --type orb/sp [--model path/to/sp_model --W" 61 | " sp_input_W --H sp_input_H --CUDA device_num]" 62 | << std::endl; 63 | 64 | cv::Mat image = cv::imread(image_filenames[0], CV_LOAD_IMAGE_UNCHANGED); 65 | 66 | LOG(INFO) << "type: " << FLAGS_type; 67 | std::shared_ptr tracker; 68 | if (FLAGS_type.compare("orb") == 0) { 69 | tracker = std::make_shared(); 70 | } else if (FLAGS_type.compare("sp") == 0 or 71 | FLAGS_type.compare("superpoint") == 0) { 72 | tracker = std::make_shared(); 73 | } else { 74 | LOG(FATAL) << "Feature type is not supported, only support orb/sp"; 75 | } 76 | 77 | tracker->Init(image.cols, image.rows, FLAGS_max_number_of_features); 78 | 79 | // coordinates in pixel plane 80 | Eigen::Matrix2Xf current_measurements; 81 | Eigen::Matrix2Xf previous_measurements; 82 | std::vector feature_ids; 83 | 84 | for (auto image_filename : image_filenames) { 85 | image = cv::imread(image_filename, CV_LOAD_IMAGE_UNCHANGED); 86 | if (image.channels() > 1) { 87 | cv::cvtColor(image, image, CV_BGR2GRAY); 88 | } 89 | if (image.empty()) { 90 | LOG(FATAL) << "Failed to load image at: " << FLAGS_image_dir << "/" 91 | << image_filename; 92 | } 93 | tracker->Track(image, ¤t_measurements, &previous_measurements, 94 | &feature_ids); 95 | tracker->Display(); 96 | VLOG(4) << "Feature size: " << current_measurements.size(); 97 | if (FLAGS_type.compare("orb") == 0) { 98 | FeatureDescriptoru descriptors; 99 | std::dynamic_pointer_cast(tracker) 100 | ->GetFeatureDescriptor(&descriptors); 101 | } else if (FLAGS_type.compare("sp") == 0 or 102 | FLAGS_type.compare("superpoint") == 0) { 103 | FeatureDescriptorf descriptors; 104 | std::dynamic_pointer_cast(tracker) 105 | ->GetFeatureDescriptor(&descriptors); 106 | } 107 | } 108 | } 109 | -------------------------------------------------------------------------------- /include/ORBFeatureTracker.h: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: ORBFeatureTracker.h 6 | * 7 | * Created On: Thu 26 Sep 2019 04:07:53 PM CST 8 | * Licensed under The MIT License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | 12 | #ifndef FEATURE_TRACKER_ORB_FEATURE_TRACKER_H_ 13 | #define FEATURE_TRACKER_ORB_FEATURE_TRACKER_H_ 14 | 15 | #include "FeatureTrackerBase.h" 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include "FeatureDescriptor.h" 23 | #include "FeatureDetectorBase.h" 24 | #include "ORBFeatureDetector.h" 25 | #include "ORBFeatureMatcher.h" 26 | 27 | class ORBFeatureTracker : public FeatureTrackerBase { 28 | public: 29 | typedef cv::NormTypes DistType; 30 | 31 | ORBFeatureTracker(); 32 | 33 | ~ORBFeatureTracker() = default; 34 | 35 | virtual void Init(int width, int height, int max_number_of_features); 36 | 37 | void Init(int width, int height, int max_number_of_features, 38 | float scale_factor, int number_of_levels, int edge_dist, 39 | int patch_size, bool refine_corners, DistType dist_type, 40 | float refine_dist); 41 | 42 | virtual void Track(const cv::Mat& image, 43 | Eigen::Matrix2Xf* current_measurements, 44 | Eigen::Matrix2Xf* previous_measurements, 45 | std::vector* feature_ids); 46 | 47 | virtual void Display(); 48 | 49 | inline virtual int GetWidth() const { return width_; } 50 | 51 | inline virtual int GetHeight() const { return height_; } 52 | 53 | inline virtual bool IsInitialized() const { return is_initialized_; } 54 | 55 | inline float GetScaleFactor() const { return detector_->GetScaleFactor(); } 56 | 57 | inline int GetEdgeDist() const { return detector_->GetEdgeDist(); } 58 | 59 | inline int GetPatchSize() const { return detector_->GetPatchSize(); } 60 | 61 | inline bool GetRefineCorners() const { return detector_->GetRefineCorners(); } 62 | 63 | inline DistType GetDistType() const { return matcher_->GetDistType(); } 64 | 65 | inline float GetRefineDist() const { return matcher_->GetRefineDist(); } 66 | 67 | inline int GetMaxNumberOfFeatures() const { 68 | return detector_->GetMaxNumberOfFeatures(); 69 | } 70 | 71 | inline int GetNumberOfLevels() const { 72 | return detector_->GetNumberOfLevels(); 73 | } 74 | 75 | void SetDetectorMask(const cv::Mat& mask); 76 | 77 | const std::vector& GetMatches() const { return matches_; } 78 | 79 | void GetFeatureDescriptor(FeatureDescriptoru* descriptors); 80 | 81 | void GetFeatureDescriptor(cv::Mat* descriptors) const { 82 | current_descriptors_.copyTo(*descriptors); 83 | } 84 | 85 | virtual void GetFeatureLocations(std::vector* points) const { 86 | CHECK_NOTNULL(points); 87 | points->resize(current_feature_locations_.size()); 88 | for (size_t i = 0; i < current_feature_locations_.size(); i++) { 89 | (*points)[i] << current_feature_locations_[i].pt.x, 90 | current_feature_locations_[i].pt.y; 91 | } 92 | } 93 | 94 | virtual const std::vector& GetNewTrackIds() const { return previous_track_ids_; } 95 | 96 | virtual int GetNumberOfFeaturesDetected() const { 97 | return current_feature_locations_.size(); 98 | } 99 | 100 | int GetNumberOfFeaturesMatched() const { return matches_.size(); } 101 | 102 | private: 103 | void LocalInit(int width, int height); 104 | 105 | std::unique_ptr detector_; 106 | std::unique_ptr matcher_; 107 | int width_; 108 | int height_; 109 | int current_track_id_; 110 | bool is_initialized_; 111 | 112 | std::vector previous_feature_locations_; 113 | std::vector current_feature_locations_; 114 | std::vector matches_; 115 | std::vector previous_track_ids_; 116 | std::vector current_track_ids_; 117 | 118 | cv::Mat previous_descriptors_; 119 | cv::Mat current_descriptors_; 120 | 121 | // For debugging we keep the images here 122 | cv::Mat previous_image_; 123 | cv::Mat current_image_; 124 | }; 125 | 126 | #endif 127 | -------------------------------------------------------------------------------- /src/SuperPointFeatureMatcher.cc: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: SuperPointFeatureMatcher.cc 6 | * 7 | * Created On: Thu 10 Oct 2019 07:47:43 AM UTC 8 | * Licensed under The MIT License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | 12 | #include "SuperPointFeatureMatcher.h" 13 | 14 | #include 15 | #include 16 | 17 | DEFINE_double( 18 | nn_thresh, 0.7, 19 | "Optional descriptor distance below which is a good match (default: 0.7)"); 20 | 21 | SuperPointFeatureMatcher::SuperPointFeatureMatcher() 22 | : is_initialized_(false) {} 23 | 24 | void SuperPointFeatureMatcher::Init() { 25 | Init(FLAGS_nn_thresh); 26 | } 27 | 28 | void SuperPointFeatureMatcher::Init(float nn_thresh) { 29 | CHECK_GT(nn_thresh, 0.0); 30 | refine_dist_ = nn_thresh; 31 | matches_.reserve(2000); 32 | is_initialized_ = true; 33 | } 34 | 35 | SuperPointFeatureMatcher::SuperPointFeatureMatcher( 36 | const SuperPointFeatureMatcher& d) { 37 | if (d.IsInitialized()) { 38 | Init(d.GetRefineDist()); 39 | } 40 | } 41 | 42 | SuperPointFeatureMatcher& SuperPointFeatureMatcher::operator=( 43 | const SuperPointFeatureMatcher& d) { 44 | if (this == &d) { 45 | return *this; 46 | } 47 | if (d.IsInitialized()) { 48 | Init(d.GetRefineDist()); 49 | } 50 | return *this; 51 | } 52 | 53 | int SuperPointFeatureMatcher::Match(const FeatureDescriptorf& descriptors_img1, 54 | const FeatureDescriptorf& descriptors_img2, 55 | Eigen::Matrix3Xf* output_matches) { 56 | if (descriptors_img1.Empty() or descriptors_img2.Empty()) return -1; 57 | 58 | int number_of_matches = Match(descriptors_img1, descriptors_img2); 59 | 60 | output_matches->resize(3, number_of_matches); 61 | #pragma omp parallel for 62 | for (int i = 0; i < number_of_matches; i++) { 63 | output_matches->col(i)[0] = matches_[i].index1; 64 | output_matches->col(i)[1] = matches_[i].index2; 65 | output_matches->col(i)[2] = matches_[i].distance; 66 | } 67 | VLOG(4) << " number_of_matches " << output_matches->cols(); 68 | return number_of_matches; 69 | } 70 | 71 | int SuperPointFeatureMatcher::Match( 72 | const FeatureDescriptorf& descriptors_img1, 73 | const FeatureDescriptorf& descriptors_img2) { 74 | matches_.resize(0); 75 | // descriptors_img1 size = N1 X 256 76 | // descriptors_img2 size = N2 X 256 77 | int number_of_features_1 = descriptors_img1.Size(); 78 | int number_of_features_2 = descriptors_img2.Size(); 79 | int descriptor_size = descriptors_img1.DescriptorSize(); 80 | 81 | Eigen::MatrixXf eig_descriptors_1, eig_descriptors_2; 82 | eig_descriptors_1.resize(number_of_features_1, descriptor_size); 83 | eig_descriptors_2.resize(number_of_features_2, descriptor_size); 84 | 85 | for (int i = 0; i < number_of_features_1; i++) { 86 | eig_descriptors_1.row(i) = Eigen::Map( 87 | descriptors_img1.descriptor(i), 1, descriptor_size); 88 | } 89 | for (int i = 0; i < number_of_features_2; i++) { 90 | eig_descriptors_2.row(i) = Eigen::Map( 91 | descriptors_img2.descriptor(i), 1, descriptor_size); 92 | } 93 | 94 | // nn_matches size = N1 X N2 95 | Eigen::MatrixXf nn_matches = 96 | eig_descriptors_1 * eig_descriptors_2.transpose(); 97 | // clip scores to range [-1.0, 1.0] 98 | nn_matches = 2.0 * nn_matches.array().max(-1.0).min(1.0); 99 | // make sure scores positive 100 | nn_matches = 2.0 - nn_matches.array(); 101 | // get sqrt root of scores 102 | nn_matches = nn_matches.array().sqrt(); 103 | 104 | // get min scores of each row 105 | Eigen::VectorXf scores(nn_matches.rows(), 1); 106 | scores = nn_matches.rowwise().minCoeff(); 107 | 108 | // get each min scores index of rows 109 | Eigen::VectorXf indices_1(nn_matches.rows(), 1); 110 | for (int i = 0; i < nn_matches.rows(); i++) 111 | nn_matches.row(i).minCoeff(&indices_1(i)); 112 | // get each min scores index of cols 113 | Eigen::VectorXf indices_2(nn_matches.cols(), 1); 114 | for (int i = 0; i < nn_matches.cols(); i++) 115 | nn_matches.col(i).minCoeff(&indices_2(i)); 116 | 117 | // Threshold the NN matches, meet condition true, else false 118 | Eigen::Matrix keep(scores.rows(), 1); 119 | keep = (scores.array() < refine_dist_); 120 | 121 | // Check if nearest neighbor goes both directions and keep those. 122 | #pragma omp parallel for 123 | for (int i = 0; i < indices_1.rows(); i++) { 124 | keep[i] = (indices_2[indices_1[i]] == i) && keep[i]; 125 | if (keep[i]) { 126 | matches_.emplace_back(FeatureMatch(i, indices_1[i], scores[i])); 127 | } 128 | } 129 | return matches_.size(); 130 | } 131 | -------------------------------------------------------------------------------- /src/SuperPointFeatureTracker.cc: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: SuperPointFeatureTracker.cc 6 | * 7 | * Created On: Fri 11 Oct 2019 07:52:29 AM UTC 8 | * Licensed under The MIT License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | 12 | #include "SuperPointFeatureTracker.h" 13 | 14 | SuperPointFeatureTracker::SuperPointFeatureTracker() 15 | : detector_(nullptr), 16 | matcher_(nullptr), 17 | width_(0), 18 | height_(0), 19 | current_track_id_(-1), 20 | is_initialized_(false) {} 21 | 22 | void SuperPointFeatureTracker::Init(int width, int height, 23 | int max_number_of_features) { 24 | LocalInit(width, height); 25 | detector_->Init(width, height, max_number_of_features); 26 | matcher_->Init(); 27 | is_initialized_ = true; 28 | } 29 | 30 | void SuperPointFeatureTracker::Init(int width, int height, 31 | int max_number_of_features, 32 | std::string model_path, float conf_thresh, 33 | int nms_dist, float nn_thresh) { 34 | LocalInit(width, height); 35 | detector_->Init(width, height, max_number_of_features, model_path, 36 | conf_thresh, nms_dist); 37 | matcher_->Init(nn_thresh); 38 | is_initialized_ = true; 39 | } 40 | 41 | void SuperPointFeatureTracker::LocalInit(int width, int height) { 42 | width_ = width; 43 | height_ = height; 44 | 45 | previous_track_ids_.reserve(1000); 46 | current_track_ids_.reserve(1000); 47 | 48 | detector_ = std::make_shared(); 49 | matcher_ = std::make_shared(); 50 | } 51 | 52 | void SuperPointFeatureTracker::GetFeatureLocations( 53 | std::vector* points) const { 54 | CHECK_NOTNULL(points); 55 | points->resize(current_feature_locations_.cols()); 56 | for (int i = 0; i < current_feature_locations_.cols(); i++) { 57 | (*points)[i] << current_feature_locations_.col(i)[0], 58 | current_feature_locations_.col(i)[1]; 59 | } 60 | } 61 | 62 | void SuperPointFeatureTracker::Track(const cv::Mat& image, 63 | Eigen::Matrix2Xf* current_measurements, 64 | Eigen::Matrix2Xf* previous_measurements, 65 | std::vector* feature_ids) { 66 | previous_feature_locations_ = current_feature_locations_; 67 | image.copyTo(current_image_); 68 | 69 | std::swap(previous_track_ids_, current_track_ids_); 70 | current_descriptors_.Swap(&previous_descriptors_); 71 | 72 | detector_->Detect(image, ¤t_feature_locations_, ¤t_descriptors_); 73 | int number_of_features = current_feature_locations_.cols(); 74 | if (previous_feature_locations_.cols() > 0 and number_of_features > 0) { 75 | matcher_->Match(current_descriptors_, previous_descriptors_, &matches_); 76 | } 77 | int number_of_matches = matches_.cols(); 78 | current_track_ids_.clear(); 79 | current_track_ids_.resize(number_of_features, -1); 80 | for (int i = 0; i < number_of_matches; i++) { 81 | current_track_ids_[matches_.col(i)[0]] = previous_track_ids_[matches_.col(i)[1]]; 82 | } 83 | for (auto& id : current_track_ids_) { 84 | if (id == -1) { 85 | id = ++current_track_id_; 86 | } 87 | } 88 | 89 | current_measurements->resize(2, number_of_matches); 90 | previous_measurements->resize(2, number_of_matches); 91 | feature_ids->clear(); 92 | 93 | for (int i = 0; i < number_of_matches; i++) { 94 | current_measurements->col(i) = current_feature_locations_.col(matches_.col(i)[0]); 95 | previous_measurements->col(i) = previous_feature_locations_.col(matches_.col(i)[1]); 96 | feature_ids->push_back(current_track_ids_[matches_.col(i)[0]]); 97 | } 98 | } 99 | 100 | void SuperPointFeatureTracker::Display() { 101 | cv::Mat image_matches; 102 | std::vector previous_cv_keypoints; 103 | std::vector current_cv_keypoints; 104 | std::vector cv_matches; 105 | int number_of_matches = matches_.cols(); 106 | if (number_of_matches > 0) { 107 | for (int i = 0; i < number_of_matches; i++) { 108 | int x = std::round(current_feature_locations_.col(matches_.col(i)[0])[0]); 109 | int y = std::round(current_feature_locations_.col(matches_.col(i)[0])[1]); 110 | int px = std::round(previous_feature_locations_.col(matches_.col(i)[0])[0]); 111 | int py = std::round(previous_feature_locations_.col(matches_.col(i)[1])[1]); 112 | // int id = current_track_ids_[matches_.col(i)[0]]; 113 | 114 | previous_cv_keypoints.emplace_back(cv::KeyPoint(px, py, 1.)); 115 | current_cv_keypoints.emplace_back(cv::KeyPoint(x, y, 1.)); 116 | cv_matches.emplace_back(i, i, matches_.col(i)[2]); 117 | // cv::Scalar(0, 255, 255)); 118 | // cv::putText(current_image_, std::to_string(id), cv::Point(px, py), 1, 1.0, 119 | // cv::Scalar(0, 0, 0), 1, 1, false); 120 | } 121 | cv::drawMatches(current_image_, current_cv_keypoints, previous_image_, 122 | previous_cv_keypoints, cv_matches, image_matches, 123 | cv::Scalar(255, 0, 0), cv::Scalar::all(-1), 124 | std::vector(), 0); 125 | cv::imshow("tracker", image_matches); 126 | cv::waitKey(1); 127 | /* save images 128 | static int iter = 0; 129 | std::stringstream ss; 130 | ss << std::setfill('0') << std::setw(6) << iter++; 131 | std::string img_name = "image_" + ss.str() + ".jpg"; 132 | cv::imwrite(img_name, image_matches); 133 | */ 134 | } 135 | current_image_.copyTo(previous_image_); 136 | } 137 | -------------------------------------------------------------------------------- /src/ORBFeatureTracker.cc: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: ORBFeatureTracker.cc 6 | * 7 | * Created On: Thu 26 Sep 2019 04:06:52 PM CST 8 | * Licensed under The MIT License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | 12 | #include "ORBFeatureTracker.h" 13 | 14 | ORBFeatureTracker::ORBFeatureTracker() 15 | : detector_(nullptr), 16 | matcher_(nullptr), 17 | width_(0), 18 | height_(0), 19 | current_track_id_(-1), 20 | is_initialized_(false) {} 21 | 22 | void ORBFeatureTracker::Init(int width, int height, 23 | int max_number_of_features) { 24 | LocalInit(width, height); 25 | detector_->Init(width, height, max_number_of_features); 26 | matcher_->Init(); 27 | is_initialized_ = true; 28 | } 29 | 30 | void ORBFeatureTracker::Init(int width, int height, int max_number_of_features, 31 | float scale_factor, int number_of_levels, 32 | int edge_dist, int patch_size, bool refine_corners, 33 | DistType dist_type, float refine_dist) { 34 | LocalInit(width, height); 35 | detector_->Init(width, height, max_number_of_features, scale_factor, 36 | number_of_levels, edge_dist, patch_size, refine_corners); 37 | matcher_->Init(dist_type, refine_dist); 38 | is_initialized_ = true; 39 | } 40 | 41 | void ORBFeatureTracker::LocalInit(int width, int height) { 42 | width_ = width; 43 | height_ = height; 44 | 45 | previous_feature_locations_.reserve(1000); 46 | current_feature_locations_.reserve(1000); 47 | matches_.reserve(1000); 48 | previous_track_ids_.reserve(1000); 49 | current_track_ids_.reserve(1000); 50 | 51 | detector_.reset(new ORBFeatureDetector); 52 | matcher_.reset(new ORBFeatureMatcher); 53 | } 54 | 55 | void ORBFeatureTracker::Track(const cv::Mat& image, 56 | Eigen::Matrix2Xf* current_measurements, 57 | Eigen::Matrix2Xf* previous_measurements, 58 | std::vector* feature_ids) { 59 | std::swap(previous_feature_locations_, current_feature_locations_); 60 | current_feature_locations_.resize(0); 61 | 62 | std::swap(previous_track_ids_, current_track_ids_); 63 | cv::swap(current_descriptors_, previous_descriptors_); 64 | 65 | int number_of_features = 66 | detector_->Detect(image, ¤t_feature_locations_, ¤t_descriptors_); 67 | current_image_ = detector_->GetCvImage(); 68 | 69 | previous_image_ = detector_->GetCvImage(); 70 | 71 | if (previous_feature_locations_.size() > 0 and number_of_features > 0) { 72 | matcher_->Match(current_descriptors_, previous_descriptors_, &matches_); 73 | } 74 | current_track_ids_.clear(); 75 | current_track_ids_.resize(current_feature_locations_.size(), -1); 76 | 77 | std::vector checked_matches; 78 | cv::Point2f diff(0., 0.); 79 | checked_matches.reserve(matches_.size()); 80 | VLOG(4) << "original matches size: " << matches_.size(); 81 | for (const auto& match : matches_) { 82 | diff = previous_feature_locations_[match.trainIdx].pt - 83 | current_feature_locations_[match.queryIdx].pt; 84 | if (std::fabs(diff.x) < width_ and std::fabs(diff.y) < height_) { 85 | checked_matches.push_back(match); 86 | } 87 | } 88 | VLOG(4) << "checked matches size: " << checked_matches.size(); 89 | std::swap(matches_, checked_matches); 90 | 91 | for (const auto& match : matches_) { 92 | current_track_ids_[match.queryIdx] = previous_track_ids_[match.trainIdx]; 93 | } 94 | 95 | for (auto& id : current_track_ids_) { 96 | if (id == -1) { 97 | id = ++current_track_id_; 98 | } 99 | } 100 | 101 | current_measurements->resize(2, matches_.size()); 102 | previous_measurements->resize(2, matches_.size()); 103 | 104 | feature_ids->clear(); 105 | for (size_t i = 0; i < matches_.size(); i++) { 106 | current_measurements->operator()(0, i) = 107 | current_feature_locations_[matches_[i].queryIdx].pt.x; 108 | current_measurements->operator()(1, i) = 109 | current_feature_locations_[matches_[i].queryIdx].pt.y; 110 | 111 | previous_measurements->operator()(0, i) = 112 | previous_feature_locations_[matches_[i].trainIdx].pt.x; 113 | previous_measurements->operator()(1, i) = 114 | previous_feature_locations_[matches_[i].trainIdx].pt.y; 115 | 116 | feature_ids->push_back(current_track_ids_[matches_[i].queryIdx]); 117 | } 118 | } 119 | 120 | void ORBFeatureTracker::GetFeatureDescriptor(FeatureDescriptoru* descriptors) { 121 | int descriptor_size = current_descriptors_.cols; 122 | int number_of_features = current_descriptors_.rows; 123 | descriptors->Configure(descriptor_size, number_of_features); 124 | descriptors->Resize(number_of_features); 125 | if (current_descriptors_.isContinuous()) { 126 | memcpy(descriptors->descriptor(0), current_descriptors_.data, 127 | descriptor_size * number_of_features); 128 | } else { 129 | for (int i = 0; i < number_of_features; i++) { 130 | memcpy(descriptors->descriptor(i), 131 | ¤t_descriptors_.at(i, 0), descriptor_size); 132 | } 133 | } 134 | } 135 | 136 | void ORBFeatureTracker::Display() { 137 | static int iter = 0; 138 | cv::Mat image_matches; 139 | if (iter > 0) { 140 | cv::drawMatches(current_image_, current_feature_locations_, previous_image_, 141 | previous_feature_locations_, matches_, image_matches, 142 | cv::Scalar(255, 0, 0), cv::Scalar::all(-1), 143 | std::vector(), 0); 144 | cv::imshow("debug_image", image_matches); 145 | cv::waitKey(1); 146 | /* save images 147 | std::stringstream ss; 148 | ss << std::setfill('0') << std::setw(6) << iter; 149 | std::string img_name = "image_" + ss.str() + ".jpg"; 150 | cv::imwrite(img_name, image_matches); 151 | */ 152 | } 153 | iter++; 154 | current_image_.copyTo(previous_image_); 155 | } 156 | -------------------------------------------------------------------------------- /src/ORBFeatureDetector.cc: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: ORBFeatureDetector.cc 6 | * 7 | * Created On: Thu 27 Jun 2019 03:26:58 PM CST 8 | * Licensed under The MIT License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | 12 | #include "ORBFeatureDetector.h" 13 | 14 | #include 15 | #include 16 | 17 | DEFINE_double(scale_factor, 1.1f, "Scale increment"); 18 | DEFINE_int32(number_of_levels, 8, "Number of scales."); 19 | DEFINE_int32(edge_dist, 5, "Distance to edge not considered."); 20 | DEFINE_int32(patch_size, 31, "Patch size."); 21 | DEFINE_bool(refine_corners, false, "Whether we want to refine corners"); 22 | 23 | const static unsigned int kDescriptorLengthInBytes = 32; 24 | 25 | ORBFeatureDetector::ORBFeatureDetector() 26 | : width_(0), 27 | height_(0), 28 | max_number_of_features_(0), 29 | scale_factor_(0.), 30 | number_of_levels_(0), 31 | edge_dist_(0), 32 | patch_size_(0), 33 | refine_corners_(false), 34 | is_initialized_(false) {} 35 | 36 | void ORBFeatureDetector::Init(int width, int height, 37 | int max_number_of_features) { 38 | CHECK_GT(width, 0); 39 | CHECK_GT(height, 0); 40 | CHECK_GT(max_number_of_features, 0); 41 | Init(width, height, max_number_of_features, FLAGS_scale_factor, 42 | FLAGS_number_of_levels, FLAGS_edge_dist, FLAGS_patch_size, 43 | FLAGS_refine_corners); 44 | } 45 | 46 | void ORBFeatureDetector::Init(int width, int height, int max_number_of_features, 47 | float scale_factor, int number_of_levels, 48 | int edge_dist, int patch_size, 49 | bool refine_corners) { 50 | CHECK_GT(width, 0); 51 | CHECK_GT(height, 0); 52 | CHECK_GT(max_number_of_features, 0); 53 | CHECK_GT(scale_factor, 0); 54 | CHECK_GT(number_of_levels, 0); 55 | CHECK_GT(edge_dist, 0); 56 | CHECK_GT(patch_size, 0); 57 | 58 | width_ = width; 59 | height_ = height; 60 | max_number_of_features_ = max_number_of_features; 61 | scale_factor_ = scale_factor; 62 | number_of_levels_ = number_of_levels; 63 | edge_dist_ = edge_dist; 64 | patch_size_ = patch_size; 65 | refine_corners_ = refine_corners; 66 | 67 | image_.create(height_, width_, CV_8UC1); 68 | mask_ = cv::Mat(cv::Size(width_, height_), CV_8UC1, 0xFF); 69 | VLOG(3) << "ORB detector parameters :"; 70 | VLOG(3) << " max_number_of_features: " << max_number_of_features_; 71 | VLOG(3) << " scale_factor: " << scale_factor_; 72 | VLOG(3) << " number_of_levels: " << number_of_levels_; 73 | VLOG(3) << " edge_dist: " << edge_dist_; 74 | detector_ = 75 | cv::ORB::create(max_number_of_features_, scale_factor_, number_of_levels_, 76 | edge_dist_, 0, 2, cv::ORB::HARRIS_SCORE, patch_size_); 77 | is_initialized_ = true; 78 | } 79 | 80 | ORBFeatureDetector::ORBFeatureDetector(const ORBFeatureDetector& d) 81 | : FeatureDetectorBase() { 82 | if (d.IsInitialized()) { 83 | Init(d.GetWidth(), d.GetHeight(), d.GetMaxNumberOfFeatures(), 84 | d.GetScaleFactor(), d.GetNumberOfLevels(), d.GetEdgeDist(), 85 | d.GetPatchSize(), d.GetRefineCorners()); 86 | } 87 | } 88 | 89 | ORBFeatureDetector& ORBFeatureDetector::operator=(const ORBFeatureDetector& d) { 90 | if (this == &d) { 91 | return *this; 92 | } 93 | if (d.IsInitialized()) { 94 | Init(d.GetWidth(), d.GetHeight(), d.GetMaxNumberOfFeatures(), 95 | d.GetScaleFactor(), d.GetNumberOfLevels(), d.GetEdgeDist(), 96 | d.GetPatchSize(), d.GetRefineCorners()); 97 | } 98 | return *this; 99 | } 100 | 101 | void ORBFeatureDetector::SetMask(const cv::Mat& mat) { 102 | mask_ = mat; 103 | } 104 | 105 | void ORBFeatureDetector::SetNumberOfLevels(int number_of_levels) { 106 | if (number_of_levels != number_of_levels_) { 107 | number_of_levels_ = number_of_levels; 108 | detector_ = cv::ORB::create(max_number_of_features_, scale_factor_, 109 | number_of_levels_, edge_dist_, 0, 2, 110 | cv::ORB::HARRIS_SCORE, patch_size_); 111 | } 112 | } 113 | 114 | void ORBFeatureDetector::Refine(const cv::Mat& image, 115 | std::vector* feature_locations) { 116 | std::vector locs; 117 | locs.resize(feature_locations->size()); 118 | for (size_t i = 0; i < feature_locations->size(); i++) { 119 | locs[i] = (*feature_locations)[i].pt; 120 | } 121 | cv::TermCriteria criteria = 122 | cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 40, 0.001); 123 | cv::cornerSubPix(image, locs, cv::Size(5, 5), cv::Size(-1, -1), criteria); 124 | for (size_t i = 0; i < feature_locations->size(); i++) { 125 | (*feature_locations)[i].pt = locs[i]; 126 | } 127 | } 128 | 129 | int ORBFeatureDetector::Detect(const cv::Mat& image, 130 | std::vector* feature_locations, 131 | cv::Mat* descriptors) { 132 | image.copyTo(image_); 133 | 134 | int number_of_features = -1; 135 | 136 | feature_locations->resize(0); 137 | detector_->detectAndCompute(image, mask_, *feature_locations, *descriptors); 138 | number_of_features = feature_locations->size(); 139 | VLOG(3) << "number_of_features: " << number_of_features; 140 | 141 | if (refine_corners_) { 142 | Refine(image, feature_locations); 143 | } 144 | return number_of_features; 145 | } 146 | 147 | void ORBFeatureDetector::Detect( 148 | const cv::Mat& image, Eigen::Matrix2Xf* current_measurements, 149 | std::vector* current_feature_orientations, 150 | std::vector* current_feature_scales, 151 | FeatureDescriptoru* current_feature_descriptors) { 152 | std::vector feature_locations; 153 | cv::Mat descriptors; 154 | int number_of_features = Detect(image, &feature_locations, &descriptors); 155 | 156 | // Prepare output 157 | current_measurements->resize(2, number_of_features); 158 | for (int i = 0; i < number_of_features; ++i) { 159 | current_measurements->operator()(0, i) = feature_locations[i].pt.x; 160 | current_measurements->operator()(1, i) = feature_locations[i].pt.y; 161 | } 162 | 163 | current_feature_orientations->clear(); 164 | current_feature_orientations->reserve(number_of_features); 165 | current_feature_scales->clear(); 166 | 167 | std::vector all_scales; 168 | for (int i = 0; i < number_of_levels_; ++i) { 169 | all_scales.push_back(std::pow(scale_factor_, i)); 170 | } 171 | current_feature_scales->resize(number_of_features, -1.); 172 | current_feature_descriptors->Configure(kDescriptorLengthInBytes, 173 | number_of_features); 174 | current_feature_descriptors->Resize(number_of_features); 175 | 176 | // Assign orientation and scale 177 | for (int i = 0; i < number_of_features; ++i) { 178 | (*current_feature_orientations)[i] = feature_locations[i].angle; 179 | (*current_feature_scales)[i] = all_scales[feature_locations[i].octave]; 180 | } 181 | 182 | if (descriptors.isContinuous()) { 183 | // if continuous and row major 184 | memcpy(current_feature_descriptors->descriptor(0), descriptors.data, 185 | kDescriptorLengthInBytes * number_of_features); 186 | } else { 187 | for (int i = 0; i < number_of_features; ++i) { 188 | memcpy(current_feature_descriptors->descriptor(i), 189 | &descriptors.at(i, 0), kDescriptorLengthInBytes); 190 | } 191 | } 192 | // for (int i = 0; i < number_of_features; i++) { 193 | // cv::circle(image, 194 | // cv::Point(current_measurements->operator()(0, i), 195 | // current_measurements->operator()(1, i)), 196 | // 5, cv::Scalar(0, 255, 255)); 197 | // } 198 | // cv::imshow("features", image); 199 | // cv::waitKey(10); 200 | } 201 | -------------------------------------------------------------------------------- /src/SuperPointFeatureDetector.cc: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | * 3 | * Author: b51 4 | * Mail: b51live@gmail.com 5 | * FileName: SuperPointFeatureDetector.cc 6 | * 7 | * Created On: Fri 27 Sep 2019 08:30:41 AM UTC 8 | * Licensed under The MIT License [see LICENSE for details] 9 | * 10 | ************************************************************************/ 11 | 12 | #include "SuperPointFeatureDetector.h" 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | DEFINE_string(model, "/home/ubuntu/Models", 20 | "Path to pytorch model"); 21 | DEFINE_double(conf_thresh, 0.015, 22 | "Detector confidence threshold (default: 0.015)"); 23 | DEFINE_int32(H, 120, "Net input image height"); 24 | DEFINE_int32(W, 160, "Net input image width"); 25 | DEFINE_int32(nms_dist, 4, "Non Maximum Suppression (NMS) distance (default: 4)."); 26 | DEFINE_int32(CUDA, 0, "Default GPU device"); 27 | 28 | //Size of each output cell, Keep this fixed 29 | const int CELL = 8; 30 | 31 | SuperPointFeatureDetector::SuperPointFeatureDetector() 32 | : width_(0), 33 | height_(0), 34 | max_number_of_features_(0), 35 | conf_thresh_(0.), 36 | nms_dist_(0), 37 | is_initialized_(false), 38 | module_(nullptr) {} 39 | 40 | void SuperPointFeatureDetector::Init(int width, int height, 41 | int max_number_of_features) { 42 | CHECK_GT(width, 0); 43 | CHECK_GT(height, 0); 44 | CHECK_GT(max_number_of_features, 0); 45 | Init(width, height, max_number_of_features, FLAGS_model, 46 | FLAGS_conf_thresh, FLAGS_nms_dist); 47 | } 48 | 49 | void SuperPointFeatureDetector::Init(int width, int height, 50 | int max_number_of_features, 51 | std::string model_path, float conf_thresh, 52 | int nms_dist) { 53 | CHECK_GT(width, 0); 54 | CHECK_GT(height, 0); 55 | CHECK_GT(max_number_of_features, 0); 56 | width_ = width; 57 | height_ = height; 58 | max_number_of_features_ = max_number_of_features; 59 | model_path_ = model_path; 60 | conf_thresh_ = conf_thresh; 61 | nms_dist_ = nms_dist; 62 | 63 | image_.create(height_, width_, CV_8UC1); 64 | 65 | try { 66 | // TODO(b51): Make shared_ptr normal useable 67 | module_ = torch::jit::load(model_path.c_str()); 68 | // Load module on tx2 69 | // module_ = std::make_shared( 70 | // torch::jit::load(model_path.c_str())); 71 | module_->to(torch::Device(torch::kCUDA, FLAGS_CUDA)); 72 | } catch (const c10::Error& e) { 73 | LOG(FATAL) << "Error loading the model from " << model_path; 74 | } 75 | VLOG(3) << "SuperPoint detector parameters:"; 76 | VLOG(3) << " max number of features: " << max_number_of_features_; 77 | VLOG(3) << " model loaded: " << model_path; 78 | is_initialized_ = true; 79 | } 80 | 81 | void SuperPointFeatureDetector::NMSFast( 82 | const Eigen::Matrix3Xf& pts, int width, int height, int dist_thresh, 83 | Eigen::Matrix3Xf& filtered_pts) { 84 | Eigen::MatrixXf sorted_descend_pts; 85 | sorted_descend_pts.resize(pts.rows(), pts.cols()); 86 | 87 | // Sort with heatmap value in descending order 88 | std::vector vec; 89 | for (int i = 0; i < pts.cols(); i++) 90 | vec.push_back(pts.col(i)); 91 | std::sort(vec.begin(), vec.end(), 92 | [](Eigen::Vector3f const& v1, Eigen::Vector3f const& v2) { 93 | return v1[2] > v2[2]; 94 | }); 95 | for (int i = 0; i < pts.cols(); i++) 96 | sorted_descend_pts.col(i) = vec[i]; 97 | 98 | // Pad the border of the grid, so that we can NMS points near the border. 99 | int pad = dist_thresh; 100 | int paded_width = width + 2 * pad; 101 | int paded_height = height + 2 * pad; 102 | Eigen::MatrixXf grid = Eigen::MatrixXf::Zero(paded_width, paded_height); 103 | #pragma omp parallel for 104 | for (int i = 0; i < sorted_descend_pts.cols(); i++) { 105 | grid(std::round(sorted_descend_pts.col(i)[0] + pad), 106 | std::round(sorted_descend_pts.col(i)[1] + pad)) = 1; 107 | } 108 | 109 | // Iterate through points, highest to lowest conf, suppress neighborhood. 110 | int count = 0; 111 | int pad_2 = 2 * pad; 112 | filtered_pts = Eigen::Matrix3Xf::Zero(3, sorted_descend_pts.cols()); 113 | for (int i = 0; i < sorted_descend_pts.cols(); i++) { 114 | // Account for top and left padding. 115 | int x = std::round(sorted_descend_pts.col(i)[0] + pad); 116 | int y = std::round(sorted_descend_pts.col(i)[1] + pad); 117 | if (grid(x, y) == 1) { 118 | // Block of size(p, q), starting at (i, j) 119 | // dynamic-size block expression: matrix.block(i, j, p, q) 120 | // https://eigen.tuxfamily.org/dox/group__TutorialBlockOperations.html 121 | grid.block(x - pad, y - pad, pad_2 + 1, pad_2 + 1) = 122 | Eigen::MatrixXf::Zero(pad_2 + 1, pad_2 + 1); 123 | grid(x, y) = -1; 124 | // Ignore points along border 125 | if ((x - pad) < pad or (x - pad >= width - pad) or 126 | (y - pad) < pad or (y - pad >= height - pad)) 127 | continue; 128 | filtered_pts.col(count++) = 129 | Eigen::Vector3f(x - pad, y - pad, sorted_descend_pts.col(i)[2]); 130 | } 131 | } 132 | // resize the matrix to rows x cols while leaving old values untouched 133 | filtered_pts.conservativeResize(Eigen::NoChange, count); 134 | } 135 | 136 | void SuperPointFeatureDetector::Detect(const cv::Mat& image, 137 | Eigen::Matrix3Xf& filtered_pts, 138 | torch::Tensor& descriptors) { 139 | int H = image.rows; 140 | int W = image.cols; 141 | // convert image to float in range [0.0, 1.0] 142 | std::vector inputs; 143 | 144 | // convert cv mat to tensor and transport to cuda 145 | at::Tensor image_tensor = 146 | torch::from_blob(image.data, {1, 1, H, W}, at::kFloat); 147 | inputs.emplace_back(image_tensor.to(torch::Device(torch::kCUDA, FLAGS_CUDA))); 148 | 149 | // inference 150 | auto outputs = module_->forward(inputs).toTuple(); 151 | torch::Tensor semi = outputs->elements()[0].toTensor(); 152 | torch::Tensor coarse_desc = outputs->elements()[1].toTensor(); 153 | 154 | // Deal with feature points locations 155 | semi = semi.squeeze(); 156 | torch::Tensor dense = semi.exp(); 157 | dense = dense / (dense.sum(0) + 0.00001); 158 | torch::Tensor nodust = dense.slice(0, 0, dense.size(0) - 1); 159 | int Hc = H / CELL; 160 | int Wc = W / CELL; 161 | nodust = nodust.transpose(0, 1).transpose(1, 2); 162 | torch::Tensor heatmap = nodust.reshape({Hc, Wc, CELL, CELL}); 163 | heatmap = heatmap.transpose(1, 2); 164 | heatmap = heatmap.reshape({Hc * CELL, Wc * CELL}); 165 | torch::Tensor heatmap_cpu = heatmap.to(torch::kCPU, /*non_blocking=*/true); 166 | 167 | // Eigen::Map eig_heatmap(heatmap_cpu.data(), 168 | // heatmap.size(0), heatmap.size(1)); 169 | cv::Mat mat_heatmap(heatmap_cpu.size(0), heatmap_cpu.size(1), CV_32F, 170 | heatmap_cpu.data()); 171 | /** 172 | * Filtered to binary mat first and than get non zero value coordinates 173 | * cv::threshold(src, dst, threshold, max_binary_value, threshold_type) 174 | * threshold_type: 0, Binary 175 | * 1, Binary Inverted 176 | * 2, Threshold Truncated 177 | * 3, Threshold to Zero 178 | * 4, Threshold to Zero Inverted 179 | */ 180 | cv::Mat bin_mat; 181 | cv::threshold(mat_heatmap, bin_mat, conf_thresh_, 1, 0); 182 | bin_mat.convertTo(bin_mat, CV_8UC1); 183 | cv::Mat coordinates; 184 | cv::findNonZero(bin_mat, coordinates); 185 | 186 | Eigen::Matrix3Xf pts; 187 | pts.resize(3, coordinates.total()); 188 | #pragma omp parallel for 189 | for (size_t i = 0; i < coordinates.total(); i++) { 190 | pts.block<3, 1>(0, i) = Eigen::Vector3f( 191 | coordinates.at(i).x, coordinates.at(i).y, 192 | mat_heatmap.at(coordinates.at(i))); 193 | } 194 | // nms fast 195 | NMSFast(pts, W, H, nms_dist_, filtered_pts); 196 | 197 | VLOG(4) << "filtered points size: " << filtered_pts.cols(); 198 | int number_of_features = filtered_pts.cols(); 199 | 200 | // Process descriptor 201 | // Interpolate into descriptor map using 2D point locations. 202 | cv::Mat mat_measurements(2, number_of_features, CV_32F); 203 | for (int i = 0; i < number_of_features; i++) { 204 | // grid_sampler needs grid range between [-1., 1.] 205 | mat_measurements.at(0, i) = 206 | (filtered_pts.col(i)[0] / (float(W) / 2.)) - 1.; 207 | mat_measurements.at(1, i) = 208 | (filtered_pts.col(i)[1] / (float(H) / 2.)) - 1.; 209 | /* For debug display 210 | Eigen::Vector3f p = filtered_pts.col(i); 211 | LOG(INFO) << "x: " << p[0] << " y: " << p[1] << " value: " << p[2]; 212 | cv::circle(image, cv::Point(p[0], p[1]), 2, cv::Scalar(0, 255, 255)); 213 | 214 | cv::imwrite("key_points.jpg", image); 215 | exit(0); 216 | */ 217 | } 218 | torch::Tensor sample_pts = 219 | torch::from_blob(mat_measurements.data, {2, number_of_features}, 220 | at::kFloat) 221 | .to(torch::Device(torch::kCUDA, FLAGS_CUDA)); 222 | sample_pts = sample_pts.transpose(0, 1); 223 | sample_pts = sample_pts.view({1, 1, -1, 2}); 224 | 225 | /** 226 | * grid_sampler(at::Tensor input, at::Tensor grid, interpolation_mode, 227 | * padding_mode) 228 | * grid: range needs to be [-1., 1] 229 | * interpolation_mode: 230 | * at::native::detail 231 | * enum class GridSamplerInterpolation {Bilinear, Nearest}; 232 | * padding_mode 233 | * at::native::detail 234 | * enum class GridSamplerPadding {Zeros, Border, Reflection} 235 | */ 236 | descriptors = at::grid_sampler( 237 | coarse_desc, sample_pts.to(torch::Device(torch::kCUDA, FLAGS_CUDA)), 0, 238 | 0); 239 | descriptors = descriptors.reshape({coarse_desc.size(1), -1}); 240 | descriptors = descriptors / (descriptors.norm(c10::nullopt, 0).view({1, -1})); 241 | descriptors = descriptors.to(torch::kCPU, true); 242 | } 243 | 244 | void SuperPointFeatureDetector::Detect( 245 | const cv::Mat& image, Eigen::Matrix2Xf* current_measurements, 246 | FeatureDescriptorf* current_feature_descriptors) { 247 | image.copyTo(image_); 248 | cv::Mat input_image; 249 | double H_scale = 1.; 250 | double W_scale = 1.; 251 | // resize to net input 252 | 253 | cv::resize(image, input_image, cv::Size(FLAGS_W, FLAGS_H), 0, 0, 254 | cv::INTER_AREA); 255 | H_scale = image.rows / FLAGS_H; 256 | W_scale = image.cols / FLAGS_W; 257 | 258 | // Superpoint net only accept gray image 259 | if (input_image.channels() > 1) { 260 | cv::cvtColor(input_image, input_image, CV_BGR2GRAY); 261 | } 262 | // normalize to [0.0, 1.0] 263 | cv::Mat normed_image; 264 | input_image.convertTo(normed_image, CV_32F, 1.0 / 255, 0); 265 | 266 | Eigen::Matrix3Xf filtered_pts; 267 | torch::Tensor descriptors; 268 | Detect(normed_image, filtered_pts, descriptors); 269 | // LOG(INFO) << " descriptor 0: " << descriptors.slice(1, 0, 1); 270 | 271 | int number_of_features = filtered_pts.cols(); 272 | int descriptor_size = descriptors.size(0); 273 | current_measurements->resize(Eigen::NoChange, number_of_features); 274 | current_measurements->block(0, 0, 2, number_of_features) = 275 | filtered_pts.block(0, 0, 2, number_of_features); 276 | 277 | // restor to original size 278 | current_measurements->row(0) = current_measurements->row(0) * W_scale; 279 | current_measurements->row(1) = current_measurements->row(1) * H_scale; 280 | 281 | CHECK_EQ(number_of_features, descriptors.size(1)); 282 | 283 | cv::Mat mat_descriptors(descriptors.size(0), descriptors.size(1), CV_32F, 284 | descriptors.data()); 285 | // Make mat_descriptors width = descriptor_size (256 here) 286 | // height = number_features 287 | mat_descriptors = mat_descriptors.t(); 288 | 289 | current_feature_descriptors->Configure(descriptor_size, 290 | number_of_features); 291 | current_feature_descriptors->Resize(number_of_features); 292 | if (mat_descriptors.isContinuous()) { 293 | // if continuous 294 | memcpy(current_feature_descriptors->descriptor(0), mat_descriptors.data, 295 | descriptor_size * number_of_features * sizeof(float)); 296 | } else { 297 | for (int i = 0; i < number_of_features; ++i) { 298 | memcpy(current_feature_descriptors->descriptor(i), 299 | &mat_descriptors.at(i, 0), descriptor_size * sizeof(float)); 300 | } 301 | } 302 | // LOG(INFO) << current_feature_descriptors->Size(); 303 | // LOG(INFO) << current_feature_descriptors->DescriptorSize(); 304 | // Eigen::Map > for_display( 305 | // current_feature_descriptors->descriptor(0), 1, 256); 306 | // LOG(INFO) << for_display; 307 | } 308 | --------------------------------------------------------------------------------