From 0b04de56488ced13dda6ffcfe9a4db79d99ec3f4 Mon Sep 17 00:00:00 2001 From: asantamaria <asantamaria@iri.upc.edu> Date: Wed, 16 May 2018 13:43:28 +0200 Subject: [PATCH] FIX: fundamental_matrix example --- src/examples/fundamental_matrix.cpp | 103 +++++++++++++++++----- src/examples/yaml/fundamental_matrix.yaml | 23 +++++ 2 files changed, 103 insertions(+), 23 deletions(-) create mode 100644 src/examples/yaml/fundamental_matrix.yaml diff --git a/src/examples/fundamental_matrix.cpp b/src/examples/fundamental_matrix.cpp index f540897..8340939 100644 --- a/src/examples/fundamental_matrix.cpp +++ b/src/examples/fundamental_matrix.cpp @@ -33,7 +33,7 @@ int main(int argc, char** argv) cv::VideoCapture capture; if (argc < 2) { - std::cout << "Please use\n\t./test_opencv <arg> " + std::cout << "Please use\n\t./fundamental_matrix <arg> " "\nwith " "\n\targ = <path/videoname.mp4> for video input " "\n\targ = 0 for camera input." << std::endl; @@ -58,23 +58,81 @@ int main(int argc, char** argv) else std::cout << "succeded" << std::endl; - // set image detector - vision_utils::DetectorParamsORBPtr det_params = std::make_shared<vision_utils::DetectorParamsORB>(); - vision_utils::DetectorBasePtr det_b_ptr = vision_utils::setupDetector("ORB", "ORB detector", det_params); - vision_utils::DetectorORBPtr det_ptr = std::static_pointer_cast<vision_utils::DetectorORB>(det_b_ptr); - - // set image descriptor - vision_utils::DescriptorParamsORBPtr desc_params = std::make_shared<vision_utils::DescriptorParamsORB>(); - vision_utils::DescriptorBasePtr des_b_ptr = vision_utils::setupDescriptor("ORB", "ORB detector", desc_params); - vision_utils::DescriptorORBPtr des_ptr = std::static_pointer_cast<vision_utils::DescriptorORB>(des_b_ptr); - - // set matcher - std::string matcher_name = "BRUTEFORCE_HAMMING_2"; - vision_utils::MatcherParamsBasePtr mat_params = std::make_shared<vision_utils::MatcherParamsBase>(matcher_name); - mat_params->match_type = vision_utils::MATCH; - vision_utils::MatcherBasePtr mat_b_ptr = vision_utils::setupMatcher(matcher_name, matcher_name, mat_params); - vision_utils::MatcherBRUTEFORCE_HAMMING_2Ptr mat_ptr = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_HAMMING_2>(mat_b_ptr); - + // YAML file with parameters + std::string path_yaml_file = "/src/examples/yaml"; + std::string vu_root = _VU_ROOT_DIR; // Root dir path + std::string yaml_file_params_vision_utils = vu_root + path_yaml_file + "/fundamental_matrix.yaml"; + + // Detector + std::string det_name = vision_utils::readYamlType(yaml_file_params_vision_utils, "detector"); + vision_utils::DetectorBasePtr det_ptr = vision_utils::setupDetector(det_name, det_name + " detector", yaml_file_params_vision_utils); + + if (det_name.compare("ORB") == 0) + det_ptr = std::static_pointer_cast<vision_utils::DetectorORB>(det_ptr); + else if (det_name.compare("FAST") == 0) + det_ptr = std::static_pointer_cast<vision_utils::DetectorFAST>(det_ptr); + else if (det_name.compare("SIFT") == 0) + det_ptr = std::static_pointer_cast<vision_utils::DetectorSIFT>(det_ptr); + else if (det_name.compare("SURF") == 0) + det_ptr = std::static_pointer_cast<vision_utils::DetectorSURF>(det_ptr); + else if (det_name.compare("BRISK") == 0) + det_ptr = std::static_pointer_cast<vision_utils::DetectorBRISK>(det_ptr); + else if (det_name.compare("MSER") == 0) + det_ptr = std::static_pointer_cast<vision_utils::DetectorMSER>(det_ptr); + else if (det_name.compare("GFTT") == 0) + det_ptr = std::static_pointer_cast<vision_utils::DetectorGFTT>(det_ptr); + else if (det_name.compare("HARRIS") == 0) + det_ptr = std::static_pointer_cast<vision_utils::DetectorHARRIS>(det_ptr); + else if (det_name.compare("SBD") == 0) + det_ptr = std::static_pointer_cast<vision_utils::DetectorSBD>(det_ptr); + else if (det_name.compare("KAZE") == 0) + det_ptr = std::static_pointer_cast<vision_utils::DetectorKAZE>(det_ptr); + else if (det_name.compare("AKAZE") == 0) + det_ptr = std::static_pointer_cast<vision_utils::DetectorAKAZE>(det_ptr); + else if (det_name.compare("AGAST") == 0) + det_ptr = std::static_pointer_cast<vision_utils::DetectorAGAST>(det_ptr); + + // Descriptor + std::string des_name = vision_utils::readYamlType(yaml_file_params_vision_utils, "descriptor"); + vision_utils::DescriptorBasePtr des_ptr = vision_utils::setupDescriptor(des_name, des_name + " descriptor", yaml_file_params_vision_utils); + + if (des_name.compare("ORB") == 0) + des_ptr = std::static_pointer_cast<vision_utils::DescriptorORB>(des_ptr); + else if (des_name.compare("SIFT") == 0) + des_ptr = std::static_pointer_cast<vision_utils::DescriptorSIFT>(des_ptr); + else if (des_name.compare("SURF") == 0) + des_ptr = std::static_pointer_cast<vision_utils::DescriptorSURF>(des_ptr); + else if (des_name.compare("BRISK") == 0) + des_ptr = std::static_pointer_cast<vision_utils::DescriptorBRISK>(des_ptr); + else if (des_name.compare("KAZE") == 0) + des_ptr = std::static_pointer_cast<vision_utils::DescriptorKAZE>(des_ptr); + else if (des_name.compare("AKAZE") == 0) + des_ptr = std::static_pointer_cast<vision_utils::DescriptorAKAZE>(des_ptr); + else if (des_name.compare("LATCH") == 0) + des_ptr = std::static_pointer_cast<vision_utils::DescriptorLATCH>(des_ptr); + else if (des_name.compare("FREAK") == 0) + des_ptr = std::static_pointer_cast<vision_utils::DescriptorFREAK>(des_ptr); + else if (des_name.compare("BRIEF") == 0) + des_ptr = std::static_pointer_cast<vision_utils::DescriptorBRIEF>(des_ptr); + else if (des_name.compare("DAISY") == 0) + des_ptr = std::static_pointer_cast<vision_utils::DescriptorDAISY>(des_ptr); + else if (des_name.compare("LUCID") == 0) + des_ptr = std::static_pointer_cast<vision_utils::DescriptorLUCID>(des_ptr); + + // Matcher + std::string mat_name = vision_utils::readYamlType(yaml_file_params_vision_utils, "matcher"); + vision_utils::MatcherBasePtr mat_ptr = vision_utils::setupMatcher(mat_name, mat_name + " matcher", yaml_file_params_vision_utils); + + if (mat_name.compare("FLANNBASED") == 0) + mat_ptr = std::static_pointer_cast<vision_utils::MatcherFLANNBASED>(mat_ptr); + if (mat_name.compare("BRUTEFORCE") == 0) + mat_ptr = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE>(mat_ptr); + if (mat_name.compare("BRUTEFORCE_L1") == 0) + mat_ptr = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_L1>(mat_ptr); + if (mat_name.compare("BRUTEFORCE_HAMMING") == 0) + mat_ptr = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_HAMMING>(mat_ptr); + if (mat_name.compare("BRUTEFORCE_HAMMING_2") == 0) + mat_ptr = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_HAMMING_2>(mat_ptr); // set and print image properties unsigned int img_width = (unsigned int)capture.get(CV_CAP_PROP_FRAME_WIDTH); @@ -91,7 +149,6 @@ int main(int argc, char** argv) cv::namedWindow("Feature tracker"); // Creates a window for display. cv::moveWindow("Feature tracker", 0, 0); - // First image KeyPointVector keypoints_1 = det_ptr->detect(image_buffer.back()); cv::Mat descriptors_1 = des_ptr->getDescriptor( image_buffer.back(), keypoints_1); @@ -117,7 +174,7 @@ int main(int argc, char** argv) { image_buffer.add(img); - cv::Mat descriptors_2 = des_ptr->getDescriptor( image_buffer.back(), keypoints_1); + cv::Mat descriptors_2 = des_ptr->getDescriptor( image_buffer.back(), keypoints_2); unsigned int max_dist = 0; unsigned int min_dist = 512; @@ -175,13 +232,13 @@ int main(int argc, char** argv) std::cout << "Matches: initial " << matches.size() << " | good " << good_matches.size() << " | inliers " << inlier_matches.size() << std::endl; - std::cout << "+++++++++++++++" << std::endl; - std::cout << keypoints_1.size() << " " << keypoints_2.size() << " " << inlier_matches.size() << " " << img_matches.size() << std::endl; + std::cout << "kp1: " << keypoints_1.size() << " kp2: " << keypoints_2.size() << " inliers:" << inlier_matches.size() << std::endl; + std::cout << "+++++++++++++++" << std::endl; // Draw RANSAC inliers cv::drawMatches(image_buffer[image_buffer.size()-2], keypoints_1, image_buffer.back(), keypoints_2, inlier_matches, img_matches, - cv::Scalar::all(-1), cv::Scalar::all(-1), std::vector<char>(), 0); //cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS ); + cv::Scalar::all(-1), cv::Scalar::all(-1), std::vector<char>(), 0); //cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS ); // Show detected matches imshow("Feature tracker", img_matches); diff --git a/src/examples/yaml/fundamental_matrix.yaml b/src/examples/yaml/fundamental_matrix.yaml new file mode 100644 index 0000000..96c8f59 --- /dev/null +++ b/src/examples/yaml/fundamental_matrix.yaml @@ -0,0 +1,23 @@ +detector: + type: "GFTT" + maxCorners: 1000 + qualityLevel: 0.015 + minDistance: 20.0 + blockSize: 15 + k: 0.04 + +descriptor: + type: "ORB" + nfeatures: 1000 + scale factor: 1.2 + nlevels: 8 + edge threshold: 31 # 16 + first level: 0 + WTA_K: 2 # See: http://docs.opencv.org/trunk/db/d95/classcv_1_1ORB.html#a180ae17d3300cf2c619aa240d9b607e5 + score type: 1 #enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 }; + patch size: 31 # 31 + +matcher: + type: "BRUTEFORCE" # BRUTEFORCE, BRUTEFORCE_HAMMING, BRUTEFORCE_HAMMING_2, BRUTEFORCE_L1, FLANNBASED + match type: 1 # Match type. MATCH = 1, KNNMATCH = 2, RADIUSMATCH = 3 + min normalized score: 0.8 \ No newline at end of file -- GitLab