Implement points tracker using ORB tracker or FAST+KLT tracker

[prashanthr05]

I had already implemented reference code in C++/Matlab. I need to find it and make it into proper classes.

[prashanthr05]

Check https://github.com/prashanthr05/kindyn-vio/blob/master/legacy/matlab/vio/%2BPerception/FeatureTracker.m

[prashanthr05]

Example C++ code for ORB Tracker,

Header file

``` cpp #ifndef ORB_FEATURE_TRACKING_H #define ORB_FEATURE_TRACKING_H #include #include /** * \ingroup Features_namespace * * Features namespace. */ namespace Features { enum class ORB_MATCHING { BRUTEFORCE_HAMMING, FLANNBASED_LSH }; struct ORBTrackerParams { ////////////////////////////////////////////////////////////// //// PARAMETERS FOR ORB FEATURE DETECTION AND DESCRIPTION //// ////////////////////////////////////////////////////////////// // maximum number of features to retain int nr_features{1000}; // Choices cv::ORB::HARRIS_SCORE and cv::ORB::FAST_SCORE // FAST is faster than HARRIS but less stable (because of edges) int score_type{cv::ORB::HARRIS_SCORE}; // image pyramid decimation ration, // greater than 1, maximum to 2, // higher value poor matching, // lower value poor scale invariance float scale_factor{1.2f}; // number of pyramid levels (I think this is somehow related to // DoG difference of Gaussians for differnet levels of blurs) int nlevels{8}; // To which level on the image pyramid // should I put the source image int first_level{0}; // theshold for border of feature detection // should roughly be equal to patch_size int edge_threshold{31}; // patch size for oriented BRIEF descriptor int patch_size{31}; // Number of pixels to sample within the patch // around a feature in order to compare // Hamming distances for defining the BRIEF descriptors // Using more than 4 is not recommended since it affects // computing hamming distances which is basically sum of xor of // pixel intensity pairs and appropriate binning int WTA_K{2}; // theshold t for comparing intensity Ip of the pixel p with // surrounding 16 pixels to check if pixel p is a corner // for p to be a coner atleast three of the neighboring // pixels must be all brighter than Ip + t // or darker than Ip - t int fast_threshold{20}; ///////////////////////////////////////////// //// PARAMETERS FOR ORB FEATURE MATCHING //// ///////////////////////////////////////////// // matching type // A warning note about FLANN based matching, it crashes if // the features of the query image is not found in current image Features::ORB_MATCHING matching_type{Features::ORB_MATCHING::BRUTEFORCE_HAMMING}; // Matching of ORB descriptors based on Hamming distances // if WTA_K was set to 3 or 4, this parameter must be set to // cv::NORM_HAMMING2 int BFMatcher_norm_type{cv::NORM_HAMMING}; // if a knnMatch with k = 1 is done using BFMatcher // then a check similar to Lowe's ratio test is done on // matching descriptors to find "metric" consistent pairs bool BFMatcher_cross_check{false}; // get k nearest neighbors for each query descriptor // matching only wih atleast 2 members int BFMatcher_knn_nr_neighbors{2}; int FLANNMatcher_knn_nr_neighbors{2}; // number of hash tables for LSH matching // lsh is called locality sensitive hashing which // hashes similar input items into same "buckets" // with high probability. Since we have similar items // in the same buckets, this can be used to search // for the nearest neighbour by reducing the high dimenisonal // input data to a lowe dimensional buckets unsigned int FLANNMatcher_lsh_table_number{12}; // size of hash key in bits unsigned int FLANNMatcher_lsh_key_size{20}; // number of bits to shift to check for neighboring buckets // 0 is regular LSH, 2 is recommended unsigned int FLANNMatcher_multi_probe_level{2}; // threshold for getting a good match from // the k nearest neighbor matches // (used if there are more than one match) float nearest_neighbor_match_ratio{0.9}; }; class ORBTracker { public: ORBTracker(); ORBTracker(const Features::ORBTrackerParams& params); void setTrackerParams(const Features::ORBTrackerParams& params); Features::ORBTrackerParams getORBTrackerParams() const; void setDatabaseQueryImagePair(const cv::Mat& database_img, const cv::Mat& query_img); void setDataBaseImage(const cv::Mat& database_img); void setQueryImageOnlyToCompare(const cv::Mat& query_img); // This function requires to independently set the database image bool getFeatureTrackedImagePair(cv::Mat& res, std::vector& database_matched, std::vector& query_matched); bool getFeatureTrackedImagePair(std::vector& database_matched, std::vector& query_matched); private: void setupTracker(const Features::ORBTrackerParams& params); void processImagePair(const bool& draw); Features::ORBTrackerParams m_params; cv::Ptr m_detector; cv::Ptr m_BFmatcher; cv::Ptr m_flannmatcher; cv::Mat m_database_img, m_query_img; std::vector m_database_matched_keypoints, m_query_matched_keypoints; cv::Mat m_img_matched; bool m_database_keypoints_computed{false}; bool m_database_descriptors_computed{false}; std::vector m_database_keypoints; cv::Mat m_database_descriptors; size_t m_matching_features_threshold{10}; }; } // namespace Features #endif ```

Source File

``` cpp #include "Features/ORBFeatureTracking.h" namespace Features { ORBTracker::ORBTracker() { setupTracker(m_params); } ORBTracker::ORBTracker(const Features::ORBTrackerParams& params) : m_params(params) { setupTracker(m_params); } void ORBTracker::setTrackerParams(const Features::ORBTrackerParams& params) { m_params = params; setupTracker(m_params); } Features::ORBTrackerParams ORBTracker::getORBTrackerParams() const { return m_params; } void ORBTracker::setupTracker(const Features::ORBTrackerParams& params) { m_detector = cv::ORB::create(params.nr_features, params.scale_factor, params.nlevels, params.edge_threshold, params.first_level, params.WTA_K, params.score_type, params.patch_size, params.fast_threshold); if (params.matching_type == Features::ORB_MATCHING::BRUTEFORCE_HAMMING) { int norm_type = params.BFMatcher_norm_type; if (params.WTA_K > 2) { norm_type = cv::NORM_HAMMING2; } m_BFmatcher = cv::makePtr(norm_type, params.BFMatcher_cross_check); } else if (params.matching_type == Features::ORB_MATCHING::FLANNBASED_LSH) { m_flannmatcher = cv::makePtr(cv::makePtr(params.FLANNMatcher_lsh_table_number, params.FLANNMatcher_lsh_key_size, params.FLANNMatcher_multi_probe_level)); } std::cout << "[Nethra::ORBFeatureTracking] Updated ORBFeatureTracker parameters." << std::endl; } void ORBTracker::setDatabaseQueryImagePair(const cv::Mat& database_img, const cv::Mat& query_img) { database_img.copyTo(m_database_img); query_img.copyTo(m_query_img); m_database_keypoints_computed = false; m_database_descriptors_computed = false; } void ORBTracker::setDataBaseImage(const cv::Mat& database_img) { database_img.copyTo(m_database_img); m_database_keypoints_computed = false; m_database_descriptors_computed = false; } void ORBTracker::setQueryImageOnlyToCompare(const cv::Mat& query_img) { query_img.copyTo(m_query_img); } void ORBTracker::processImagePair(const bool& draw) { // detect keypoints - Harris corners or FAST corners/edges if (!m_database_keypoints_computed) { m_detector->detect(m_database_img, m_database_keypoints); m_database_keypoints_computed = true; } std::vector query_keypoints; m_detector->detect(m_query_img, query_keypoints); // extract the ORB descriptors if (!m_database_descriptors_computed) { m_detector->compute(m_database_img, m_database_keypoints, m_database_descriptors); m_database_descriptors_computed = true; } cv::Mat query_descriptors; m_detector->compute(m_query_img, query_keypoints, query_descriptors); // this prevents from crashing if the matching image doesnt // have enough features for the matching to work if (query_keypoints.size() < m_matching_features_threshold) { return; } // match the descriptors and get feature correspondences std::vector< std::vector > matches; if (m_params.matching_type == Features::ORB_MATCHING::BRUTEFORCE_HAMMING) { m_BFmatcher->knnMatch(m_database_descriptors, query_descriptors, matches, m_params.BFMatcher_knn_nr_neighbors); } else if (m_params.matching_type == Features::ORB_MATCHING::FLANNBASED_LSH) { m_flannmatcher->knnMatch(m_database_descriptors, query_descriptors, matches, m_params.FLANNMatcher_knn_nr_neighbors); } // Get only good matches m_database_matched_keypoints.clear(); m_query_matched_keypoints.clear(); std::vector good_matches; if (matches.size() >= 2) { for (size_t idx = 0; idx < matches.size(); idx++) { // pick a good match by checking a ratio // similar to Lowe's test to get distinct correspondences if (matches[idx].size() >= 2) { double ratio_multiple = m_params.nearest_neighbor_match_ratio*matches[idx][1].distance; if (matches[idx][0].distance < ratio_multiple) { auto database_point = m_database_keypoints[matches[idx][0].queryIdx]; auto query_point = query_keypoints[matches[idx][0].trainIdx]; good_matches.push_back(matches[idx][0]); m_database_matched_keypoints.push_back(database_point); m_query_matched_keypoints.push_back(query_point); } } } } else { if (matches.size() != 0) { if (matches[0].size() != 0) { auto database_point = m_database_keypoints[matches[0][0].queryIdx]; auto query_point = query_keypoints[matches[0][0].trainIdx]; good_matches.push_back(matches[0][0]); m_database_matched_keypoints.push_back(database_point); m_query_matched_keypoints.push_back(query_point); } } } if (draw) { auto match_color = cv::Scalar(0, 255, 0); // green auto point_color = cv::Scalar::all(-1); // random auto draw_flag = cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS; cv::drawMatches(m_database_img, m_database_keypoints, m_query_img, query_keypoints, good_matches, m_img_matched, match_color, point_color, std::vector(), draw_flag); } } bool ORBTracker::getFeatureTrackedImagePair(cv::Mat& res, std::vector& database_matched, std::vector& query_matched) { bool draw{true}; processImagePair(draw); m_img_matched.copyTo(res); database_matched = m_database_matched_keypoints; query_matched = m_query_matched_keypoints; return true; } bool ORBTracker::getFeatureTrackedImagePair(std::vector& database_matched, std::vector& query_matched) { bool draw{false}; processImagePair(draw); database_matched = m_database_matched_keypoints; query_matched = m_query_matched_keypoints; return true; } } // namespace Features ```

Test file

``` cpp #include "Capture/RealSenseCapture.h" #include "Features/ORBFeatureTracking.h" #include #include namespace { volatile std::sig_atomic_t gSignalStatus; } void signal_handler(int signal) { gSignalStatus = signal; } int main() { // install a signal handler std::signal(SIGINT, signal_handler); bool image_pair{true}; // setup RealSense capture Capture::RealSenseCapture rs_cap; Capture::RealSenseStreamParams rs_params; rs_params.enable_ir = true; rs_params.enable_depth = true; // setup ORBTracker Features::ORBTrackerParams orb_params; orb_params.matching_type = Features::ORB_MATCHING::FLANNBASED_LSH; Features::ORBTracker orbtracker; orbtracker.setTrackerParams(orb_params); // start streaming RGB images rs_cap.setStreamParams(rs_params); rs_cap.startStream(); // fix the first image // alternately we can also set image pairs on the fly if (!image_pair) { cv::Mat bgr_frame1; rs_cap.getCurrentBGRFrame(bgr_frame1); cv::imshow("BGR Frame 1", bgr_frame1); cv::waitKey(5); // set the fixed first image orbtracker.setDataBaseImage(bgr_frame1); } cv::namedWindow("BGR Frame 1", cv::WINDOW_AUTOSIZE); cv::namedWindow("BGR Frame 2", cv::WINDOW_AUTOSIZE); cv::namedWindow("ORB Matcher", cv::WINDOW_AUTOSIZE); while (gSignalStatus != SIGINT) { cv::Mat bgr_frame1; rs_cap.getCurrentBGRFrame(bgr_frame1); cv::imshow("BGR Frame 1", bgr_frame1); cv::waitKey(5); cv::Mat bgr_frame2; rs_cap.getCurrentBGRFrame(bgr_frame2); cv::imshow("BGR Frame 2", bgr_frame2); // set the image to compare if (image_pair) { orbtracker.setDatabaseQueryImagePair(bgr_frame1, bgr_frame2); } else { orbtracker.setQueryImageOnlyToCompare(bgr_frame2); } // get the results - the actual processing step is here - if you want to time-profile it cv::Mat out; std::vector matched_keypoints1, matched_keypoints2; orbtracker.getFeatureTrackedImagePair(out, matched_keypoints1,matched_keypoints2); cv::imshow("ORB Matcher", out); cv::waitKey(5); } rs_cap.stopStream(); cv::destroyAllWindows(); return 0; } ```

[prashanthr05]

One of the state-of-the-art VINS method known as VINS-MONO implements a point feature tracker. It's an open-source code. I modified their code a bit into a PointsTracker class which is to be used by ImageProcessor class repsonsible for managing the tracking of points and lines.

The PointTracker class uses optical flow based KLT tracker to track features between two images, while rejecting outliers using RANSAC based on an essential matrix (VINS-MONO uses RANSAC based on fundamental matrix. Former uses 5-point RANSAC while the latter uses a 8-point RANSAC.). Feature detection is done using goodFeaturesToTrack detector, while detecting new features only in the regions of the image where there are no features already detected. We keep track of the features with longest frame counts, to be used for building factors.

An initial implementation was added in this commit https://github.com/prashanthr05/kindyn-vio/commit/1c810cc5ae0a4932caa592316ccd118aa19787e2.

[prashanthr05]

We may close this issue, given the preliminary implementation of the PointsTracker class to be used by configuring the ImageProcessor class.

A sample test case available in PointsTrackerTest.

[prashanthr05]

The PointsTracker class was improved in https://github.com/dic-iit/kindyn-vio/commit/bb6907128e3a172f8b49169b05b8a6b3c306a5f5 by considering bi-directional optical flow check for tracking features across consecutive frames. The video below shows the features tracked across multiple frames, where each marker is described as P<feature-id>, <frame-track-count>, where P signifies a point feature . For example P3, 10 means the point feature was assigned with the ID 3 and has been tracked for 10 consecutive frames. The color of the marker changes from blue to red depending on how long it has been tracked.

https://user-images.githubusercontent.com/6506093/127321716-7c632284-7efd-4071-99de-fa81e2ccfd29.mp4

This example is implemented in PointsTrackerTest class.

It can be noticed that the features close to the Aruco marker are tracked as expected. While a very significant feature, which is the corner on the laptop is not being tracked across images. This might be due to the reason that since it's a single point it get's rejected as an outlier in the RANSAC based elimination which uses the essential matrix as the model fit. Indirectly, this point might not be explicable within the rigid body transformation between the two camera frames, as much as those dense points over the aruco marker.