diff --git a/libs/3rdparty/libgtest/googletest/src/gtest-death-test.cc b/libs/3rdparty/libgtest/googletest/src/gtest-death-test.cc index 43cbd78a..0a8309ec 100755 --- a/libs/3rdparty/libgtest/googletest/src/gtest-death-test.cc +++ b/libs/3rdparty/libgtest/googletest/src/gtest-death-test.cc @@ -32,6 +32,7 @@ #include "gtest/gtest-death-test.h" +#include #include #include "gtest/internal/gtest-port.h" diff --git a/libs/gpu/libgpu/opencl/enum.cpp b/libs/gpu/libgpu/opencl/enum.cpp index 61bc9672..429b4ff1 100755 --- a/libs/gpu/libgpu/opencl/enum.cpp +++ b/libs/gpu/libgpu/opencl/enum.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include "enum.h" @@ -32,6 +33,9 @@ bool OpenCLEnum::enumPlatforms() // Get OpenCL platform count ciErrNum = clGetPlatformIDs (0, NULL, &num_platforms); + if (ciErrNum == CL_PLATFORM_NOT_FOUND_KHR) { + return true; + } if (ciErrNum != CL_SUCCESS) { std::cerr << "clGetPlatformIDs failed: " << ocl::errorString(ciErrNum) << std::endl; return false; diff --git a/src/phg/matching/bruteforce_matcher_gpu.cpp b/src/phg/matching/bruteforce_matcher_gpu.cpp index 83795f92..9b116fc4 100644 --- a/src/phg/matching/bruteforce_matcher_gpu.cpp +++ b/src/phg/matching/bruteforce_matcher_gpu.cpp @@ -13,6 +13,41 @@ #define BF_MATCHER_GPU_VERBOSE 0 +bool phg::BruteforceMatcherGPU::isAvailable(std::string *reason, bool require_non_cpu_device) +{ + const std::vector devices = gpu::enumDevices(); + + bool has_opencl_device = false; + bool has_non_cpu_opencl_device = false; + for (const gpu::Device &device : devices) { + if (!device.supports_opencl) { + continue; + } + + has_opencl_device = true; + if (!device.is_cpu) { + has_non_cpu_opencl_device = true; + break; + } + } + + if (require_non_cpu_device ? has_non_cpu_opencl_device : has_opencl_device) { + return true; + } + + if (!reason) { + return false; + } + + if (!has_opencl_device) { + *reason = "no OpenCL platforms/devices detected; install an OpenCL ICD/runtime and verify with clinfo"; + } else { + *reason = "only CPU OpenCL devices are available; the GPU matcher benchmark needs a non-CPU OpenCL device"; + } + + return false; +} + void phg::BruteforceMatcherGPU::train(const cv::Mat &train_desc) { if (train_desc.rows < 2) { @@ -26,6 +61,11 @@ void phg::BruteforceMatcherGPU::knnMatch(const cv::Mat &query_desc, std::vector> &matches, int k) const { + std::string availability_reason; + if (!isAvailable(&availability_reason)) { + throw std::runtime_error("BruteforceMatcherGPU:: knnMatch : " + availability_reason); + } + if (!train_desc_ptr) { throw std::runtime_error("BruteforceMatcher:: knnMatch : matcher is not trained"); } @@ -37,9 +77,6 @@ void phg::BruteforceMatcherGPU::knnMatch(const cv::Mat &query_desc, std::cout << "BruteforceMatcher::knnMatch : n query desc : " << query_desc.rows << ", n train desc : " << train_desc_ptr->rows << std::endl; gpu::Device device = gpu::chooseDevice(BF_MATCHER_GPU_VERBOSE); - if (!device.supports_opencl) { - throw std::runtime_error("No OpenCL device found"); - } gpu::Context context; context.init(device.device_id_opencl); diff --git a/src/phg/matching/bruteforce_matcher_gpu.h b/src/phg/matching/bruteforce_matcher_gpu.h index 0e65c52a..d44a5545 100644 --- a/src/phg/matching/bruteforce_matcher_gpu.h +++ b/src/phg/matching/bruteforce_matcher_gpu.h @@ -6,6 +6,8 @@ namespace phg { struct BruteforceMatcherGPU : DescriptorMatcher { + static bool isAvailable(std::string *reason = nullptr, bool require_non_cpu_device = false); + void train(const cv::Mat &train_desc) override; void knnMatch(const cv::Mat &query_desc, std::vector> &matches, int k) const override; diff --git a/src/phg/matching/cl/bruteforce_matcher.cl b/src/phg/matching/cl/bruteforce_matcher.cl index e51a34ac..17d621ef 100644 --- a/src/phg/matching/cl/bruteforce_matcher.cl +++ b/src/phg/matching/cl/bruteforce_matcher.cl @@ -25,13 +25,23 @@ __kernel void bruteforce_matcher(__global const float* train, // храним два лучших сопоставления для каждого дескриптора-query: __local uint res_train_idx_local[KEYPOINTS_PER_WG * 2]; __local float res_distance2_local[KEYPOINTS_PER_WG * 2]; // храним квадраты чтобы не считать корень до самого последнего момента + __local float dist2_for_reduction[NDIM]; // заполняем текущие лучшие дистанции большими значениями if (dim_id < KEYPOINTS_PER_WG * 2) { res_distance2_local[dim_id] = FLT_MAX; // полагаемся на то что res_distance2_local размера KEYPOINTS_PER_WG*2==4*2 0) { if (dim_id < step) { - // TODO + dist2_for_reduction[dim_id] += dist2_for_reduction[dim_id + step]; } barrier(CLK_LOCAL_MEM_FENCE); step /= 2; @@ -63,13 +75,17 @@ __kernel void bruteforce_matcher(__global const float* train, #define SECOND_BEST_INDEX 1 // пытаемся улучшить самое лучшее сопоставление для локального дескриптора - if (dist2 <= res_distance2_local[query_local_i * 2 + BEST_INDEX]) { + if (dist2 < res_distance2_local[query_local_i * 2 + BEST_INDEX]) { // не забываем что прошлое лучшее сопоставление теперь стало вторым по лучшевизне (на данный момент) res_distance2_local[query_local_i * 2 + SECOND_BEST_INDEX] = res_distance2_local[query_local_i * 2 + BEST_INDEX]; res_train_idx_local[query_local_i * 2 + SECOND_BEST_INDEX] = res_train_idx_local[query_local_i * 2 + BEST_INDEX]; // TODO заменяем нашим (dist2, train_idx) самое лучшее сопоставление для локального дескриптора - } else if (dist2 <= res_distance2_local[query_local_i * 2 + SECOND_BEST_INDEX]) { // может мы улучшили хотя бы второе по лучшевизне сопоставление? + res_distance2_local[query_local_i * 2 + BEST_INDEX] = dist2; + res_train_idx_local[query_local_i * 2 + BEST_INDEX] = train_idx; + } else if (dist2 < res_distance2_local[query_local_i * 2 + SECOND_BEST_INDEX]) { // может мы улучшили хотя бы второе по лучшевизне сопоставление? // TODO заменяем второе по лучшевизне сопоставление для локального дескриптора + res_distance2_local[query_local_i * 2 + SECOND_BEST_INDEX] = dist2; + res_train_idx_local[query_local_i * 2 + SECOND_BEST_INDEX] = train_idx; } } } @@ -82,9 +98,9 @@ __kernel void bruteforce_matcher(__global const float* train, int query_id = query_id0 + query_local_i; if (query_id < n_query_desc) { - res_train_idx[query_id * 2 + k] = // TODO - res_query_idx[query_id * 2 + k] = // TODO хм, не масло масленное ли? - res_distance [query_id * 2 + k] = // TODO не забудьте извлечь корень + res_train_idx[query_id * 2 + k] = res_train_idx_local[query_local_i * 2 + k]; + res_query_idx[query_id * 2 + k] = query_id; + res_distance [query_id * 2 + k] = sqrt(res_distance2_local[query_local_i * 2 + k]); } } } diff --git a/src/phg/matching/descriptor_matcher.cpp b/src/phg/matching/descriptor_matcher.cpp index f4bcd871..62f8d4f2 100644 --- a/src/phg/matching/descriptor_matcher.cpp +++ b/src/phg/matching/descriptor_matcher.cpp @@ -4,11 +4,20 @@ #include "flann_factory.h" void phg::DescriptorMatcher::filterMatchesRatioTest(const std::vector> &matches, - std::vector &filtered_matches) + std::vector &filtered_matches) { filtered_matches.clear(); - - throw std::runtime_error("not implemented yet"); + filtered_matches.reserve(matches.size()); + for (const std::vector &knn_match : matches) { + if (knn_match.size() < 2) { + continue; + } + const cv::DMatch &best = knn_match[0]; + const cv::DMatch &second = knn_match[1]; + if (constexpr float ratio_thresh = 0.75f; best.distance < ratio_thresh * second.distance) { + filtered_matches.push_back(best); + } + } } @@ -19,8 +28,8 @@ void phg::DescriptorMatcher::filterMatchesClusters(const std::vector { filtered_matches.clear(); - const size_t total_neighbours = 5; // total number of neighbours to test (including candidate) - const size_t consistent_matches = 3; // minimum number of consistent matches (including candidate) + const size_t total_neighbours = 7; // total number of neighbours to test (including candidate) + const size_t consistent_matches = 4; // minimum number of consistent matches (including candidate) const float radius_limit_scale = 2.f; // limit search radius by scaled median const int n_matches = matches.size(); @@ -32,45 +41,75 @@ void phg::DescriptorMatcher::filterMatchesClusters(const std::vector cv::Mat points_query(n_matches, 2, CV_32FC1); cv::Mat points_train(n_matches, 2, CV_32FC1); for (int i = 0; i < n_matches; ++i) { - points_query.at(i) = keypoints_query[matches[i].queryIdx].pt; - points_train.at(i) = keypoints_train[matches[i].trainIdx].pt; + const cv::Point2f pt_query = keypoints_query[matches[i].queryIdx].pt; + const cv::Point2f pt_train = keypoints_train[matches[i].trainIdx].pt; + + points_query.at(i, 0) = pt_query.x; + points_query.at(i, 1) = pt_query.y; + points_train.at(i, 0) = pt_train.x; + points_train.at(i, 1) = pt_train.y; } -// -// // размерность всего 2, так что точное KD-дерево -// std::shared_ptr index_params = flannKdTreeIndexParams(TODO); -// std::shared_ptr search_params = flannKsTreeSearchParams(TODO); -// -// std::shared_ptr index_query = flannKdTreeIndex(points_query, index_params); -// std::shared_ptr index_train = flannKdTreeIndex(points_train, index_params); -// -// // для каждой точки найти total neighbors ближайших соседей -// cv::Mat indices_query(n_matches, total_neighbours, CV_32SC1); -// cv::Mat distances2_query(n_matches, total_neighbours, CV_32FC1); -// cv::Mat indices_train(n_matches, total_neighbours, CV_32SC1); -// cv::Mat distances2_train(n_matches, total_neighbours, CV_32FC1); -// -// index_query->knnSearch(points_query, indices_query, distances2_query, total_neighbours, *search_params); -// index_train->knnSearch(points_train, indices_train, distances2_train, total_neighbours, *search_params); -// -// // оценить радиус поиска для каждой картинки -// // NB: radius2_query, radius2_train: квадраты радиуса! -// float radius2_query, radius2_train; -// { -// std::vector max_dists2_query(n_matches); -// std::vector max_dists2_train(n_matches); -// for (int i = 0; i < n_matches; ++i) { -// max_dists2_query[i] = distances2_query.at(i, total_neighbours - 1); -// max_dists2_train[i] = distances2_train.at(i, total_neighbours - 1); -// } -// -// int median_pos = n_matches / 2; -// std::nth_element(max_dists2_query.begin(), max_dists2_query.begin() + median_pos, max_dists2_query.end()); -// std::nth_element(max_dists2_train.begin(), max_dists2_train.begin() + median_pos, max_dists2_train.end()); -// -// radius2_query = max_dists2_query[median_pos] * radius_limit_scale * radius_limit_scale; -// radius2_train = max_dists2_train[median_pos] * radius_limit_scale * radius_limit_scale; -// } -// + std::shared_ptr index_params = flannKdTreeIndexParams(1); + std::shared_ptr search_params = flannKsTreeSearchParams(128); + + std::shared_ptr index_query = flannKdTreeIndex(points_query, index_params); + std::shared_ptr index_train = flannKdTreeIndex(points_train, index_params); + + // для каждой точки найти total neighbors ближайших соседей + cv::Mat indices_query(n_matches, total_neighbours, CV_32SC1); + cv::Mat distances2_query(n_matches, total_neighbours, CV_32FC1); + cv::Mat indices_train(n_matches, total_neighbours, CV_32SC1); + cv::Mat distances2_train(n_matches, total_neighbours, CV_32FC1); + + index_query->knnSearch(points_query, indices_query, distances2_query, total_neighbours, *search_params); + index_train->knnSearch(points_train, indices_train, distances2_train, total_neighbours, *search_params); + + // оценить радиус поиска для каждой картинки + // NB: radius2_query, radius2_train: квадраты радиуса! + float radius2_query, radius2_train; + { + std::vector max_dists2_query(n_matches); + std::vector max_dists2_train(n_matches); + for (int i = 0; i < n_matches; ++i) { + max_dists2_query[i] = distances2_query.at(i, total_neighbours - 1); + max_dists2_train[i] = distances2_train.at(i, total_neighbours - 1); + } + + int median_pos = n_matches / 2; + std::nth_element(max_dists2_query.begin(), max_dists2_query.begin() + median_pos, max_dists2_query.end()); + std::nth_element(max_dists2_train.begin(), max_dists2_train.begin() + median_pos, max_dists2_train.end()); + + radius2_query = max_dists2_query[median_pos] * radius_limit_scale * radius_limit_scale; + radius2_train = max_dists2_train[median_pos] * radius_limit_scale * radius_limit_scale; + } + // метч остается, если левое и правое множества первых total_neighbors соседей в радиусах поиска(radius2_query, radius2_train) имеют как минимум consistent_matches общих элементов -// // TODO заполнить filtered_matches + filtered_matches.reserve(matches.size()); + for (int i = 0; i < n_matches; ++i) { + std::vector neigh_query; + std::vector neigh_train; + + neigh_query.reserve(total_neighbours); + neigh_train.reserve(total_neighbours); + + for (size_t j = 0; j < total_neighbours; ++j) { + if (distances2_query.at(i, j) <= radius2_query) { + neigh_query.push_back(indices_query.at(i, j)); + } + if (distances2_train.at(i, j) <= radius2_train) { + neigh_train.push_back(indices_train.at(i, j)); + } + } + + int n_consistent = 0; + for (int idx_q : neigh_query) { + if (std::find(neigh_train.begin(), neigh_train.end(), idx_q) != neigh_train.end()) { + ++n_consistent; + } + } + + if (n_consistent >= static_cast(consistent_matches)) { + filtered_matches.push_back(matches[i]); + } + } } diff --git a/src/phg/matching/flann_matcher.cpp b/src/phg/matching/flann_matcher.cpp index 9e9f5180..725b550a 100644 --- a/src/phg/matching/flann_matcher.cpp +++ b/src/phg/matching/flann_matcher.cpp @@ -6,8 +6,8 @@ phg::FlannMatcher::FlannMatcher() { // параметры для приближенного поиска -// index_params = flannKdTreeIndexParams(TODO); -// search_params = flannKsTreeSearchParams(TODO); + index_params = flannKdTreeIndexParams(4); + search_params = flannKsTreeSearchParams(32); } void phg::FlannMatcher::train(const cv::Mat &train_desc) @@ -17,5 +17,33 @@ void phg::FlannMatcher::train(const cv::Mat &train_desc) void phg::FlannMatcher::knnMatch(const cv::Mat &query_desc, std::vector> &matches, int k) const { - throw std::runtime_error("not implemented yet"); + if (!flann_index) { + throw std::runtime_error("FlannMatcher:: knnMatch : matcher is not trained"); + } + if (k <= 0) { + throw std::runtime_error("FlannMatcher:: knnMatch : k must be positive"); + } + + if (query_desc.empty()) { + matches.clear(); + return; + } + + cv::Mat indices(query_desc.rows, k, CV_32SC1); + cv::Mat distances2(query_desc.rows, k, CV_32FC1); + flann_index->knnSearch(query_desc, indices, distances2, k, *search_params); + matches.resize(query_desc.rows); + for (int qi = 0; qi < query_desc.rows; qi++) { + std::vector &dst = matches[qi]; + dst.clear(); + dst.reserve(k); + for (int ki = 0; ki < k; ki++) { + cv::DMatch match; + match.imgIdx = 0; + match.queryIdx = qi; + match.trainIdx = indices.at(qi, ki); + match.distance = std::sqrt(distances2.at(qi, ki)); + dst.push_back(match); + } + } } diff --git a/src/phg/sfm/homography.cpp b/src/phg/sfm/homography.cpp index 5cbc780c..1ab6349c 100644 --- a/src/phg/sfm/homography.cpp +++ b/src/phg/sfm/homography.cpp @@ -1,10 +1,14 @@ #include "homography.h" #include +#include +#include #include namespace { + constexpr double kPi = 3.14159265358979323846; + // источник: https://e-maxx.ru/algo/linear_systems_gauss // очень важно при выполнении метода гаусса использовать выбор опорного элемента: об этом можно почитать в источнике кода // или на вики: https://en.wikipedia.org/wiki/Pivot_element @@ -57,6 +61,105 @@ namespace { } // см. Hartley, Zisserman: Multiple View Geometry in Computer Vision. Second Edition 4.1, 4.1.2 + cv::Mat estimateHomographyLeastSquares(const std::vector &points_lhs, + const std::vector &points_rhs) + { + if (points_lhs.size() != points_rhs.size()) { + throw std::runtime_error("estimateHomographyLeastSquares: points_lhs.size() != points_rhs.size()"); + } + if (points_lhs.size() < 4) { + throw std::runtime_error("estimateHomographyLeastSquares: too few points"); + } + + const int n_points = static_cast(points_lhs.size()); + cv::Mat A(2 * n_points, 8, CV_64FC1); + cv::Mat b(2 * n_points, 1, CV_64FC1); + + for (int i = 0; i < n_points; ++i) { + const double x0 = points_lhs[i].x; + const double y0 = points_lhs[i].y; + const double x1 = points_rhs[i].x; + const double y1 = points_rhs[i].y; + + A.at(2 * i + 0, 0) = x0; + A.at(2 * i + 0, 1) = y0; + A.at(2 * i + 0, 2) = 1.0; + A.at(2 * i + 0, 3) = 0.0; + A.at(2 * i + 0, 4) = 0.0; + A.at(2 * i + 0, 5) = 0.0; + A.at(2 * i + 0, 6) = -x0 * x1; + A.at(2 * i + 0, 7) = -y0 * x1; + b.at(2 * i + 0, 0) = x1; + + A.at(2 * i + 1, 0) = 0.0; + A.at(2 * i + 1, 1) = 0.0; + A.at(2 * i + 1, 2) = 0.0; + A.at(2 * i + 1, 3) = x0; + A.at(2 * i + 1, 4) = y0; + A.at(2 * i + 1, 5) = 1.0; + A.at(2 * i + 1, 6) = -x0 * y1; + A.at(2 * i + 1, 7) = -y0 * y1; + b.at(2 * i + 1, 0) = y1; + } + + cv::Mat h; + if (!cv::solve(A, b, h, cv::DECOMP_SVD)) { + throw std::runtime_error("estimateHomographyLeastSquares: cv::solve failed"); + } + + cv::Mat H(3, 3, CV_64FC1); + H.at(0, 0) = h.at(0, 0); + H.at(0, 1) = h.at(1, 0); + H.at(0, 2) = h.at(2, 0); + H.at(1, 0) = h.at(3, 0); + H.at(1, 1) = h.at(4, 0); + H.at(1, 2) = h.at(5, 0); + H.at(2, 0) = h.at(6, 0); + H.at(2, 1) = h.at(7, 0); + H.at(2, 2) = 1.0; + return H; + } + + double logBinomialCoeff(int n, int k) + { + if (k < 0 || k > n) { + return -std::numeric_limits::infinity(); + } + return std::lgamma(n + 1.0) - std::lgamma(k + 1.0) - std::lgamma(n - k + 1.0); + } + + double estimateBackgroundArea(const std::vector &points) + { + float min_x = std::numeric_limits::max(); + float min_y = std::numeric_limits::max(); + float max_x = std::numeric_limits::lowest(); + float max_y = std::numeric_limits::lowest(); + + for (const cv::Point2f &pt : points) { + min_x = std::min(min_x, pt.x); + min_y = std::min(min_y, pt.y); + max_x = std::max(max_x, pt.x); + max_y = std::max(max_y, pt.y); + } + + const double width = std::max(1.0, double(max_x - min_x + 1.0f)); + const double height = std::max(1.0, double(max_y - min_y + 1.0f)); + return width * height; + } + + double computeLogNFA(const std::vector &sorted_errors, + int k, + int sample_size, + double alpha0) + { + const double eps = std::max(1e-12, sorted_errors[k - 1]); + const double log_p = std::min(0.0, 2.0 * std::log(eps) + std::log(alpha0)); + return std::log(double(sorted_errors.size() - sample_size)) + + logBinomialCoeff(static_cast(sorted_errors.size()), k) + + logBinomialCoeff(k, sample_size) + + (k - sample_size) * log_p; + } + cv::Mat estimateHomography4Points(const cv::Point2f &l0, const cv::Point2f &l1, const cv::Point2f &l2, const cv::Point2f &l3, const cv::Point2f &r0, const cv::Point2f &r1, @@ -84,8 +187,8 @@ namespace { double w1 = ws1[i]; // 8 elements of matrix + free term as needed by gauss routine -// A.push_back({TODO}); -// A.push_back({TODO}); + A.push_back({x0, y0, w0, 0.0, 0.0, 0.0, -x0 * x1 / w1, -y0 * x1 / w1, x1 / w1}); + A.push_back({0.0, 0.0, 0.0, x0, y0, w0, -x0 * y1 / w1, -y0 * y1 / w1, y1 / w1}); } int res = gauss(A, H); @@ -98,16 +201,7 @@ namespace { } else if (res == std::numeric_limits::max()) { - std::cerr << "gauss: infinitely many solutions found" << std::endl; - std::cerr << "gauss: xs0: "; - for (int i = 0; i < 4; ++i) { - std::cerr << xs0[i] << ", "; - } - std::cerr << "\ngauss: ys0: "; - for (int i = 0; i < 4; ++i) { - std::cerr << ys0[i] << ", "; - } - std::cerr << std::endl; + throw std::runtime_error("gauss: degenerate 4-point sample"); } else { @@ -156,11 +250,68 @@ namespace { } } + int updateRequiredTrials(int current_limit, int n_matches, int n_inliers, int sample_size) + { + const int min_trials = 128; + + if (n_inliers < sample_size) { + return current_limit; + } + + const double confidence = 0.999; + const double inlier_ratio = std::clamp(double(n_inliers) / double(n_matches), 1e-12, 1.0 - 1e-12); + const double all_inliers_prob = std::clamp(std::pow(inlier_ratio, sample_size), 1e-12, 1.0 - 1e-12); + const int required = int(std::ceil(std::log(1.0 - confidence) / std::log(1.0 - all_inliers_prob))); + return std::max(min_trials, std::min(current_limit, required)); + } + + bool evaluateHomographyAContrario(const cv::Mat &H, + const std::vector &points_lhs, + const std::vector &points_rhs, + int sample_size, + double alpha0, + std::vector> &residuals_with_idx, + std::vector &sorted_errors, + double &best_log_nfa, + int &best_k) + { + const int n_matches = static_cast(points_lhs.size()); + for (int i = 0; i < n_matches; ++i) { + const cv::Point2d proj = phg::transformPoint(points_lhs[i], H); + const double err = cv::norm(proj - cv::Point2d(points_rhs[i])); + residuals_with_idx[i] = {err, i}; + } + + std::sort(residuals_with_idx.begin(), residuals_with_idx.end(), + [](const std::pair &lhs, const std::pair &rhs) { + return lhs.first < rhs.first; + }); + + for (int i = 0; i < n_matches; ++i) { + sorted_errors[i] = residuals_with_idx[i].first; + } + + best_log_nfa = std::numeric_limits::infinity(); + best_k = -1; + for (int k = sample_size + 1; k <= n_matches; ++k) { + const double log_nfa = computeLogNFA(sorted_errors, k, sample_size, alpha0); + if (log_nfa < best_log_nfa) { + best_log_nfa = log_nfa; + best_k = k; + } + } + + return best_k >= sample_size + 1 && std::isfinite(best_log_nfa); + } + cv::Mat estimateHomographyRANSAC(const std::vector &points_lhs, const std::vector &points_rhs) { if (points_lhs.size() != points_rhs.size()) { throw std::runtime_error("findHomography: points_lhs.size() != points_rhs.size()"); } + if (points_lhs.size() < 4) { + throw std::runtime_error("findHomography: too few correspondences"); + } // TODO Дополнительный балл, если вместо обычной версии будет использована модификация a-contrario RANSAC // * [1] Automatic Homographic Registration of a Pair of Images, with A Contrario Elimination of Outliers. (Lionel Moisan, Pierre Moulon, Pascal Monasse) @@ -168,57 +319,116 @@ namespace { // * (простое описание для понимания) // * [3] http://ikrisoft.blogspot.com/2015/01/ransac-with-contrario-approach.html -// const int n_matches = points_lhs.size(); -// -// // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters -// const int n_trials = TODO; -// -// const int n_samples = TODO; -// uint64_t seed = 1; -// const double reprojection_error_threshold_px = 2; -// -// int best_support = 0; -// cv::Mat best_H; -// -// std::vector sample; -// for (int i_trial = 0; i_trial < n_trials; ++i_trial) { -// randomSample(sample, n_matches, n_samples, &seed); -// -// cv::Mat H = estimateHomography4Points(points_lhs[sample[0]], points_lhs[sample[1]], points_lhs[sample[2]], points_lhs[sample[3]], -// points_rhs[sample[0]], points_rhs[sample[1]], points_rhs[sample[2]], points_rhs[sample[3]]); -// -// int support = 0; -// for (int i_point = 0; i_point < n_matches; ++i_point) { -// try { -// cv::Point2d proj = phg::transformPoint(points_lhs[i_point], H); -// if (cv::norm(proj - cv::Point2d(points_rhs[i_point])) < reprojection_error_threshold_px) { -// ++support; -// } -// } catch (const std::exception &e) -// { -// std::cerr << e.what() << std::endl; -// } -// } -// -// if (support > best_support) { -// best_support = support; -// best_H = H; -// -// std::cout << "estimateHomographyRANSAC : support: " << best_support << "/" << n_matches << std::endl; -// -// if (best_support == n_matches) { -// break; -// } -// } -// } -// -// std::cout << "estimateHomographyRANSAC : best support: " << best_support << "/" << n_matches << std::endl; -// -// if (best_support == 0) { -// throw std::runtime_error("estimateHomographyRANSAC : failed to estimate homography"); -// } -// -// return best_H; + const int n_matches = static_cast(points_lhs.size()); + const int max_trials = 5000; + const int n_samples = 4; + const double alpha0 = kPi / std::max(4.0 * kPi, estimateBackgroundArea(points_rhs)); + + double best_log_nfa = std::numeric_limits::infinity(); + int best_k = 0; + cv::Mat best_H; + std::vector best_inliers; + + uint64_t seed = 1; + std::vector sample; + std::vector> residuals_with_idx(n_matches); + std::vector sorted_errors(n_matches); + int required_trials = max_trials; + + for (int i_trial = 0; i_trial < required_trials; ++i_trial) { + randomSample(sample, n_matches, n_samples, &seed); + + cv::Mat H; + try { + H = estimateHomography4Points(points_lhs[sample[0]], points_lhs[sample[1]], points_lhs[sample[2]], points_lhs[sample[3]], + points_rhs[sample[0]], points_rhs[sample[1]], points_rhs[sample[2]], points_rhs[sample[3]]); + } catch (const std::exception &) { + continue; + } + + double model_log_nfa; + int model_k; + try { + if (!evaluateHomographyAContrario(H, points_lhs, points_rhs, n_samples, alpha0, + residuals_with_idx, sorted_errors, model_log_nfa, model_k)) { + continue; + } + } catch (const std::exception &) { + continue; + } + + if (model_log_nfa < best_log_nfa || + (model_log_nfa == best_log_nfa && model_k > best_k)) { + best_log_nfa = model_log_nfa; + best_k = model_k; + best_H = H; + best_inliers.clear(); + best_inliers.reserve(model_k); + for (int i = 0; i < model_k; ++i) { + best_inliers.push_back(residuals_with_idx[i].second); + } + required_trials = updateRequiredTrials(required_trials, n_matches, model_k, n_samples); + } + if (best_k == n_matches) { + break; + } + } + + if (best_H.empty() || best_k < n_samples + 1) { + throw std::runtime_error("estimateHomographyRANSAC : failed to estimate homography"); + } + + cv::Mat refined_H = best_H; + std::vector refined_inliers = best_inliers; + double refined_log_nfa = best_log_nfa; + + for (int i_refine = 0; i_refine < 2; ++i_refine) { + if (refined_inliers.size() < n_samples) { + break; + } + + std::vector inliers_lhs; + std::vector inliers_rhs; + inliers_lhs.reserve(refined_inliers.size()); + inliers_rhs.reserve(refined_inliers.size()); + for (int idx : refined_inliers) { + inliers_lhs.push_back(points_lhs[idx]); + inliers_rhs.push_back(points_rhs[idx]); + } + + cv::Mat candidate_H; + try { + candidate_H = estimateHomographyLeastSquares(inliers_lhs, inliers_rhs); + } catch (const std::exception &) { + break; + } + + double candidate_log_nfa; + int candidate_k; + try { + if (!evaluateHomographyAContrario(candidate_H, points_lhs, points_rhs, n_samples, alpha0, + residuals_with_idx, sorted_errors, candidate_log_nfa, candidate_k)) { + break; + } + } catch (const std::exception &) { + break; + } + + if (candidate_log_nfa >= refined_log_nfa && + candidate_k <= static_cast(refined_inliers.size())) { + break; + } + + refined_H = candidate_H; + refined_log_nfa = candidate_log_nfa; + refined_inliers.clear(); + refined_inliers.reserve(candidate_k); + for (int i = 0; i < candidate_k; ++i) { + refined_inliers.push_back(residuals_with_idx[i].second); + } + } + + return refined_H; } } @@ -238,7 +448,22 @@ cv::Mat phg::findHomographyCV(const std::vector &points_lhs, const // таким преобразованием внутри занимается функции cv::perspectiveTransform и cv::warpPerspective cv::Point2d phg::transformPoint(const cv::Point2d &pt, const cv::Mat &T) { - throw std::runtime_error("not implemented yet"); + if (T.rows != 3 || T.cols != 3) { + throw std::runtime_error("transformPoint: expected 3x3 matrix"); + } + + const double x = pt.x; + const double y = pt.y; + + const double tx = T.at(0, 0) * x + T.at(0, 1) * y + T.at(0, 2); + const double ty = T.at(1, 0) * x + T.at(1, 1) * y + T.at(1, 2); + const double tw = T.at(2, 0) * x + T.at(2, 1) * y + T.at(2, 2); + + if (std::abs(tw) < 1e-12) { + throw std::runtime_error("transformPoint: homogeneous coordinate is too small"); + } + + return cv::Point2d(tx / tw, ty / tw); } cv::Point2d phg::transformPointCV(const cv::Point2d &pt, const cv::Mat &T) { diff --git a/src/phg/sfm/panorama_stitcher.cpp b/src/phg/sfm/panorama_stitcher.cpp index 8d76939b..9aeb6340 100644 --- a/src/phg/sfm/panorama_stitcher.cpp +++ b/src/phg/sfm/panorama_stitcher.cpp @@ -15,15 +15,55 @@ cv::Mat phg::stitchPanorama(const std::vector &imgs, std::function &homography_builder) { const int n_images = imgs.size(); + if (parent.size() != imgs.size()) { + throw std::runtime_error("stitchPanorama: parent.size() != imgs.size()"); + } // склеивание панорамы происходит через приклеивание всех картинок к корню, некоторые приклеиваются не напрямую, а через цепочку других картинок // вектор гомографий, для каждой картинки описывает преобразование до корня std::vector Hs(n_images); { - // здесь надо посчитать вектор Hs - // при этом можно обойтись n_images - 1 вызовами функтора homography_builder - throw std::runtime_error("not implemented yet"); + std::vector edge_Hs(n_images); + std::vector edge_ready(n_images, 0); + std::vector ready(n_images, 0); + std::vector in_stack(n_images, 0); + + std::function build_to_root = [&](int i) { + if (ready[i]) { + return; + } + if (in_stack[i]) { + throw std::runtime_error("stitchPanorama: cycle in parent graph"); + } + + in_stack[i] = 1; + + const int p = parent[i]; + if (p == -1) { + Hs[i] = cv::Mat::eye(3, 3, CV_64FC1); + } else { + if (p < 0 || p >= n_images) { + throw std::runtime_error("stitchPanorama: invalid parent index"); + } + + build_to_root(p); + + if (!edge_ready[i]) { + edge_Hs[i] = homography_builder(imgs[i], imgs[p]); + edge_ready[i] = 1; + } + + Hs[i] = Hs[p] * edge_Hs[i]; + } + + ready[i] = 1; + in_stack[i] = 0; + }; + + for (int i = 0; i < n_images; ++i) { + build_to_root(i); + } } bbox2 bbox; diff --git a/src/phg/sift/sift.cpp b/src/phg/sift/sift.cpp index 72047711..34cbd81a 100755 --- a/src/phg/sift/sift.cpp +++ b/src/phg/sift/sift.cpp @@ -107,21 +107,17 @@ std::vector phg::buildOctaves(const cv::Mat& img, const phg:: // для простоты в каждой октаве будем каждый раз блюрить базовую картинку с полной сигмой // можно подумать, как сделать эффективнее - для построения n+1 слоя доблюревать уже поблюренный n-ый слой, так чтобы в итоге получилась такая же сигма // это будет немного быстрее, тк нужно более маленькое ядро свертки на каждый шаг + double k = std::pow(2, (double) 1 / s); + double sigma_prev = sigma0; for (int i = 1; i < n_layers; i++) { - // TODO double sigma_layer = sigma0 * корень из двух нужной степени, чтобы при i==s получали удвоение базового блюра; - // // вычтем sigma0 чтобы размыть ровно до нужной суммарной сигмы - // TODO sigma_layer = ... (вычитаем как в sigma base); - // cv::GaussianBlur(oct.layers[0], oct.layers[i], cv::Size(), sigma_layer, sigma_layer); + double sigma_layer = std::sqrt(sigma_prev * sigma_prev * k * k - sigma_prev * sigma_prev); + cv::GaussianBlur(oct.layers[i-1], oct.layers[i], cv::Size(), sigma_layer, sigma_layer); + sigma_prev *= k; } // подготавливаем базовый слой для следующей октавы if (o + 1 < n_octaves) { - // используется в opencv, формула для пересчета ключевых точек: pt_upscaled = 2^o * pt_downscaled - // TODO cv::resize(даунскейлим текущий слой в два раза, без интерполяции, просто сабсепмлинг); - - // можно использовать и downsample2x_avg(oct.layers[s]), это позволяет потом заапскейлить слои обратно до оригинального разрешения без сдвига - // но потребуется везде изменить формулу для пересчета ключевых точек: pt_upscaled = (pt_downscaled + 0.5) * 2^o - 0.5 - + cv::resize(oct.layers[s], base, cv::Size(oct.layers[s].cols/2, oct.layers[s].rows/2), 0, 0, cv::INTER_NEAREST); if (verbose_level) std::cout << "new octave base size: " << base.size().width << std::endl; } @@ -133,12 +129,12 @@ std::vector phg::buildOctaves(const cv::Mat& img, const phg:: std::vector phg::buildDoG(const std::vector& octaves, const phg::SIFTParams& p, int verbose_level) { std::vector dog(octaves.size()); - for (size_t o = 0; o < octaves.size(); o++) { const phg::SIFT::Octave& octave = octaves[o]; dog[o].layers.resize(octave.layers.size() - 1); - - // TODO каждый слой дога это разница n+1 и n-й гауссианы + for (size_t i = 0; i < dog[o].layers.size(); i++) { + dog[o].layers[i] = octave.layers[i+1] - octave.layers[i]; + } } return dog; @@ -207,17 +203,28 @@ std::vector phg::findScaleSpaceExtrema(const std::vector phg::findScaleSpaceExtrema(const std::vector(yi, xi) - pL.at(yi, xi)) * 0.5f; // гессиан - float dxx, dxy, dyy, dxs, dys, dss; -// float dxx = cL.at(yi, xi + 1) + cL.at(yi, xi - 1) - 2.f * resp_center; -// float dyy = TODO; -// float dss = TODO; -// -// float dxy = (cL.at(yi + 1, xi + 1) - cL.at(yi + 1, xi - 1) - cL.at(yi - 1, xi + 1) + cL.at(yi - 1, xi - 1)) * 0.25f; -// float dxs = TODO; -// float dys = TODO; + float dxx = cL.at(yi, xi + 1) + cL.at(yi, xi - 1) - 2.f * resp_center; + float dyy = cL.at(yi + 1, xi) + cL.at(yi - 1, xi) - 2.f * resp_center; + float dss = nL.at(yi, xi) + pL.at(yi, xi) - 2.f * resp_center; + + float dxy = (cL.at(yi + 1, xi + 1) - cL.at(yi + 1, xi - 1) - cL.at(yi - 1, xi + 1) + cL.at(yi - 1, xi - 1)) * 0.25f; + float dxs = (nL.at(yi, xi + 1) - nL.at(yi, xi - 1) - pL.at(yi, xi + 1) + pL.at(yi, xi - 1)) * 0.25f; + float dys = (nL.at(yi + 1, xi) - nL.at(yi - 1, xi) - pL.at(yi + 1, xi) + pL.at(yi - 1, xi)) * 0.25f; cv::Matx33f H(dxx, dxy, dxs, dxy, dyy, dys, dxs, dys, dss); @@ -270,26 +276,18 @@ std::vector phg::findScaleSpaceExtrema(const std::vector= ((r + 1) * (r + 1) / r)) { + break; + } + } // скейлим координаты точек обратно до родных размеров картинки // !!! если выбираем при даунскейле другую политику, с усреднением вместо ресемплинга, то надо здесь применять формулу со сдвигами на полпикселя float scale = (real_octave >= 0) ? (float)(1 << real_octave) : (1.f / (float)(1 << (-real_octave))); @@ -379,39 +377,39 @@ std::vector phg::computeOrientations(const std::vector(py, px + 1) - img.at(py, px - 1); -// float gy = img.at(py + 1, px) - img.at(py - 1, px); -// -// float mag = TODO; -// float angle = std::atan2(TODO); // [-pi, pi] -// -// float angle_deg = angle * 180.f / (float) CV_PI; -// if (angle_deg < 0.f) angle_deg += 360.f; -// -// // гауссово взвешивание голоса точки с затуханием к краям -// float weight = std::exp(-(TODO) / (2.f * sigma_win * sigma_win)); -// if (!params.enable_orientation_gaussian_weighting) { -// weight = 1.f; -// } -// -// // голосуем в гистограмме направлений. находим два ближайших бина и гладко распределяем голос между ними -// // в таком случае, голос попавший близко к границе между бинами, проголосует поровну за оба бина -// float bin = TODO; -// if (bin >= n_bins) bin -= n_bins; -// int bin0 = (int) bin; -// int bin1 = (bin0 + 1) % n_bins; -// -// float frac = bin - bin0; -// if (!params.enable_orientation_bin_interpolation) { -// frac = 0.f; -// } -// -// histogram[bin0] += TODO; -// histogram[bin1] += TODO; + int px = xi + dx; + int py = yi + dy; + + // градиент + float gx = img.at(py, px + 1) - img.at(py, px - 1); + float gy = img.at(py + 1, px) - img.at(py - 1, px); + + float mag = std::sqrt(gx*gx + gy*gy); + float angle = std::atan2(gy, gx); // [-pi, pi] + + float angle_deg = angle * 180.f / (float) CV_PI; + if (angle_deg < 0.f) angle_deg += 360.f; + + // гауссово взвешивание голоса точки с затуханием к краям + float weight = std::exp(-(dx*dx + dy*dy) / (2.f * sigma_win * sigma_win)); + if (!params.enable_orientation_gaussian_weighting) { + weight = 1.f; + } + + // голосуем в гистограмме направлений. находим два ближайших бина и гладко распределяем голос между ними + // в таком случае, голос попавший близко к границе между бинами, проголосует поровну за оба бина + float bin = angle_deg * n_bins / 360; + if (bin >= n_bins) bin -= n_bins; + int bin0 = (int) bin; + int bin1 = (bin0 + 1) % n_bins; + + float frac = bin - bin0; + if (!params.enable_orientation_bin_interpolation) { + frac = 0.f; + } + + histogram[bin0] += (1-frac)*weight*mag; + histogram[bin1] += frac*weight*mag; } } @@ -450,20 +448,20 @@ std::vector phg::computeOrientations(const std::vector a = (left + right - 2 * center) / 2 // f(1) - f(-1) = 2b -> b = (right - left) / 2 -// float offset = TODO; -// if (!params.enable_orientation_subpixel_localization) { -// offset = 0.f; -// } -// -// float bin_real = i + offset; -// if (bin_real < 0.f) bin_real += n_bins; -// if (bin_real >= n_bins) bin_real -= n_bins; -// -// float angle = bin_real * 360.f / n_bins; -// -// cv::KeyPoint new_kp = kp; -// new_kp.angle = angle; -// oriented_kpts.push_back(new_kp); + float offset = (left-right)/(2*(left-2*center+right)); + if (!params.enable_orientation_subpixel_localization) { + offset = 0.f; + } + + float bin_real = i + offset; + if (bin_real < 0.f) bin_real += n_bins; + if (bin_real >= n_bins) bin_real -= n_bins; + + float angle = bin_real * 360.f / n_bins; + + cv::KeyPoint new_kp = kp; + new_kp.angle = angle; + oriented_kpts.push_back(new_kp); } } } @@ -574,11 +572,11 @@ std::pair> phg::computeDescriptors(const std: bin_o -= n_orient_bins; // семплы вблизи края патча взвешиваем с меньшим весом -// float weight = std::exp(-(TODO) / (2.f * sigma_desc * sigma_desc)); -// if (!params.enable_descriptor_gaussian_weighting) { -// weight = 1.f; -// } -// float weighted_mag = mag * weight; + float weight = std::exp(-(rot_x*rot_x + rot_y*rot_y) / (2.f * sigma_desc * sigma_desc)); + if (!params.enable_descriptor_gaussian_weighting) { + weight = 1.f; + } + float weighted_mag = mag * weight; if (params.enable_descriptor_bin_interpolation) { // размажем вклад weighted_mag по пространственным бинам и по бинам гистограммок трилинейной интерполяцией @@ -609,8 +607,8 @@ std::pair> phg::computeDescriptors(const std: io += n_orient_bins; float wo = (dio == 0) ? (1.f - fo) : fo; -// int idx = TODO; -// desc[idx] += TODO; + int idx = (iy*n_spatial_bins+ix)*n_orient_bins + io; + desc[idx] += weighted_mag * wx * wy * wo; } } } @@ -620,9 +618,8 @@ std::pair> phg::computeDescriptors(const std: int io_nearest = (int)std::round(bin_o) % n_orient_bins; if (ix_nearest >= 0 && ix_nearest < n_spatial_bins && iy_nearest >= 0 && iy_nearest < n_spatial_bins) { - // TODO uncomment -// int idx = (iy_nearest * n_spatial_bins + ix_nearest) * n_orient_bins + io_nearest; -// desc[idx] += weighted_mag; + int idx = (iy_nearest * n_spatial_bins + ix_nearest) * n_orient_bins + io_nearest; + desc[idx] += weighted_mag; } } } diff --git a/tests/test_matching.cpp b/tests/test_matching.cpp index adaac65e..0d6607ae 100644 --- a/tests/test_matching.cpp +++ b/tests/test_matching.cpp @@ -19,9 +19,9 @@ // TODO enable both toggles for testing custom detector & matcher -#define ENABLE_MY_DESCRIPTOR 0 -#define ENABLE_MY_MATCHING 0 -#define ENABLE_GPU_BRUTEFORCE_MATCHER 0 +#define ENABLE_MY_DESCRIPTOR 1 +#define ENABLE_MY_MATCHING 1 +#define ENABLE_GPU_BRUTEFORCE_MATCHER 1 // TODO disable for local testing but do not commit #define SERVER_TESTING 1 @@ -44,6 +44,22 @@ const double max_color_rmse_8u = 20; namespace { +#if ENABLE_GPU_BRUTEFORCE_MATCHER + bool canRunGpuBruteforceMatcher() + { + static bool printed = false; + + std::string reason; + const bool available = phg::BruteforceMatcherGPU::isAvailable(&reason, true); + if (!available && !printed) { + std::cout << "skipping brute force GPU matching: " << reason << std::endl; + printed = true; + } + + return available; + } +#endif + void drawMatches(const cv::Mat &img1, const cv::Mat &img2, const std::vector &keypoints1, @@ -315,24 +331,27 @@ namespace { } time_bruteforce = tm.elapsed(); - tm.restart(); std::vector> knn_matches_bruteforce_gpu; + time_bruteforce_gpu = -1.0; #if ENABLE_GPU_BRUTEFORCE_MATCHER - if (do_bruteforce) { + if (do_bruteforce && canRunGpuBruteforceMatcher()) { std::cout << "brute force GPU matching" << std::endl; + tm.restart(); phg::BruteforceMatcherGPU matcher; matcher.train(descriptors2); matcher.knnMatch(descriptors1, knn_matches_bruteforce_gpu, 2); + time_bruteforce_gpu = tm.elapsed(); } #endif - time_bruteforce_gpu = tm.elapsed(); #if ENABLE_GPU_BRUTEFORCE_MATCHER - ASSERT_EQ(knn_matches_bruteforce_gpu.size(), knn_matches_bruteforce.size()); - for (int i = 0; i < (int) knn_matches_bruteforce_gpu.size(); ++i) { - ASSERT_EQ(knn_matches_bruteforce_gpu[i].size(), knn_matches_bruteforce[i].size()); - for (int j = 0; j < (int) knn_matches_bruteforce_gpu[i].size(); ++j) { - ASSERT_TRUE(matcheq(knn_matches_bruteforce_gpu[i][j], knn_matches_bruteforce[i][j])); + if (time_bruteforce_gpu >= 0.0) { + ASSERT_EQ(knn_matches_bruteforce_gpu.size(), knn_matches_bruteforce.size()); + for (int i = 0; i < (int) knn_matches_bruteforce_gpu.size(); ++i) { + ASSERT_EQ(knn_matches_bruteforce_gpu[i].size(), knn_matches_bruteforce[i].size()); + for (int j = 0; j < (int) knn_matches_bruteforce_gpu[i].size(); ++j) { + ASSERT_TRUE(matcheq(knn_matches_bruteforce_gpu[i][j], knn_matches_bruteforce[i][j])); + } } } #endif @@ -493,7 +512,11 @@ namespace { std::cout << "time_cv: " << time_cv << ", "; std::cout << "time_bruteforce: " << time_bruteforce << ", "; #if ENABLE_GPU_BRUTEFORCE_MATCHER - std::cout << "time_bruteforce_gpu: " << time_bruteforce_gpu << ", "; + if (time_bruteforce_gpu >= 0.0) { + std::cout << "time_bruteforce_gpu: " << time_bruteforce_gpu << ", "; + } else { + std::cout << "time_bruteforce_gpu: skipped, "; + } #endif std::cout << "good_nn: " << good_nn << ", "; std::cout << "good_ratio: " << good_ratio << ", "; diff --git a/tests/test_sift.cpp b/tests/test_sift.cpp index cf3bd7df..7433f9bb 100755 --- a/tests/test_sift.cpp +++ b/tests/test_sift.cpp @@ -28,7 +28,7 @@ // TODO ENABLE ME // TODO ENABLE ME // TODO ENABLE ME -#define ENABLE_MY_SIFT_TESTING 0 +#define ENABLE_MY_SIFT_TESTING 1 #define DENY_CREATE_REF_DATA 1