Frontend with various algorithms (SIFT, ORB, different matching algor…

…ithms) done

experimental/learn_descriptors/BUILD

@@ -35,4 +35,23 @@ cc_test(
     "@com_google_googletest//:gtest_main",
     ":symphony_lake_parser"
   ]
+)
+
+cc_library(
+  name = "vo",
+  hdrs = ["vo.hh"],
+  visibility = ["//visibility:public"],
+  srcs = ["vo.cc"],
+  deps = [
+    "@opencv//:opencv",
+  ]
+)
+
+cc_test(
+  name = "vo_test",
+  srcs = ["vo_test.cc"],  
+  deps = [
+    "@com_google_googletest//:gtest_main",
+    ":vo"
+  ]
 )

experimental/learn_descriptors/vo.cc

@@ -0,0 +1,114 @@
+#include "experimental/learn_descriptors/vo.hh"
+
+namespace robot::experimental::learn_descriptors::vo {
+VO::VO(Frontend::ExtractorType frontend_extractor, Frontend::MatcherType frontend_matcher) {
+    _frontend = Frontend(frontend_extractor, frontend_matcher);
+}
+
+Frontend::Frontend(ExtractorType frontend_algorithm, MatcherType frontend_matcher) {
+    _extractor_type = frontend_algorithm;
+    _matcher_type = frontend_matcher;
+
+    switch (_extractor_type) {
+        case ExtractorType::SIFT:
+            _feature_extractor = cv::SIFT::create();
+            break;
+        case ExtractorType::ORB:
+            _feature_extractor = cv::ORB::create();
+            break;
+        default:
+            // Error handling needed?
+            break;
+    }
+    switch (_matcher_type) {
+        case MatcherType::BRUTE_FORCE:
+            _descriptor_matcher = cv::BFMatcher::create(cv::NORM_L2);
+        case MatcherType::KNN:
+            _descriptor_matcher = cv::BFMatcher::create(cv::NORM_L2);
+            break;
+        case MatcherType::FLANN:
+            if (frontend_algorithm == ExtractorType::ORB) {
+                throw std::invalid_argument("FLANN can not be used with ORB.");
+            }
+            _descriptor_matcher = cv::FlannBasedMatcher::create();
+        default:
+            // Error handling needed?
+            break;
+    }
+}
+
+std::pair<std::vector<cv::KeyPoint>, cv::Mat> Frontend::getKeypointsAndDescriptors(
+    const cv::Mat &img) const {
+    std::vector<cv::KeyPoint> keypoints;
+    cv::Mat descriptors;
+    switch (_extractor_type) {
+        default:  // the opencv extractors have the same function signature
+            _feature_extractor->detectAndCompute(img, cv::noArray(), keypoints, descriptors);
+            break;
+    }
+    return std::pair<std::vector<cv::KeyPoint>, cv::Mat>(keypoints, descriptors);
+}
+
+std::vector<cv::DMatch> Frontend::getMatches(const cv::Mat &descriptors1,
+                                             const cv::Mat &descriptors2) const {
+    std::vector<cv::DMatch> matches;
+    switch (_matcher_type) {
+        case MatcherType::BRUTE_FORCE:
+            getBruteMatches(descriptors1, descriptors2, matches);
+            break;
+        case MatcherType::KNN:
+            getKNNMatches(descriptors1, descriptors2, matches);
+            break;
+        case MatcherType::FLANN:
+            getFLANNMatches(descriptors1, descriptors2, matches);
+        default:
+            break;
+    }
+    std::sort(matches.begin(), matches.end());
+    return matches;
+}
+
+bool Frontend::getBruteMatches(const cv::Mat &descriptors1, const cv::Mat &descriptors2,
+                               std::vector<cv::DMatch> &matches_out) const {
+    if (_matcher_type != MatcherType::BRUTE_FORCE) {
+        return false;
+    }
+    matches_out.clear();
+    _descriptor_matcher->match(descriptors1, descriptors2, matches_out);
+    return true;
+}
+
+bool Frontend::getKNNMatches(const cv::Mat &descriptors1, const cv::Mat &descriptors2,
+                             std::vector<cv::DMatch> &matches_out) const {
+    if (_matcher_type != MatcherType::KNN) {
+        return false;
+    }
+    std::vector<std::vector<cv::DMatch>> knn_matches;
+    _descriptor_matcher->knnMatch(descriptors1, descriptors2, knn_matches, 2);
+    const float ratio_thresh = 0.7f;
+    matches_out.clear();
+    for (size_t i = 0; i < knn_matches.size(); i++) {
+        if (knn_matches[i][0].distance < ratio_thresh * knn_matches[i][1].distance) {
+            matches_out.push_back(knn_matches[i][0]);
+        }
+    }
+    return true;
+}
+
+bool Frontend::getFLANNMatches(const cv::Mat &descriptors1, const cv::Mat &descriptors2,
+                               std::vector<cv::DMatch> &matches_out) const {
+    if (_matcher_type != MatcherType::FLANN) {
+        return false;
+    }
+    std::vector<std::vector<cv::DMatch>> knn_matches;
+    _descriptor_matcher->knnMatch(descriptors1, descriptors2, knn_matches, 2);
+    const float ratio_thresh = 0.7f;
+    matches_out.clear();
+    for (size_t i = 0; i < knn_matches.size(); i++) {
+        if (knn_matches[i][0].distance < ratio_thresh * knn_matches[i][1].distance) {
+            matches_out.push_back(knn_matches[i][0]);
+        }
+    }
+    return true;
+}
+}  // namespace robot::experimental::learn_descriptors::vo

experimental/learn_descriptors/vo.hh

@@ -0,0 +1,58 @@
+#pragma once
+
+#include <opencv2/opencv.hpp>
+
+namespace robot::experimental::learn_descriptors::vo {
+class Frontend {
+   public:
+    enum class ExtractorType { SIFT, ORB };
+    enum class MatcherType { BRUTE_FORCE, KNN, FLANN };
+
+    Frontend(){};
+    Frontend(ExtractorType frontend_extractor, MatcherType frontend_matcher);
+    ~Frontend(){};
+
+    ExtractorType getExtractorType() const { return _extractor_type; };
+    MatcherType getMatcherType() const { return _matcher_type; };
+
+    std::pair<std::vector<cv::KeyPoint>, cv::Mat> getKeypointsAndDescriptors(
+        const cv::Mat &img) const;
+    std::vector<cv::DMatch> getMatches(const cv::Mat &descriptors1,
+                                       const cv::Mat &descriptors2) const;
+
+    static void drawKeypoints(const cv::Mat &img, std::vector<cv::KeyPoint> keypoints,
+                              cv::Mat img_keypoints_out) {
+        cv::drawKeypoints(img, keypoints, img_keypoints_out, cv::Scalar::all(-1),
+                          cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
+    }
+    static void drawMatches(const cv::Mat &img1, std::vector<cv::KeyPoint> keypoints1,
+                            const cv::Mat &img2, std::vector<cv::KeyPoint> keypoints2,
+                            std::vector<cv::DMatch> matches, cv::Mat img_matches_out) {
+        cv::drawMatches(img1, keypoints1, img2, keypoints2, matches, img_matches_out);
+    }
+
+   private:
+    bool getBruteMatches(const cv::Mat &descriptors1, const cv::Mat &descriptors2,
+                         std::vector<cv::DMatch> &matches_out) const;
+    bool getKNNMatches(const cv::Mat &descriptors1, const cv::Mat &descriptors2,
+                       std::vector<cv::DMatch> &matches_out) const;
+    bool getFLANNMatches(const cv::Mat &descriptors1, const cv::Mat &descriptors2,
+                         std::vector<cv::DMatch> &matches_out) const;
+    ExtractorType _extractor_type;
+    MatcherType _matcher_type;
+
+    cv::Ptr<cv::Feature2D> _feature_extractor;
+    cv::Ptr<cv::DescriptorMatcher> _descriptor_matcher;
+};
+class VO {
+   public:
+    VO(Frontend::ExtractorType frontend_extractor,
+       Frontend::MatcherType frontend_matcher = Frontend::MatcherType::KNN);
+    ~VO(){};
+
+   private:
+    cv::Mat prev_image;
+
+    Frontend _frontend;
+};
+}  // namespace robot::experimental::learn_descriptors::vo

experimental/learn_descriptors/vo_test.cc

@@ -0,0 +1,56 @@
+#include "experimental/learn_descriptors/vo.hh"
+
+#include "gtest/gtest.h"
+
+namespace robot::experimental::learn_descriptors::vo {
+TEST(VIO_TEST, frontend_pipeline_sweep) {
+    int width = 640;
+    int height = 480;
+
+    cv::Mat random_image_1(height, width, CV_8UC3), random_image_2(height, width, CV_8UC3);
+    cv::randu(random_image_1, cv::Scalar::all(0), cv::Scalar::all(256));
+    cv::randu(random_image_2, cv::Scalar::all(0), cv::Scalar::all(256));
+
+    Frontend::ExtractorType extractor_types[2] = {Frontend::ExtractorType::SIFT,
+                                                  Frontend::ExtractorType::ORB};
+    Frontend::MatcherType matcher_types[3] = {Frontend::MatcherType::BRUTE_FORCE,
+                                              Frontend::MatcherType::FLANN,
+                                              Frontend::MatcherType::KNN};
+
+    Frontend frontend;
+    std::pair<std::vector<cv::KeyPoint>, cv::Mat> keypoints_descriptors_pair_1;
+    std::pair<std::vector<cv::KeyPoint>, cv::Mat> keypoints_descriptors_pair_2;
+    std::vector<cv::DMatch> matches;
+    cv::Mat img_keypoints_out_1(height, width, CV_8UC3),
+        img_keypoints_out_2(height, width, CV_8UC3), img_matches_out(height, 2 * width, CV_8UC3);
+    cv::Mat img_display_test;
+    for (Frontend::ExtractorType extractor_type : extractor_types) {
+        for (Frontend::MatcherType matcher_type : matcher_types) {
+            printf("started frontend combination: (%d, %d)\n", static_cast<int>(extractor_type),
+                   static_cast<int>(matcher_type));
+            try {
+                frontend = Frontend(extractor_type, matcher_type);
+            } catch (const std::invalid_argument &e) {
+                assert(std::string(e.what()) == "FLANN can not be used with ORB.");  // very jank...
+                continue;
+            }
+            keypoints_descriptors_pair_1 = frontend.getKeypointsAndDescriptors(random_image_1);
+            keypoints_descriptors_pair_2 = frontend.getKeypointsAndDescriptors(random_image_2);
+            matches = frontend.getMatches(keypoints_descriptors_pair_1.second,
+                                          keypoints_descriptors_pair_2.second);
+            frontend.drawKeypoints(random_image_1, keypoints_descriptors_pair_1.first,
+                                   img_keypoints_out_1);
+            frontend.drawKeypoints(random_image_2, keypoints_descriptors_pair_2.first,
+                                   img_keypoints_out_2);
+            frontend.drawMatches(random_image_1, keypoints_descriptors_pair_1.first, random_image_2,
+                                 keypoints_descriptors_pair_2.first, matches, img_matches_out);
+            cv::hconcat(img_keypoints_out_1, img_keypoints_out_2, img_display_test);
+            cv::vconcat(img_display_test, img_matches_out, img_display_test);
+            cv::imshow("Keypoints and Matches Output", img_display_test);
+            cv::waitKey(100);
+            printf("completed frontend combination: (%d, %d)\n", static_cast<int>(extractor_type),
+                   static_cast<int>(matcher_type));
+        }
+    }
+}
+}  // namespace robot::experimental::learn_descriptors::vo