diff --git a/experimental/learn_descriptors/BUILD b/experimental/learn_descriptors/BUILD index 6cdd6274..ca359cd1 100644 --- a/experimental/learn_descriptors/BUILD +++ b/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" + ] ) \ No newline at end of file diff --git a/experimental/learn_descriptors/vo.cc b/experimental/learn_descriptors/vo.cc new file mode 100644 index 00000000..121710e4 --- /dev/null +++ b/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, cv::Mat> Frontend::getKeypointsAndDescriptors( + const cv::Mat &img) const { + std::vector 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, cv::Mat>(keypoints, descriptors); +} + +std::vector Frontend::getMatches(const cv::Mat &descriptors1, + const cv::Mat &descriptors2) const { + std::vector 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 &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 &matches_out) const { + if (_matcher_type != MatcherType::KNN) { + return false; + } + std::vector> 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 &matches_out) const { + if (_matcher_type != MatcherType::FLANN) { + return false; + } + std::vector> 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 \ No newline at end of file diff --git a/experimental/learn_descriptors/vo.hh b/experimental/learn_descriptors/vo.hh new file mode 100644 index 00000000..c2e7ec94 --- /dev/null +++ b/experimental/learn_descriptors/vo.hh @@ -0,0 +1,58 @@ +#pragma once + +#include + +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, cv::Mat> getKeypointsAndDescriptors( + const cv::Mat &img) const; + std::vector getMatches(const cv::Mat &descriptors1, + const cv::Mat &descriptors2) const; + + static void drawKeypoints(const cv::Mat &img, std::vector 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 keypoints1, + const cv::Mat &img2, std::vector keypoints2, + std::vector 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 &matches_out) const; + bool getKNNMatches(const cv::Mat &descriptors1, const cv::Mat &descriptors2, + std::vector &matches_out) const; + bool getFLANNMatches(const cv::Mat &descriptors1, const cv::Mat &descriptors2, + std::vector &matches_out) const; + ExtractorType _extractor_type; + MatcherType _matcher_type; + + cv::Ptr _feature_extractor; + cv::Ptr _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 \ No newline at end of file diff --git a/experimental/learn_descriptors/vo_test.cc b/experimental/learn_descriptors/vo_test.cc new file mode 100644 index 00000000..df65627f --- /dev/null +++ b/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, cv::Mat> keypoints_descriptors_pair_1; + std::pair, cv::Mat> keypoints_descriptors_pair_2; + std::vector 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(extractor_type), + static_cast(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(extractor_type), + static_cast(matcher_type)); + } + } +} +} // namespace robot::experimental::learn_descriptors::vo