diff --git a/README.md b/README.md index 18397ee..5c49de6 100755 --- a/README.md +++ b/README.md @@ -1,51 +1,51 @@ -В этом репозитории предложены задания курса по Фотограмметрии для студентов МКН/ИТМО/ВШЭ/CsSpace. - -[Остальные задания](https://github.com/PhotogrammetryCourse/PhotogrammetryTasks2026/). - -# Задание 3. Structure from motion - -[![Build Status](https://github.com/PhotogrammetryCourse/PhotogrammetryTasks2026/actions/workflows/cmake.yml/badge.svg?branch=task03&event=push)](https://github.com/PhotogrammetryCourse/PhotogrammetryTasks2026/actions/workflows/cmake.yml) - -0. Установить Eigen -1. Выполнить задания ниже (при тестировании Github Actions CI использует GCC 11, поэтому если вы используете фичи свежее чем C++17 - есть риск что не скомпилируется, в таком случае поправьте пожалуйста) -2. Отправить **Pull-request** с названием```Task03 <Имя> <Фамилия> <Аффиляция>```: - - - Скопируйте в описание [шаблон](https://raw.githubusercontent.com/PhotogrammetryCourse/PhotogrammetryTasks2026/task03/.github/pull_request_template.md) - - Обязательно отправляйте PR из вашей ветки **task03** (вашего форка) в ветку **task03** (основного репозитория) - - Перечислите свои мысли по вопросам поднятым в коде и просто появившиеся в процессе выполнения задания (выписывайте их с самого начала в отдельный текстовый файл, в шаблоне предложены некоторые вопросы) - - Создайте PR - - Затем дождавшись отработку Github Actions CI (около 15 минут) - скопируйте в описание PR вывод исполнения вашей программы **на CI** (через редактирование описания PR) - -**Мягкий дедлайн**: вечер 22 марта. - -**Жесткий дедлайн**: вечер 29 марта. - -Желательно начать заранее, так как домашка (потенциально) довольно объемная. - -Задание 3.0. -========= - -Ознакомьтесь со структурой проекта: - -1. ```src/phg/sfm/``` - основная часть где вы будете реализовывать алгоритм - -2. ```tests/test_sfm.cpp``` - тесты которые будут прогонять ваш алгоритм на каких-то относительно простых манипуляциях с маленькими картинками, если вам хочется добавить другие сценарии тестирования (возможно с другими метриками) - здорово! - -3. ```data/src``` - исходные данные используемые при тестировании (к ним используются относительные пути, поэтому нужно выставить Working directory = путь к проекту) - -4. ```data/debug/test_sfm``` - сюда тесты сохранят картинки с визуализацией результата. Так же будут сохранены облака точек (.ply файлы). Их можно просмотреть во внешней программе, например, в [Meshlab](https://www.meshlab.net/) - -5. ```data/debug/test_sfm``` - сюда вам предлагается сохранять любые промежуточные картинки-визуализации, это очень полезно для отладки, оценки качества, уверенности и в целом один из немногих способов качественно "заглянуть в черную коробку" - -Задание 3.1. -========= - -1. Убедитесь что у вас все компилируется. В этот раз не будет зеркальной реализации на openCV, но тесты организованы от простого к сложному и вам не нужно иметь полностью готовый алгоритм, чтобы следить за прогрессом. - -2. Ознакомьтесь с тем как проводится тестирование - ```tests/test_sfm.cpp```: - -3. Включите тестирование вашего алгоритма - см. **ENABLE_MY_SFM** в ```test/test_sfm.cpp``` - -5. Реализуйте пропущенные участки алгоритма, проверьте, что все тесты проходят`` - - - Если все хорошо, за выполненное задание дается **10 баллов** +В этом репозитории предложены задания курса по Фотограмметрии для студентов МКН/ИТМО/ВШЭ/CsSpace. + +[Остальные задания](https://github.com/PhotogrammetryCourse/PhotogrammetryTasks2026/). + +# Задание 3. Structure from motion + +[![Build Status](https://github.com/PhotogrammetryCourse/PhotogrammetryTasks2026/actions/workflows/cmake.yml/badge.svg?branch=task03&event=push)](https://github.com/PhotogrammetryCourse/PhotogrammetryTasks2026/actions/workflows/cmake.yml) + +0. Установить Eigen +1. Выполнить задания ниже (при тестировании Github Actions CI использует GCC 11, поэтому если вы используете фичи свежее чем C++17 - есть риск что не скомпилируется, в таком случае поправьте пожалуйста) +2. Отправить **Pull-request** с названием```Task03 <Имя> <Фамилия> <Аффиляция>```: + + - Скопируйте в описание [шаблон](https://raw.githubusercontent.com/PhotogrammetryCourse/PhotogrammetryTasks2026/task03/.github/pull_request_template.md) + - Обязательно отправляйте PR из вашей ветки **task03** (вашего форка) в ветку **task03** (основного репозитория) + - Перечислите свои мысли по вопросам поднятым в коде и просто появившиеся в процессе выполнения задания (выписывайте их с самого начала в отдельный текстовый файл, в шаблоне предложены некоторые вопросы) + - Создайте PR + - Затем дождавшись отработку Github Actions CI (около 15 минут) - скопируйте в описание PR вывод исполнения вашей программы **на CI** (через редактирование описания PR) + +**Мягкий дедлайн**: вечер 22 марта. + +**Жесткий дедлайн**: вечер 29 марта. + +Желательно начать заранее, так как домашка (потенциально) довольно объемная. + +Задание 3.0. +========= + +Ознакомьтесь со структурой проекта: + +1. ```src/phg/sfm/``` - основная часть где вы будете реализовывать алгоритм + +2. ```tests/test_sfm.cpp``` - тесты которые будут прогонять ваш алгоритм на каких-то относительно простых манипуляциях с маленькими картинками, если вам хочется добавить другие сценарии тестирования (возможно с другими метриками) - здорово! + +3. ```data/src``` - исходные данные используемые при тестировании (к ним используются относительные пути, поэтому нужно выставить Working directory = путь к проекту) + +4. ```data/debug/test_sfm``` - сюда тесты сохранят картинки с визуализацией результата. Так же будут сохранены облака точек (.ply файлы). Их можно просмотреть во внешней программе, например, в [Meshlab](https://www.meshlab.net/) + +5. ```data/debug/test_sfm``` - сюда вам предлагается сохранять любые промежуточные картинки-визуализации, это очень полезно для отладки, оценки качества, уверенности и в целом один из немногих способов качественно "заглянуть в черную коробку" + +Задание 3.1. +========= + +1. Убедитесь что у вас все компилируется. В этот раз не будет зеркальной реализации на openCV, но тесты организованы от простого к сложному и вам не нужно иметь полностью готовый алгоритм, чтобы следить за прогрессом. + +2. Ознакомьтесь с тем как проводится тестирование - ```tests/test_sfm.cpp```: + +3. Включите тестирование вашего алгоритма - см. **ENABLE_MY_SFM** в ```test/test_sfm.cpp``` + +5. Реализуйте пропущенные участки алгоритма, проверьте, что все тесты проходят`` + + - Если все хорошо, за выполненное задание дается **10 баллов** diff --git a/src/phg/matching/descriptor_matcher.cpp b/src/phg/matching/descriptor_matcher.cpp index f4bcd87..d9557e6 100644 --- a/src/phg/matching/descriptor_matcher.cpp +++ b/src/phg/matching/descriptor_matcher.cpp @@ -1,76 +1,147 @@ -#include "descriptor_matcher.h" - -#include -#include "flann_factory.h" - -void phg::DescriptorMatcher::filterMatchesRatioTest(const std::vector> &matches, - std::vector &filtered_matches) -{ - filtered_matches.clear(); - - throw std::runtime_error("not implemented yet"); -} - - -void phg::DescriptorMatcher::filterMatchesClusters(const std::vector &matches, - const std::vector keypoints_query, - const std::vector keypoints_train, - std::vector &filtered_matches) -{ - 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 float radius_limit_scale = 2.f; // limit search radius by scaled median - - const int n_matches = matches.size(); - - if (n_matches < total_neighbours) { - throw std::runtime_error("DescriptorMatcher::filterMatchesClusters : too few matches"); - } - - 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; - } -// -// // размерность всего 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; -// } -// -// метч остается, если левое и правое множества первых total_neighbors соседей в радиусах поиска(radius2_query, radius2_train) имеют как минимум consistent_matches общих элементов -// // TODO заполнить filtered_matches -} +#include "descriptor_matcher.h" + +#include +#include "flann_factory.h" + +void phg::DescriptorMatcher::filterMatchesRatioTest(const std::vector> &matches, + std::vector &filtered_matches) +{ + /* + The ratio test in descriptor matching is a technique used in computer vision to filter out false or ambiguous matches between features in two images. Proposed by David Lowe alongside the SIFT algorithm, it ensures that a match is valid by checking if the best match is significantly better than the second-best match. + + Here is a breakdown of how it works: + 1. Mechanism + K-Nearest Neighbors (KNN): For a descriptor in Image A, the matching algorithm finds the two closest descriptors in Image B using Euclidean distance ( + norm) or Hamming distance. Let these be + (best match) and + (second-best match). + The Ratio: The ratio is calculated as: r = d2 / d1 + + Thresholding: A threshold (usually 0.7 or 0.8) is set. A match is accepted only if: r < threshold + */ + filtered_matches.clear(); + + const float ratio_threshold = 0.6f; // scale50 test forced me to lower the threshold to 0.6, otherwise there were too few matches after filtering + + for (const auto &match_pair : matches) { + if (match_pair.size() < 2) { + // Skip if there are fewer than 2 matches for the query + continue; + } + + const cv::DMatch &best_match = match_pair[0]; + const cv::DMatch &second_best_match = match_pair[1]; + + // Apply the ratio test !!!! + if (best_match.distance < ratio_threshold * second_best_match.distance) { + filtered_matches.push_back(best_match); + } + } +} + + +void phg::DescriptorMatcher::filterMatchesClusters(const std::vector &matches, + const std::vector keypoints_query, + const std::vector keypoints_train, + std::vector &filtered_matches) +{ + + /* + here we got already foltered matched (after ratio test) and we want to filter them by clusters. + The idea is that correct matches should form spatially consistent clusters in both images, + while incorrect matches are more likely to be randomly distributed. + + */ + filtered_matches.clear(); + + const size_t total_neighbours = 15; // total number of neighbours to test (including candidate) + const size_t consistent_matches = 5; // 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(); + + if (n_matches < total_neighbours) { + throw std::runtime_error("DescriptorMatcher::filterMatchesClusters : too few matches"); + } + + 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; + } + + // размерность всего 2, так что точное KD-дерево + std::shared_ptr index_params = flannKdTreeIndexParams(1); + std::shared_ptr search_params = flannKsTreeSearchParams(std::max(64, n_matches)); + + // when we call kd tree index , we "train" it on the points, + // so it builds the tree structure for fast nearest neighbor search. + // my intuition :: its like database to search in + 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); + + // now we know for each point in query and train, who are their total_neighbours nearest neighbors and what are the distances to them. + 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) { + // here we collect the distances to the farthest neighbor (the total_neighbours-th neighbor) for each point, + // and then we will use the median of these distances to set a search radius. + 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()); + + /* example + radius2_query = 120 means that for each point in the query dataset, neighbors within a squared distance of 120 will be considered for spatial consistency checks. + */ + 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 общих элементов + + for (int i = 0; i < n_matches; ++i) { + int count_consistent = 0; // we gonna keep track of how many neighbors are consistent between the query and train sets for the current match. + for (int j_query = 0; j_query < total_neighbours; ++j_query) { + if (distances2_query.at(i, j_query) > radius2_query) { + break; // remove neighbors that are too far in the query set + } + + int idx_query = indices_query.at(i, j_query); + for (int j_train = 0; j_train < total_neighbours; ++j_train) { + if (distances2_train.at(i, j_train) > radius2_train) { + break; // exclude neighbors that are too far in the train set + } + + int idx_train = indices_train.at(i, j_train); + if (idx_query == idx_train) { + ++count_consistent; + break; + } + } + } + + if (count_consistent >= 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 9e9f518..390aa89 100644 --- a/src/phg/matching/flann_matcher.cpp +++ b/src/phg/matching/flann_matcher.cpp @@ -1,21 +1,52 @@ -#include -#include "flann_matcher.h" -#include "flann_factory.h" - - -phg::FlannMatcher::FlannMatcher() -{ - // параметры для приближенного поиска -// index_params = flannKdTreeIndexParams(TODO); -// search_params = flannKsTreeSearchParams(TODO); -} - -void phg::FlannMatcher::train(const cv::Mat &train_desc) -{ - flann_index = flannKdTreeIndex(train_desc, index_params); -} - -void phg::FlannMatcher::knnMatch(const cv::Mat &query_desc, std::vector> &matches, int k) const -{ - throw std::runtime_error("not implemented yet"); -} +#include +#include "flann_matcher.h" +#include "flann_factory.h" + + +phg::FlannMatcher::FlannMatcher() +{ + // параметры для приближенного поиска + + index_params = flannKdTreeIndexParams(4); + search_params = flannKsTreeSearchParams(32); + +} + +void phg::FlannMatcher::train(const cv::Mat &train_desc) +{ + flann_index = flannKdTreeIndex(train_desc, index_params); +} + +void phg::FlannMatcher::knnMatch(const cv::Mat &query_desc, std::vector> &matches, int k) const +{ + cv::Mat indices(query_desc.rows, k, CV_32SC1); + cv::Mat dists(query_desc.rows, k, CV_32FC1); + + // do k-nearest neighbor search + flann_index->knnSearch(query_desc, indices, dists, k, *search_params); + + // store results + matches.resize(query_desc.rows); + for (int i = 0; i < query_desc.rows; ++i) { + matches[i].reserve(k); + for (int j = 0; j < k; ++j) { + matches[i].emplace_back(i, indices.at(i, j), std::sqrt(dists.at(i, j)) ); + } + } + + // matches.resize(query_desc.rows); + // for (int qi = 0; qi < query_desc.rows; ++qi) { + // std::vector &dst = matches[qi]; + // dst.resize(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(dists.at(qi, ki)); + // dst[ki] = match; + // } + // } + + +} diff --git a/src/phg/sfm/ematrix.cpp b/src/phg/sfm/ematrix.cpp index 3bc052b..513583d 100644 --- a/src/phg/sfm/ematrix.cpp +++ b/src/phg/sfm/ematrix.cpp @@ -18,9 +18,29 @@ namespace { copy(Ecv, E); Eigen::JacobiSVD svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV); - throw std::runtime_error("not implemented yet"); -// TODO - + Eigen::MatrixXd U = svd.matrixU(); + Eigen::VectorXd s = svd.singularValues(); + Eigen::MatrixXd V = svd.matrixV(); + if (U.determinant() < 0) { + U = -U; + } + if (V.determinant() < 0) { + V = -V; + } + + if (s[0] < 1e-10 || s[1] < 1e-10) { + std::cout << "ensureSpectralProperty: s: " << s.transpose() << std::endl; + throw std::runtime_error("essential matrix is degenerate"); + } + double avg = (s[0] + s[1]) / 2.0; + s[0] = avg; + s[1] = avg; + s[2] = 0; + + Eigen::MatrixXd S = Eigen::MatrixXd::Zero(3, 3); + S(0, 0) = s[0]; + S(1, 1) = s[1]; + E = U * S * V.transpose(); copy(E, Ecv); } @@ -28,12 +48,11 @@ namespace { cv::Matx33d phg::fmatrix2ematrix(const cv::Matx33d &F, const phg::Calibration &calib0, const phg::Calibration &calib1) { - throw std::runtime_error("not implemented yet"); -// matrix3d E = TODO; -// -// ensureSpectralProperty(E); -// -// return E; + // convert fundamental matrix to essential matrix using camera calibration + // E = K1^T * F * K0 + cv::Matx33d E = calib1.K().t() * F * calib0.K(); + ensureSpectralProperty(E); + return E; } namespace { @@ -58,110 +77,122 @@ namespace { return result; } + bool depthTest(const vector2d &m0, const vector2d &m1, const phg::Calibration &calib0, const phg::Calibration &calib1, const matrix34d &P0, const matrix34d &P1) { - throw std::runtime_error("not implemented yet"); -// // скомпенсировать калибровки камер -// vector3d p0 = TODO; -// vector3d p1 = TODO; -// -// vector3d ps[2] = {p0, p1}; -// matrix34d Ps[2] = {P0, P1}; -// -// vector4d X = phg::triangulatePoint(Ps, ps, 2); -// if (X[3] != 0) { -// X /= X[3]; -// } -// -// // точка должна иметь положительную глубину для обеих камер -// return TODO && TODO; + // in front of cameras (positive depths) + // positive depth means normalized image point (x, y, 1) should have positive + // dot product with the ray from camera center to the point + + // скомпенсировать калибровки камер + vector3d p0 = calib0.unproject(m0); + vector3d p1 = calib1.unproject(m1); + + vector3d ps[2] = {p0, p1}; + matrix34d Ps[2] = {P0, P1}; + + vector4d X = phg::triangulatePoint(Ps, ps, 2); + if (X[3] != 0) { + X /= X[3]; + } + + + double depth0 = P0(2, 0) * X[0] + P0(2, 1) * X[1] + P0(2, 2) * X[2] + P0(2, 3); + double depth1 = P1(2, 0) * X[0] + P1(2, 1) * X[1] + P1(2, 2) * X[2] + P1(2, 3); + return depth0 > 0 && depth1 > 0; + } } // Матрицы камер для фундаментальной матрицы определены с точностью до проективного преобразования // То есть, можно исказить трехмерный мир (применив 4-мерную однородную матрицу), и одновременно поменять матрицы P0, P1 так, что проекции в пикселях не изменятся -// Если мы знаем калибровки камер (матрицы K0, K1 в структуре матриц P0, P1), то можем наложить дополнительные ограничения, в частности, известно, что -// существенная матрица (Essential matrix = K1t * F * K0) имеет ровно два совпадающих ненулевых сингулярных значения, тогда как для фундаментальной матрицы они могут различаться +// Если мы знаем калибровки камер (матрицы K0, K1 в структуре матриц P0, P1), то можем наложить дополнительные ограничения, +// в частности, известно, что: +// существенная матрица (Essential matrix = K1t * F * K0) имеет ровно ДВА совпадающих ненулевых сингулярных значения, +// тогда как для фундаментальной матрицы они могут различаться + // Это дополнительное ограничение позволяет разложить существенную матрицу с точностью до 4 решений, вместо произвольного проективного преобразования (см. Hartley & Zisserman p.258) // Обычно мы можем использовать одну общую калибровку, более менее верную для большого количества реальных камер и с ее помощью выполнить // первичное разложение существенной матрицы (а из него, взаимное расположение камер) для последующего уточнения методом нелинейной оптимизации -void phg::decomposeEMatrix(cv::Matx34d &P0, cv::Matx34d &P1, const cv::Matx33d &Ecv, const std::vector &m0, const std::vector &m1, const Calibration &calib0, const Calibration &calib1) +void phg::decomposeEMatrix(cv::Matx34d &P0, cv::Matx34d &P1, const cv::Matx33d &Ecv, + const std::vector &m0, const std::vector &m1, const Calibration &calib0, const Calibration &calib1) { - throw std::runtime_error("not implemented yet"); -// if (m0.size() != m1.size()) { -// throw std::runtime_error("decomposeEMatrix : m0.size() != m1.size()"); -// } -// -// using mat = Eigen::MatrixXd; -// using vec = Eigen::VectorXd; -// -// mat E; -// copy(Ecv, E); -// -// // (см. Hartley & Zisserman p.258) -// -// Eigen::JacobiSVD svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV); -// -// mat U = svd.matrixU(); -// vec s = svd.singularValues(); -// mat V = svd.matrixV(); -// -// // U, V must be rotation matrices, not just orthogonal -// if (U.determinant() < 0) U = -U; -// if (V.determinant() < 0) V = -V; -// -// std::cout << "U:\n" << U << std::endl; -// std::cout << "s:\n" << s << std::endl; -// std::cout << "V:\n" << V << std::endl; -// -// -// mat R0 = TODO; -// mat R1 = TODO; -// -// std::cout << "R0:\n" << R0 << std::endl; -// std::cout << "R1:\n" << R1 << std::endl; -// -// vec t0 = TODO; -// vec t1 = TODO; -// -// std::cout << "t0:\n" << t0 << std::endl; -// -// P0 = matrix34d::eye(); -// -// // 4 possible solutions -// matrix34d P10 = composeP(R0, t0); -// matrix34d P11 = composeP(R0, t1); -// matrix34d P12 = composeP(R1, t0); -// matrix34d P13 = composeP(R1, t1); -// matrix34d P1s[4] = {P10, P11, P12, P13}; -// -// // need to select best of 4 solutions: 3d points should be in front of cameras (positive depths) -// int best_count = 0; -// int best_idx = -1; -// for (int i = 0; i < 4; ++i) { -// int count = 0; -// for (int j = 0; j < (int) m0.size(); ++j) { -// if (depthTest(m0[j], m1[j], calib0, calib1, P0, P1s[i])) { -// ++count; -// } -// } -// std::cout << "decomposeEMatrix: count: " << count << std::endl; -// if (count > best_count) { -// best_count = count; -// best_idx = i; -// } -// } -// -// if (best_count == 0) { -// throw std::runtime_error("decomposeEMatrix : can't decompose ematrix"); -// } -// -// P1 = P1s[best_idx]; -// -// std::cout << "best idx: " << best_idx << std::endl; -// std::cout << "P0: \n" << P0 << std::endl; -// std::cout << "P1: \n" << P1 << std::endl; + if (m0.size() != m1.size()) { + throw std::runtime_error("decomposeEMatrix : m0.size() != m1.size()"); + } + + using mat = Eigen::MatrixXd; + using vec = Eigen::VectorXd; + + mat E; + copy(Ecv, E); + + // (см. Hartley & Zisserman p.258) + + Eigen::JacobiSVD svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV); + + mat U = svd.matrixU(); + vec s = svd.singularValues(); + mat V = svd.matrixV(); + + // U, V must be rotation matrices, not just orthogonal + if (U.determinant() < 0) U = -U; + if (V.determinant() < 0) V = -V; + + std::cout << "U:\n" << U << std::endl; + std::cout << "s:\n" << s << std::endl; + std::cout << "V:\n" << V << std::endl; + // alternative SVD is given by +// E = (U diag(R2×2, 1)) diag(1, 1, 0)(diag(RT 2×2, 1))VT for any 2 × 2 rotation matrix R. + +// then + mat R0 = U * mat{{0, -1, 0}, {1, 0, 0}, {0, 0, 1}} * V.transpose(); + mat R1 = U * mat{{0, 1, 0}, {-1, 0, 0}, {0, 0, 1}} * V.transpose(); + + std::cout << "R0:\n" << R0 << std::endl; + std::cout << "R1:\n" << R1 << std::endl; + + vec t0 = U.col(2); + vec t1 = -U.col(2); + + std::cout << "t0:\n" << t0 << std::endl; + + P0 = matrix34d::eye(); + + // we found 4 possible solutions + matrix34d P10 = composeP(R0, t0); + matrix34d P11 = composeP(R0, t1); + matrix34d P12 = composeP(R1, t0); + matrix34d P13 = composeP(R1, t1); + matrix34d P1s[4] = {P10, P11, P12, P13}; + + // need to select best of 4 solutions: 3d points should be in front of cameras (positive depths) + int best_count = 0; + int best_idx = -1; + for (int i = 0; i < 4; ++i) { + int count = 0; + for (int j = 0; j < (int) m0.size(); ++j) { + if (depthTest(m0[j], m1[j], calib0, calib1, P0, P1s[i])) { + ++count; + } + } + std::cout << "decomposeEMatrix: count: " << count << std::endl; + if (count > best_count) { + best_count = count; + best_idx = i; + } + } + + if (best_count == 0) { + throw std::runtime_error("decomposeEMatrix : can't decompose ematrix"); + } + + P1 = P1s[best_idx]; + + std::cout << "best idx: " << best_idx << std::endl; + std::cout << "P0: \n" << P0 << std::endl; + std::cout << "P1: \n" << P1 << std::endl; } void phg::decomposeUndistortedPMatrix(cv::Matx33d &R, cv::Vec3d &O, const cv::Matx34d &P) diff --git a/src/phg/sfm/fmatrix.cpp b/src/phg/sfm/fmatrix.cpp index 5012718..6972233 100644 --- a/src/phg/sfm/fmatrix.cpp +++ b/src/phg/sfm/fmatrix.cpp @@ -19,55 +19,110 @@ namespace { Eigen::VectorXd s = svdf.singularValues(); Eigen::MatrixXd V = svdf.matrixV(); - std::cout << "F info:\nF:\n" << F << "\nU:\n" << U << "\ns:\n" << s << "\nV:\n" << V << std::endl; + // std::cout << "F info:\nF:\n" << F << "\nU:\n" << U << "\ns:\n" << s << "\nV:\n" << V << std::endl; } // (см. Hartley & Zisserman p.279) + // DLT to find the fundamental matrix from point correspondences equations: + // x1' * F * x0 = 0 + // f matrix coefficients are found as the null space of the matrix A, which is constructed from point correspondences + + // (8 point alhgorithm) A coefficients of the equations are constructed from point correspondences as follows: + // for each point (u, v) <--> (u`, v`) we have a row in A: [u`*u, u`*v, u`, v`*u, v`*v, v`, u, v, 1] + cv::Matx33d estimateFMatrixDLT(const cv::Vec2d *m0, const cv::Vec2d *m1, int count) { - throw std::runtime_error("not implemented yet"); -// int a_rows = TODO; -// int a_cols = TODO; -// -// Eigen::MatrixXd A(a_rows, a_cols); -// -// for (int i_pair = 0; i_pair < count; ++i_pair) { -// -// double x0 = m0[i_pair][0]; -// double y0 = m0[i_pair][1]; -// -// double x1 = m1[i_pair][0]; -// double y1 = m1[i_pair][1]; -// -//// std::cout << "(" << x0 << ", " << y0 << "), (" << x1 << ", " << y1 << ")" << std::endl; -// -// TODO -// } -// -// Eigen::JacobiSVD svda(A, Eigen::ComputeFullU | Eigen::ComputeFullV); -// Eigen::VectorXd null_space = TODO -// -// Eigen::MatrixXd F(3, 3); -// F.row(0) << null_space[0], null_space[1], null_space[2]; -// F.row(1) << null_space[3], null_space[4], null_space[5]; -// F.row(2) << null_space[6], null_space[7], null_space[8]; -// -//// Поправить F так, чтобы соблюдалось свойство фундаментальной матрицы (последнее сингулярное значение = 0) -// Eigen::JacobiSVD svdf(F, Eigen::ComputeFullU | Eigen::ComputeFullV); -// -// TODO -// -// cv::Matx33d Fcv; -// copy(F, Fcv); -// -// return Fcv; + // throw std::runtime_error("not implemented yet"); + int a_rows = count; // number of point correspondences + int a_cols = 9; // 9 coefficients of the fundamental matrix + + Eigen::MatrixXd A(a_rows, a_cols); + + for (int i_pair = 0; i_pair < count; ++i_pair) { + + double x0 = m0[i_pair][0]; + double y0 = m0[i_pair][1]; + + double x1 = m1[i_pair][0]; + double y1 = m1[i_pair][1]; + + // std::cout << "(" << x0 << ", " << y0 << "), (" << x1 << ", " << y1 << ")" << std::endl; + A(i_pair, 0) = x1 * x0; + A(i_pair, 1) = x1 * y0; + A(i_pair, 2) = x1; + A(i_pair, 3) = y1 * x0; + A(i_pair, 4) = y1 * y0; + A(i_pair, 5) = y1; + A(i_pair, 6) = x0; + A(i_pair, 7) = y0; + A(i_pair, 8) = 1; + + } + + Eigen::JacobiSVD svda(A, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::VectorXd null_space = svda.matrixV().col(a_cols - 1); + // we take last column of V, which corresponds to the smallest singular value, + // which is the null space of A + + Eigen::MatrixXd F(3, 3); + F.row(0) << null_space[0], null_space[1], null_space[2]; + F.row(1) << null_space[3], null_space[4], null_space[5]; + F.row(2) << null_space[6], null_space[7], null_space[8]; + + // Поправить F так, чтобы соблюдалось свойство фундаментальной матрицы (последнее сингулярное значение = 0) + Eigen::JacobiSVD svdf(F, Eigen::ComputeFullU | Eigen::ComputeFullV); + // svd -> replace smallest singular value with 0 ->> reconstruct F + Eigen::VectorXd s = svdf.singularValues(); + s[2] = 0; // set the smallest singular value to 0 + F = svdf.matrixU() * s.asDiagonal() * svdf.matrixV().transpose(); + cv::Matx33d Fcv; + copy(F, Fcv); + + return Fcv; } - // Нужно создать матрицу преобразования, которая сдвинет переданное множество точек так, что центр масс перейдет в ноль, а Root Mean Square расстояние до него станет sqrt(2) + // Нужно создать матрицу преобразования, которая сдвинет переданное множество точек так, + // что центр масс перейдет в ноль, а Root Mean Square расстояние до него станет sqrt(2) // (см. Hartley & Zisserman p.107 Why is normalization essential?) cv::Matx33d getNormalizeTransform(const std::vector &m) { - throw std::runtime_error("not implemented yet"); + // Compute the centroid of the points + cv::Vec2d centroid(0, 0); + for (const auto &pt : m) { + centroid += pt; + } + + + // centroid = centroid.div(m.size()); + centroid[0] /= m.size(); + centroid[1] /= m.size(); + + // Compute the average distance from the centroid + // double avg_dist = 0; + // for (const auto &pt : m) { + // avg_dist += cv::norm(pt - centroid); + // } + // avg_dist /= m.size(); + // // Compute the scaling factor to make the average distance sqrt(2) + // double scale = std::sqrt(2) / avg_dist; + + + double sum_sq = 0; + for (const auto &pt : m) { + double d = cv::norm(pt - centroid); + sum_sq += d * d; + } + double rms_dist = std::sqrt(sum_sq / m.size()); + double scale = std::sqrt(2) / rms_dist; + + // Construct the normalization transformation matrix + cv::Matx33d T = { + scale, 0, -scale * centroid[0], + 0, scale, -scale * centroid[1], + 0, 0, 1 + }; + + return T; } cv::Vec2d transformPoint(const cv::Vec2d &pt, const cv::Matx33d &T) @@ -99,66 +154,80 @@ namespace { m1_t[i] = transformPoint(m1[i], TN1); } - { -// Проверьте лог: при повторной нормализации должно найтись почти единичное преобразование - getNormalizeTransform(m0_t); - getNormalizeTransform(m1_t); - } - throw std::runtime_error("not implemented yet"); -// // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters -// // будет отличаться от случая с гомографией -// const int n_trials = TODO; -// -// const int n_samples = TODO; -// uint64_t seed = 1; -// -// int best_support = 0; -// cv::Matx33d best_F; -// -// std::vector sample; -// for (int i_trial = 0; i_trial < n_trials; ++i_trial) { -// phg::randomSample(sample, n_matches, n_samples, &seed); -// -// cv::Vec2d ms0[n_samples]; -// cv::Vec2d ms1[n_samples]; -// for (int i = 0; i < n_samples; ++i) { -// ms0[i] = m0_t[sample[i]]; -// ms1[i] = m1_t[sample[i]]; -// } -// -// cv::Matx33d F = estimateFMatrixDLT(ms0, ms1, n_samples); -// -// // denormalize -// F = TODO -// -// int support = 0; -// for (int i = 0; i < n_matches; ++i) { -// if (phg::epipolarTest(m0[i], m1[i], todo, threshold_px) && phg::epipolarTest(m1[i], m0[i], todo, threshold_px)) -// { -// ++support; -// } -// } -// -// if (support > best_support) { -// best_support = support; -// best_F = F; -// -// std::cout << "estimateFMatrixRANSAC : support: " << best_support << "/" << n_matches << std::endl; -// infoF(F); -// -// if (best_support == n_matches) { -// break; -// } -// } -// } -// -// std::cout << "estimateFMatrixRANSAC : best support: " << best_support << "/" << n_matches << std::endl; -// -// if (best_support == 0) { -// throw std::runtime_error("estimateFMatrixRANSAC : failed to estimate fundamental matrix"); -// } -// -// return best_F; + // { + // // Проверьте лог: при повторной нормализации должно найтись почти единичное преобразование + // getNormalizeTransform(m0_t); + // getNormalizeTransform(m1_t); + // } + + // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters + // будет отличаться от случая с гомографией + // k or n_trials = number of iterations + // w = = number of inliers in data / number of points in data + // RANSAC returns a successful result if in some iteration it selects only inliers + // from the input data set when it chooses n points from the data set from which the model parameters are estimated. + // p = desired probability of success + // k = log(1 - p) / log(1 - w^n) + + // lets put p = 0.99, + // double w = 0.8; // we expect at least 50% of inliers in the data + // double p = 0.9; + const int n_samples = 8; + const int n_trials = 20000; // used this value to pass SFM.FmatrixMatchFiltering + + uint64_t seed = 1; + + int best_support = 0; + cv::Matx33d best_F; + + std::vector sample; + for (int i_trial = 0; i_trial < n_trials; ++i_trial) { + phg::randomSample(sample, n_matches, n_samples, &seed); + + cv::Vec2d ms0[n_samples]; + cv::Vec2d ms1[n_samples]; + for (int i = 0; i < n_samples; ++i) { + ms0[i] = m0_t[sample[i]]; + ms1[i] = m1_t[sample[i]]; + } + + cv::Matx33d F = estimateFMatrixDLT(ms0, ms1, n_samples); + + // denormalize + // F = TODO + F = TN1.t() * F * TN0; + + int support = 0; + for (int i = 0; i < n_matches; ++i) { + // if (phg::epipolarTest(m0[i], m1[i], F, threshold_px) && phg::epipolarTest(m1[i], m0[i], F, threshold_px)) + // checking epipolar constraint using epipolarTest: + if (phg::epipolarTest(m0[i], m1[i], F, threshold_px) && phg::epipolarTest(m1[i], m0[i], F.t(), threshold_px)) + + { + ++support; + } + } + + if (support > best_support) { + best_support = support; + best_F = F; + + // std::cout << "estimateFMatrixRANSAC : support: " << best_support << "/" << n_matches << std::endl; + infoF(F); + + if (best_support == n_matches) { + break; + } + } + } + + std::cout << "estimateFMatrixRANSAC : best support: " << best_support << "/" << n_matches << std::endl; + + if (best_support == 0) { + throw std::runtime_error("estimateFMatrixRANSAC : failed to estimate fundamental matrix"); + } + + return best_F; } } diff --git a/src/phg/sfm/homography.cpp b/src/phg/sfm/homography.cpp index 5cbc780..dba5988 100644 --- a/src/phg/sfm/homography.cpp +++ b/src/phg/sfm/homography.cpp @@ -1,250 +1,301 @@ -#include "homography.h" - -#include -#include - -namespace { - - // источник: https://e-maxx.ru/algo/linear_systems_gauss - // очень важно при выполнении метода гаусса использовать выбор опорного элемента: об этом можно почитать в источнике кода - // или на вики: https://en.wikipedia.org/wiki/Pivot_element - int gauss(std::vector> a, std::vector &ans) - { - using namespace std; - const double EPS = 1e-8; - const int INF = std::numeric_limits::max(); - - int n = (int) a.size(); - int m = (int) a[0].size() - 1; - - vector where (m, -1); - for (int col=0, row=0; col abs (a[sel][col])) - sel = i; - if (abs (a[sel][col]) < EPS) - continue; - for (int i=col; i<=m; ++i) - swap (a[sel][i], a[row][i]); - where[col] = row; - - for (int i=0; i EPS) - return 0; - } - - for (int i=0; i> A; - std::vector H; - - double xs0[4] = {l0.x, l1.x, l2.x, l3.x}; - double xs1[4] = {r0.x, r1.x, r2.x, r3.x}; - double ys0[4] = {l0.y, l1.y, l2.y, l3.y}; - double ys1[4] = {r0.y, r1.y, r2.y, r3.y}; - double ws0[4] = {1, 1, 1, 1}; - double ws1[4] = {1, 1, 1, 1}; - - for (int i = 0; i < 4; ++i) { - // fill 2 rows of matrix A - - double x0 = xs0[i]; - double y0 = ys0[i]; - double w0 = ws0[i]; - - double x1 = xs1[i]; - double y1 = ys1[i]; - double w1 = ws1[i]; - - // 8 elements of matrix + free term as needed by gauss routine -// A.push_back({TODO}); -// A.push_back({TODO}); - } - - int res = gauss(A, H); - if (res == 0) { - throw std::runtime_error("gauss: no solution found"); - } - else - if (res == 1) { -// std::cout << "gauss: unique solution found" << std::endl; - } - 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; - } - else - { - throw std::runtime_error("gauss: unexpected return code"); - } - - // add fixed element H33 = 1 - H.push_back(1.0); - - cv::Mat H_mat(3, 3, CV_64FC1); - std::copy(H.begin(), H.end(), H_mat.ptr()); - return H_mat; - } - - // pseudorandom number generator - inline uint64_t xorshift64(uint64_t *state) - { - if (*state == 0) { - *state = 1; - } - - uint64_t x = *state; - x ^= x << 13; - x ^= x >> 7; - x ^= x << 17; - return *state = x; - } - - void randomSample(std::vector &dst, int max_id, int sample_size, uint64_t *state) - { - dst.clear(); - - const int max_attempts = 1000; - - for (int i = 0; i < sample_size; ++i) { - for (int k = 0; k < max_attempts; ++k) { - int v = xorshift64(state) % max_id; - if (dst.empty() || std::find(dst.begin(), dst.end(), v) == dst.end()) { - dst.push_back(v); - break; - } - } - if (dst.size() < i + 1) { - throw std::runtime_error("Failed to sample ids"); - } - } - } - - 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()"); - } - - // 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) - // * [2] Adaptive Structure from Motion with a contrario model estimation. (Pierre Moulon, Pascal Monasse, Renaud Marlet) - // * (простое описание для понимания) - // * [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; - } - -} - -cv::Mat phg::findHomography(const std::vector &points_lhs, const std::vector &points_rhs) -{ - return estimateHomographyRANSAC(points_lhs, points_rhs); -} - -// чтобы заработало, нужно пересобрать библиотеку с дополнительным модулем calib3d (см. инструкцию в корневом CMakeLists.txt) -cv::Mat phg::findHomographyCV(const std::vector &points_lhs, const std::vector &points_rhs) -{ - return cv::findHomography(points_lhs, points_rhs, cv::RANSAC); -} - -// T - 3x3 однородная матрица, например, гомография -// таким преобразованием внутри занимается функции cv::perspectiveTransform и cv::warpPerspective -cv::Point2d phg::transformPoint(const cv::Point2d &pt, const cv::Mat &T) -{ - throw std::runtime_error("not implemented yet"); -} - -cv::Point2d phg::transformPointCV(const cv::Point2d &pt, const cv::Mat &T) { - // ineffective but ok for testing - std::vector tmp0 = {pt}; - std::vector tmp1(1); - cv::perspectiveTransform(tmp0, tmp1, T); - return tmp1[0]; -} +#include "homography.h" + +#include +#include + +namespace { + + // источник: https://e-maxx.ru/algo/linear_systems_gauss + // очень важно при выполнении метода гаусса использовать выбор опорного элемента: об этом можно почитать в источнике кода + // или на вики: https://en.wikipedia.org/wiki/Pivot_element + int gauss(std::vector> a, std::vector &ans) + { + using namespace std; + const double EPS = 1e-8; + const int INF = std::numeric_limits::max(); + + int n = (int) a.size(); + int m = (int) a[0].size() - 1; + + vector where (m, -1); + for (int col=0, row=0; col abs (a[sel][col])) + sel = i; + if (abs (a[sel][col]) < EPS) + continue; + for (int i=col; i<=m; ++i) + swap (a[sel][i], a[row][i]); + where[col] = row; + + for (int i=0; i EPS) + return 0; + } + + for (int i=0; i> A; + std::vector H; + + double xs0[4] = {l0.x, l1.x, l2.x, l3.x}; + double xs1[4] = {r0.x, r1.x, r2.x, r3.x}; + double ys0[4] = {l0.y, l1.y, l2.y, l3.y}; + double ys1[4] = {r0.y, r1.y, r2.y, r3.y}; + double ws0[4] = {1, 1, 1, 1}; + double ws1[4] = {1, 1, 1, 1}; + + for (int i = 0; i < 4; ++i) { + // fill 2 rows of matrix A + + double x0 = xs0[i]; + double y0 = ys0[i]; + double w0 = ws0[i]; + + double x1 = xs1[i]; + double y1 = ys1[i]; + double w1 = ws1[i]; + + // 8 elements of matrix + free term as needed by gauss routine + A.push_back({x0, y0, w0, 0, 0, 0, -x1 * x0, -x1 * y0, x1 * w0}); + A.push_back({0, 0, 0, x0, y0, w0, -y1 * x0, -y1 * y0, y1 * w0}); + } + + int res = gauss(A, H); + if (res == 0) { + throw std::runtime_error("gauss: no solution found"); + } + else + if (res == 1) { +// std::cout << "gauss: unique solution found" << std::endl; + } + 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; + } + else + { + throw std::runtime_error("gauss: unexpected return code"); + } + + // add fixed element H33 = 1 + H.push_back(1.0); + + cv::Mat H_mat(3, 3, CV_64FC1); + std::copy(H.begin(), H.end(), H_mat.ptr()); + return H_mat; + } + + cv::Mat estimateHomographyNPoints(const std::vector &lhs_points, const std::vector &rhs_points) + { + if (lhs_points.size() != rhs_points.size()) { + throw std::runtime_error("estimateHomographyNPoints: lhs_points and rhs_points must have the same size"); + } + + if (lhs_points.size() < 4) { + throw std::runtime_error("estimateHomographyNPoints: At least 4 points are required to estimate a homography"); + } + + // Use OpenCV's built-in method to compute the homography + cv::Mat mask; // Optional mask to store inliers + cv::Mat H = cv::findHomography(lhs_points, rhs_points, cv::RANSAC, 3.0, mask); + + if (H.empty()) { + throw std::runtime_error("estimateHomographyNPoints: Failed to compute homography"); + } + + return H; + } + + // pseudorandom number generator + inline uint64_t xorshift64(uint64_t *state) + { + if (*state == 0) { + *state = 1; + } + + uint64_t x = *state; + x ^= x << 13; + x ^= x >> 7; + x ^= x << 17; + return *state = x; + } + + void randomSample(std::vector &dst, int max_id, int sample_size, uint64_t *state) + { + dst.clear(); + + const int max_attempts = 1000; + + for (int i = 0; i < sample_size; ++i) { + for (int k = 0; k < max_attempts; ++k) { + int v = xorshift64(state) % max_id; + if (dst.empty() || std::find(dst.begin(), dst.end(), v) == dst.end()) { + dst.push_back(v); + break; + } + } + if (dst.size() < i + 1) { + throw std::runtime_error("Failed to sample ids"); + } + } + } + + 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()"); + } + + // 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) + // * [2] Adaptive Structure from Motion with a contrario model estimation. (Pierre Moulon, Pascal Monasse, Renaud Marlet) + // * (простое описание для понимания) + // * [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 + float p = 0.9; // lets put 0.9 as probability of success + // (1 - p) = (1 - w^n) ^ k + // w ^ n :: is the probability that all n points are inliers + // w :: number of inliers in data / number of points in data + // n_samples = number of inliers in data + // k :: number of iterations = n_trials + + + const int n_samples = int (0.8*n_matches); // number of inliers in dat + int n_trials = int (std::log(1 - p) / std::log(1 - std::pow(double(n_samples) / n_matches, 4))); // number of iterations + // lets set at least n_trials = 10 + if (n_trials < 10) { + n_trials = 5; + } + + std::cout << "estimateHomographyRANSAC : n_matches: " << n_matches << ", n_samples: " << n_samples << ", n_trials: " << n_trials << std::endl; + uint64_t seed = 1; + const double reprojection_error_threshold_px = 2.5; + + 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]]); + + cv::Mat H = estimateHomographyNPoints(points_lhs, points_rhs); + + 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; + } + +} + +cv::Mat phg::findHomography(const std::vector &points_lhs, const std::vector &points_rhs) +{ + return estimateHomographyRANSAC(points_lhs, points_rhs); +} + +// чтобы заработало, нужно пересобрать библиотеку с дополнительным модулем calib3d (см. инструкцию в корневом CMakeLists.txt) +cv::Mat phg::findHomographyCV(const std::vector &points_lhs, const std::vector &points_rhs) +{ + return cv::findHomography(points_lhs, points_rhs, cv::RANSAC); +} + +// T - 3x3 однородная матрица, например, гомография +// таким преобразованием внутри занимается функции cv::perspectiveTransform и cv::warpPerspective +cv::Point2d phg::transformPoint(const cv::Point2d &pt, const cv::Mat &T) +{ + // here we wanna transform point pt by transformation T. + // For example, if T is a homography: + // [x', y', w']^T = H * [x, y, 1]^T + // then we need to divide x' and y' by w' to get the final coordinates of the transformed point. + cv::Mat pt_homogeneous(3, 1, CV_64FC1); + pt_homogeneous.at(0, 0) = pt.x; + pt_homogeneous.at(1, 0) = pt.y; + pt_homogeneous.at(2, 0) = 1.0; + + + cv::Mat transformed_pt_homogeneous = T * pt_homogeneous; + return cv::Point2d(transformed_pt_homogeneous.at(0, 0) / transformed_pt_homogeneous.at(2, 0), + transformed_pt_homogeneous.at(1, 0) / transformed_pt_homogeneous.at(2, 0)); +} + +cv::Point2d phg::transformPointCV(const cv::Point2d &pt, const cv::Mat &T) { + // ineffective but ok for testing + std::vector tmp0 = {pt}; + std::vector tmp1(1); + cv::perspectiveTransform(tmp0, tmp1, T); + return tmp1[0]; +} diff --git a/src/phg/sfm/panorama_stitcher.cpp b/src/phg/sfm/panorama_stitcher.cpp index 8d76939..db8f94b 100644 --- a/src/phg/sfm/panorama_stitcher.cpp +++ b/src/phg/sfm/panorama_stitcher.cpp @@ -1,90 +1,101 @@ -#include "panorama_stitcher.h" -#include "homography.h" - -#include -#include - -/* - * imgs - список картинок - * parent - список индексов, каждый индекс указывает, к какой картинке должна быть приклеена текущая картинка - * этот список образует дерево, корень дерева (картинка, которая ни к кому не приклеивается, приклеиваются только к ней), в данном массиве имеет значение -1 - * homography_builder - функтор, возвращающий гомографию по паре картинок - * */ -cv::Mat phg::stitchPanorama(const std::vector &imgs, - const std::vector &parent, - std::function &homography_builder) -{ - const int n_images = imgs.size(); - - // склеивание панорамы происходит через приклеивание всех картинок к корню, некоторые приклеиваются не напрямую, а через цепочку других картинок - - // вектор гомографий, для каждой картинки описывает преобразование до корня - std::vector Hs(n_images); - { - // здесь надо посчитать вектор Hs - // при этом можно обойтись n_images - 1 вызовами функтора homography_builder - throw std::runtime_error("not implemented yet"); - } - - bbox2 bbox; - for (int i = 0; i < n_images; ++i) { - double w = imgs[i].cols; - double h = imgs[i].rows; - bbox.grow(phg::transformPoint(cv::Point2d(0.0, 0.0), Hs[i])); - bbox.grow(phg::transformPoint(cv::Point2d(w, 0.0), Hs[i])); - bbox.grow(phg::transformPoint(cv::Point2d(w, h), Hs[i])); - bbox.grow(phg::transformPoint(cv::Point2d(0, h), Hs[i])); - } - - std::cout << "bbox: " << bbox.max() << ", " << bbox.min() << std::endl; - - int result_width = bbox.width() + 1; - int result_height = bbox.height() + 1; - - cv::Mat result = cv::Mat::zeros(result_height, result_width, CV_8UC3); - - // из-за растяжения пикселей при использовании прямой матрицы гомографии после отображения между пикселями остается пустое пространство - // лучше использовать обратную и для каждого пикселя на итоговвой картинке проверять, с какой картинки он может получить цвет - // тогда в некоторых пикселях цвет будет дублироваться, но изображение будет непрерывным -// for (int i = 0; i < n_images; ++i) { -// for (int y = 0; y < imgs[i].rows; ++y) { -// for (int x = 0; x < imgs[i].cols; ++x) { -// cv::Vec3b color = imgs[i].at(y, x); -// -// cv::Point2d pt_dst = applyH(cv::Point2d(x, y), Hs[i]) - bbox.min(); -// int y_dst = std::max(0, std::min((int) std::round(pt_dst.y), result_height - 1)); -// int x_dst = std::max(0, std::min((int) std::round(pt_dst.x), result_width - 1)); -// -// result.at(y_dst, x_dst) = color; -// } -// } -// } - - std::vector Hs_inv; - std::transform(Hs.begin(), Hs.end(), std::back_inserter(Hs_inv), [&](const cv::Mat &H){ return H.inv(); }); - -#pragma omp parallel for - for (int y = 0; y < result_height; ++y) { - for (int x = 0; x < result_width; ++x) { - - cv::Point2d pt_dst(x, y); - - // test all images, pick first - for (int i = 0; i < n_images; ++i) { - - cv::Point2d pt_src = phg::transformPoint(pt_dst + bbox.min(), Hs_inv[i]); - - int x_src = std::round(pt_src.x); - int y_src = std::round(pt_src.y); - - if (x_src >= 0 && x_src < imgs[i].cols && y_src >= 0 && y_src < imgs[i].rows) { - result.at(y, x) = imgs[i].at(y_src, x_src); - break; - } - } - - } - } - - return result; -} +#include "panorama_stitcher.h" +#include "homography.h" + +#include +#include + +/* + * imgs - список картинок + * parent - список индексов, каждый индекс указывает, к какой картинке должна быть приклеена текущая картинка + * этот список образует дерево, корень дерева (картинка, которая ни к кому не приклеивается, приклеиваются только к ней), в данном массиве имеет значение -1 + * homography_builder - функтор, возвращающий гомографию по паре картинок + * */ +cv::Mat phg::stitchPanorama(const std::vector& imgs, const std::vector& parent, std::function& homography_builder) +{ + const int n_images = imgs.size(); + + // Compute homographies for each image relative to the root + std::vector Hs(n_images); + std::vector already_calculated(n_images, false); + std::vector parent_Hs(n_images); + auto calc_Hs = [&](auto&& self, const int& imgs_idx) -> const cv::Mat& { + if (already_calculated[imgs_idx]) + return Hs[imgs_idx]; + if (parent[imgs_idx] < 0) { + Hs[imgs_idx] = cv::Mat::eye(3, 3, CV_64FC1); + already_calculated[imgs_idx] = true; + return Hs[imgs_idx]; + } + + if (parent_Hs[imgs_idx].empty()) + parent_Hs[imgs_idx] = homography_builder(imgs[imgs_idx], imgs[parent[imgs_idx]]); + + Hs[imgs_idx] = self(self, parent[imgs_idx]) * parent_Hs[imgs_idx]; + already_calculated[imgs_idx] = true; + return Hs[imgs_idx]; + }; + for (int i = 0; i < n_images; ++i) { + calc_Hs(calc_Hs, i); + } + + // Compute the bounding box for the panorama + bbox2 bbox; + for (int i = 0; i < n_images; ++i) { + double w = imgs[i].cols; + double h = imgs[i].rows; + bbox.grow(phg::transformPoint(cv::Point2d(0.0, 0.0), Hs[i])); + bbox.grow(phg::transformPoint(cv::Point2d(w, 0.0), Hs[i])); + bbox.grow(phg::transformPoint(cv::Point2d(w, h), Hs[i])); + bbox.grow(phg::transformPoint(cv::Point2d(0, h), Hs[i])); + } + + std::cout << "bbox: " << bbox.max() << ", " << bbox.min() << std::endl; + + int result_width = bbox.width() + 1; + int result_height = bbox.height() + 1; + + cv::Mat result = cv::Mat::zeros(result_height, result_width, CV_8UC3); + + // из-за растяжения пикселей при использовании прямой матрицы гомографии после отображения между пикселями остается пустое пространство + // лучше использовать обратную и для каждого пикселя на итоговвой картинке проверять, с какой картинки он может получить цвет + // тогда в некоторых пикселях цвет будет дублироваться, но изображение будет непрерывным + // for (int i = 0; i < n_images; ++i) { + // for (int y = 0; y < imgs[i].rows; ++y) { + // for (int x = 0; x < imgs[i].cols; ++x) { + // cv::Vec3b color = imgs[i].at(y, x); + + // cv::Point2d pt_dst = applyH(cv::Point2d(x, y), Hs[i]) - bbox.min(); + // int y_dst = std::max(0, std::min((int) std::round(pt_dst.y), result_height - 1)); + // int x_dst = std::max(0, std::min((int) std::round(pt_dst.x), result_width - 1)); + + // result.at(y_dst, x_dst) = color; + // } + // } + // } + + std::vector Hs_inv; + std::transform(Hs.begin(), Hs.end(), std::back_inserter(Hs_inv), [&](const cv::Mat& H) { return H.inv(); }); + + // Blend all images into the panorama +#pragma omp parallel for + for (int y = 0; y < result_height; ++y) { + for (int x = 0; x < result_width; ++x) { + cv::Point2d pt_dst(x, y); + + // Test all images, pick the first valid one + for (int i = 0; i < n_images; ++i) { + cv::Point2d pt_src = phg::transformPoint(pt_dst + bbox.min(), Hs_inv[i]); + + int x_src = std::round(pt_src.x); + int y_src = std::round(pt_src.y); + +if (x_src >= 0 && x_src < imgs[i].cols && y_src >= 0 && y_src < imgs[i].rows) { + result.at(y, x) = imgs[i].at(y_src, x_src); + break; + } + } + } + } + + return result; +} diff --git a/src/phg/sfm/resection.cpp b/src/phg/sfm/resection.cpp index d2cf643..d439596 100644 --- a/src/phg/sfm/resection.cpp +++ b/src/phg/sfm/resection.cpp @@ -49,95 +49,107 @@ namespace { // (см. Hartley & Zisserman p.178) cv::Matx34d estimateCameraMatrixDLT(const cv::Vec3d *Xs, const cv::Vec3d *xs, int count) { - throw std::runtime_error("not implemented yet"); -// using mat = Eigen::MatrixXd; -// using vec = Eigen::VectorXd; -// -// mat A(TODO); -// -// for (int i = 0; i < count; ++i) { -// -// double x = xs[i][0]; -// double y = xs[i][1]; -// double w = xs[i][2]; -// -// double X = Xs[i][0]; -// double Y = Xs[i][1]; -// double Z = Xs[i][2]; -// double W = 1.0; -// -// TODO -// } -// -// matrix34d result; -// TODO -// -// return canonicalizeP(result); + + using mat = Eigen::MatrixXd; + using vec = Eigen::VectorXd; + + mat A(count * 2, 12); + + for (int i = 0; i < count; ++i) { + + double x = xs[i][0]; + double y = xs[i][1]; + double w = xs[i][2]; + + double X = Xs[i][0]; + double Y = Xs[i][1]; + double Z = Xs[i][2]; + double W = 1.0; + + // we fill A with coefficients from equations x (cross) PX = 0, where x is a homogeneous image point, P is the camera matrix, and X is a homogeneous 3D point + A.row(i * 2) << 0, 0, 0, 0, -w * X, -w * Y, -w * Z, -w * W, y * X, y * Y, y * Z, y * W; + A.row(i * 2 + 1) << w * X, w * Y, w * Z, w * W, 0, 0, 0, 0, -x * X, -x * Y, -x * Z, -x * W; + + } + + matrix34d result; + + Eigen::JacobiSVD svd(A, Eigen::ComputeFullV); + Eigen::VectorXd null_space = svd.matrixV().col(11); // Last column of V corresponds to the solution + for (int i = 0; i < 12; ++i) { + result(i / 4, i % 4) = null_space(i); + } + + return canonicalizeP(result); } // По трехмерным точкам и их проекциям на изображении определяем положение камеры cv::Matx34d estimateCameraMatrixRANSAC(const phg::Calibration &calib, const std::vector &X, const std::vector &x) { - throw std::runtime_error("not implemented yet"); -// if (X.size() != x.size()) { -// throw std::runtime_error("estimateCameraMatrixRANSAC: X.size() != x.size()"); -// } -// -// const int n_points = X.size(); -// -// // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters -// // будет отличаться от случая с гомографией -// const int n_trials = TODO; -// -// const double threshold_px = 3; -// -// const int n_samples = TODO; -// uint64_t seed = 1; -// -// int best_support = 0; -// cv::Matx34d best_P; -// -// std::vector sample; -// for (int i_trial = 0; i_trial < n_trials; ++i_trial) { -// phg::randomSample(sample, n_points, n_samples, &seed); -// -// cv::Vec3d ms0[n_samples]; -// cv::Vec3d ms1[n_samples]; -// for (int i = 0; i < n_samples; ++i) { -// ms0[i] = TODO; -// ms1[i] = TODO; -// } -// -// cv::Matx34d P = estimateCameraMatrixDLT(ms0, ms1, n_samples); -// -// int support = 0; -// for (int i = 0; i < n_points; ++i) { -// cv::Vec2d px = TODO спроецировать 3Д точку в пиксель с использованием P и calib; -// if (cv::norm(px - x[i]) < threshold_px) { -// ++support; -// } -// } -// -// if (support > best_support) { -// best_support = support; -// best_P = P; -// -// std::cout << "estimateCameraMatrixRANSAC : support: " << best_support << "/" << n_points << std::endl; -// -// if (best_support == n_points) { -// break; -// } -// } -// } -// -// std::cout << "estimateCameraMatrixRANSAC : best support: " << best_support << "/" << n_points << std::endl; -// -// if (best_support == 0) { -// throw std::runtime_error("estimateCameraMatrixRANSAC : failed to estimate camera matrix"); -// } -// -// return best_P; + + if (X.size() != x.size()) { + throw std::runtime_error("estimateCameraMatrixRANSAC: X.size() != x.size()"); + } + + const int n_points = X.size(); + + // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters + // будет отличаться от случая с гомографией + const int n_trials = 40000; + const double threshold_px = 3; + const int n_samples = 8; + uint64_t seed = 1; + + int best_support = 0; + cv::Matx34d best_P; + + std::vector sample; + for (int i_trial = 0; i_trial < n_trials; ++i_trial) { + phg::randomSample(sample, n_points, n_samples, &seed); + + cv::Vec3d ms0[n_samples]; + cv::Vec3d ms1[n_samples]; + // prepare samples for DLT: unproject 2D points to normalized image coordinates using calibration + for (int i = 0; i < n_samples; ++i) { + ms0[i] = cv::Vec3d(X[sample[i]][0], X[sample[i]][1], X[sample[i]][2]); + ms1[i] = calib.unproject(x[sample[i]]); + + } + + cv::Matx34d P = estimateCameraMatrixDLT(ms0, ms1, n_samples); + // after we found P we can check how many points from the input data set are inliers, i.e. have projections close to the observed ones + int support = 0; + for (int i = 0; i < n_points; ++i) { + cv::Vec3d proj = P * cv::Vec4d(X[i][0], X[i][1], X[i][2], 1.0); + cv::Vec3d px3 = calib.project(proj); + // cv::Vec3d px_normed(px3[0]/px3[2], px3[1]/px3[2], 1.); + + cv::Vec2d px = cv::Vec2d(px3[0]/px3[2], px3[1]/px3[2]); + if (cv::norm(px - x[i]) < threshold_px) { // reprojection error (!!!) is less than threshold + ++support; + } + } + + if (support > best_support) { + best_support = support; + best_P = P; + + std::cout << "estimateCameraMatrixRANSAC : support: " << best_support << "/" << n_points << std::endl; + + if (best_support == n_points) { + break; + } + } + } + + std::cout << "estimateCameraMatrixRANSAC : best support: " << best_support << "/" << n_points << std::endl; + + if (best_support == 0) { + throw std::runtime_error("estimateCameraMatrixRANSAC : failed to estimate camera matrix"); + } + + return best_P; } diff --git a/src/phg/sfm/sfm_utils.cpp b/src/phg/sfm/sfm_utils.cpp index d2d2e29..666c0cc 100644 --- a/src/phg/sfm/sfm_utils.cpp +++ b/src/phg/sfm/sfm_utils.cpp @@ -41,5 +41,15 @@ void phg::randomSample(std::vector &dst, int max_id, int sample_size, uint6 // проверяет, что расстояние от точки до линии меньше порога bool phg::epipolarTest(const cv::Vec2d &pt0, const cv::Vec2d &pt1, const cv::Matx33d &F, double t) { - throw std::runtime_error("not implemented yet"); + // throw std::runtime_error("not implemented yet"); + // compute the corresponding epipolar line in the second image. : + cv::Vec3d l = F * cv::Vec3d(pt0[0], pt0[1], 1); + + // For point pt1 in the second image, calculate its distance to the epipolar line : + double d = std::abs(l[0] * pt1[0] + l[1] * pt1[1] + l[2]) / std::sqrt(l[0] * l[0] + l[1] * l[1]); + + // verify if pt1 lies on the epipolar line within a specified threshold : + return d < t; + + } diff --git a/src/phg/sfm/triangulation.cpp b/src/phg/sfm/triangulation.cpp index 8dd11e6..f6840fa 100644 --- a/src/phg/sfm/triangulation.cpp +++ b/src/phg/sfm/triangulation.cpp @@ -11,6 +11,39 @@ cv::Vec4d phg::triangulatePoint(const cv::Matx34d *Ps, const cv::Vec3d *ms, int count) { // составление однородной системы + SVD - // без подвохов - throw std::runtime_error("not implemented yet"); + // Ps is a camera matrix, ms is a normalized image point (after applying inverse of K), + // count is number of views + // For two cameras (P, and P2) and observed image points (x1, x2), + // this results in 4 linear equations (2 from each image) + + int a_rows = count * 2; // number of equations + int a_cols = 4; // homogeneous coordinates of the point + + // A matrix construction + Eigen::MatrixXd A(a_rows, a_cols); + + for (int i = 0; i < count; ++i) { + const cv::Matx34d &P = Ps[i]; + const cv::Vec3d &m = ms[i]; + + // First row of equations + A.row(i * 2) << m[0] * P(2, 0) - P(0, 0), + m[0] * P(2, 1) - P(0, 1), + m[0] * P(2, 2) - P(0, 2), + m[0] * P(2, 3) - P(0, 3); + + // Second row of equations + A.row(i * 2 + 1) << m[1] * P(2, 0) - P(1, 0), + m[1] * P(2, 1) - P(1, 1), + m[1] * P(2, 2) - P(1, 2), + m[1] * P(2, 3) - P(1, 3); + } + + // Solve the homogeneous system using SVD + Eigen::JacobiSVD svd(A, Eigen::ComputeFullV); + Eigen::VectorXd X = svd.matrixV().col(3); // Last column of V corresponds to the solution + + // Return the triangulated point in homogeneous coordinates + + return cv::Vec4d(X(0), X(1), X(2), X(3)); } diff --git a/src/phg/sift/sift.cpp b/src/phg/sift/sift.cpp index 7204771..3a27f34 100755 --- a/src/phg/sift/sift.cpp +++ b/src/phg/sift/sift.cpp @@ -1,785 +1,905 @@ -#include "sift.h" -#include "libutils/rasserts.h" - -#include -#include -#include -#include -#include - -// Ссылки: -// [lowe04] - Distinctive Image Features from Scale-Invariant Keypoints, David G. Lowe, 2004 -// -// Примеры реализаций (стоит обращаться только если совсем не понятны какие-то места): -// 1) https://github.com/robwhess/opensift/blob/master/src/sift.c -// 2) https://gist.github.com/lxc-xx/7088609 -// 3) https://github.com/opencv/opencv/blob/1834eed8098aa2c595f4d1099eeaa0992ce8b321/modules/features2d/src/sift.dispatch.cpp -// 4) https://github.com/opencv/opencv/blob/1834eed8098aa2c595f4d1099eeaa0992ce8b321/modules/features2d/src/sift.simd.hpp - -namespace { - -cv::Mat upsample2x(const cv::Mat& src) -{ - cv::Mat dst; - cv::resize(src, dst, cv::Size(src.cols * 2, src.rows * 2), 0, 0, cv::INTER_LINEAR); - return dst; -} - -cv::Mat downsample2x(const cv::Mat& src) -{ - int dstW = src.cols / 2; - int dstH = src.rows / 2; - cv::Mat dst(dstH, dstW, src.type()); - const int ch = src.channels(); - - for (int y = 0; y < dstH; y++) { - const float* srcRow = src.ptr(y * 2); - float* dstRow = dst.ptr(y); - for (int x = 0; x < dstW; x++) { - std::copy(srcRow + x * 2 * ch, srcRow + x * 2 * ch + ch, dstRow + x * ch); - } - } - return dst; -} - -[[maybe_unused]] cv::Mat downsample2x_avg(const cv::Mat& src) -{ - int dstW = src.cols / 2; - int dstH = src.rows / 2; - cv::Mat dst(dstH, dstW, src.type()); - - for (int y = 0; y < dstH; y++) { - const float* r0 = src.ptr(y * 2); - const float* r1 = src.ptr(y * 2 + 1); - float* dstRow = dst.ptr(y); - for (int x = 0; x < dstW; x++) { - dstRow[x] = (r0[x * 2] + r0[x * 2 + 1] + r1[x * 2] + r1[x * 2 + 1]) * 0.25f; - } - } - return dst; -} -} - -cv::Mat phg::toGray32F(const cv::Mat& img) -{ - cv::Mat gray; - if (img.channels() == 3) { - cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY); - } else if (img.channels() == 4) { - cv::cvtColor(img, gray, cv::COLOR_BGRA2GRAY); - } else { - gray = img; - } - - cv::Mat gray_float; - gray.convertTo(gray_float, CV_32F, 1.0 / 255.0); - return gray_float; -} - -std::vector phg::buildOctaves(const cv::Mat& img, const phg::SIFTParams& p, int verbose_level) -{ - const int s = p.n_octave_layers; - const double sigma0 = p.sigma; - // взятое с потолка значение блюра который уже есть в картинке. используем для того, чтобы не так сильно блюрить базовую картинку и не терять лишний раз фичи - // upd: хотя llm не соглашается со "взятое с потолка": - // It is strictly not taken from the ceiling. sigma=0.5 is the theoretical minimum blur needed to prevent aliasing (Nyquist frequency) - // when sampling a continuous signal into a discrete grid. It is a mathematically grounded assumption for digital images. - // общая идея в том, что у нас есть какой-то сигнал реального мира, и есть входное изображение - // сигнал реального мира: потенциально высочайшего разрешения, можем зумиться почти до молекул, сигма почти нулевая - // сигнал с камеры, входное изображение: было произведено усреднение хотя бы по отдельным пикселям матрицы камеры, что соответствует сигме в полпикселя - const double sigma_nominal = p.upscale_first ? 1.0 /*2x от неапскейленного*/ : 0.5; - const int n_layers = s + 3; // нужно +2 слоя для того чтобы крайних было по соседу для поиска максимума в scale space, и еще +1 слой, чтобы получить s DoG слоев (DoG = разность двух) - - int n_octaves = std::max(1, (int)std::round(std::log2(std::min(img.cols, img.rows))) - 3); // не даунскейлим дальше размера картинки в 16 пикселей, там уже не имеет смысла что-то детектировать - - cv::Mat base; - double sigma_base = std::sqrt(sigma0 * sigma0 - sigma_nominal * sigma_nominal); // можно использовать дальше как идею для инкрементального блюра слоев - cv::GaussianBlur(img, base, cv::Size(), sigma_base, sigma_base); - - std::vector octaves(n_octaves); - - for (int o = 0; o < n_octaves; o++) { - phg::SIFT::Octave& oct = octaves[o]; - oct.layers.resize(n_layers); - - oct.layers[0] = base.clone(); - - // для простоты в каждой октаве будем каждый раз блюрить базовую картинку с полной сигмой - // можно подумать, как сделать эффективнее - для построения n+1 слоя доблюревать уже поблюренный n-ый слой, так чтобы в итоге получилась такая же сигма - // это будет немного быстрее, тк нужно более маленькое ядро свертки на каждый шаг - 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); - } - - // подготавливаем базовый слой для следующей октавы - 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 - - if (verbose_level) - std::cout << "new octave base size: " << base.size().width << std::endl; - } - } - - return octaves; -} - -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-й гауссианы - } - - return dog; -} - -std::vector phg::findScaleSpaceExtrema(const std::vector& dog, const phg::SIFTParams& params, int verbose_level) -{ - const int s = params.n_octave_layers; - const double sigma0 = params.sigma; - const double contrast_threshold = params.contrast_threshold; - const double edge_threshold = params.edge_threshold; - - // чем больше слоев в октаве, тем меньше разница между ними -> компенсируем порог - const float thresh = (float)(contrast_threshold / s); - - const int border = 5; - - const int max_interp_steps = 5; - - const int first_octave = params.upscale_first ? -1 : 0; - - std::vector keypoints; - - for (int o = 0; o < (int)dog.size(); o++) { - int real_octave = o + first_octave; - - const std::vector& dog_layers = dog[o].layers; - const int n_dog_layers = (int)dog_layers.size(); - rassert(n_dog_layers == s + 2, 2138971238612312); - - // итерируемся по внутренним слоям пирамиды, у нас всегда есть предыдущий и следующий сосед - for (int layer = 1; layer <= s; layer++) { - const cv::Mat& dog_curr = dog_layers[layer]; - const cv::Mat& dog_prev = dog_layers[layer - 1]; - const cv::Mat& dog_next = dog_layers[layer + 1]; - - int rows = dog_curr.rows, cols = dog_curr.cols; - - for (int y = border; y < rows - border; y++) { - const float* c = dog_curr.ptr(y); - const float* cp = dog_curr.ptr(y - 1); - const float* cn = dog_curr.ptr(y + 1); - - const float* p = dog_prev.ptr(y); - const float* pp = dog_prev.ptr(y - 1); - const float* pn = dog_prev.ptr(y + 1); - - const float* n = dog_next.ptr(y); - const float* np = dog_next.ptr(y - 1); - const float* nn = dog_next.ptr(y + 1); - - for (int x = border; x < cols - border; x++) { - float val = c[x]; - - // cascade filtering: предварительная слабая проверка, отбрасываем точку если она не дает хотя бы половину от требуемого порога, - // в надежде что потом после оптимизации порог будет пробит - // так мы и отбрасываем кучу мусора и не слишком строго судим точки которые пока еще не оптимальные - if (std::abs(val) < thresh * 0.5f) - continue; - - bool is_max = true, is_min = true; - auto check = [&](float v) { - if (v >= val) - is_max = false; - if (v <= val) - is_min = false; - }; - - // TODO проверить локальный максимум на текущем скейле - - if (!is_max && !is_min) - continue; - - // TODO проверить локальный максимум на предыдущем скейле - - if (!is_max && !is_min) - continue; - - // TODO проверить локальный максимум на следующем скейле - - if (!is_max && !is_min) - continue; - - int xi = x, yi = y, li = layer; - - for (int step = 0; step < max_interp_steps; step++) { - const cv::Mat& cL = dog_layers[li]; - const cv::Mat& pL = dog_layers[li - 1]; - const cv::Mat& nL = dog_layers[li + 1]; - - float resp_center = cL.at(yi, xi); - - // градиент - float dx = (cL.at(yi, xi + 1) - cL.at(yi, xi - 1)) * 0.5f; - float dy = (cL.at(yi + 1, xi) - cL.at(yi - 1, xi)) * 0.5f; - float ds = (nL.at(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; - - cv::Matx33f H(dxx, dxy, dxs, dxy, dyy, dys, dxs, dys, dss); - - cv::Vec3f g(dx, dy, ds); - - // в нашей точке производная (градиент) еще не равна нулю (т.к. еще мы скорее всего не точно в оптимуме) - // хотим найти такой offset, где ноль производной. в предположении что оптимизируемая функция это парабола, ищем корни ее производной, линейной функции - // grad(x + offset) = grad(x) + grad'(x) * offset = grad(x) + hessian(x) * offset = 0 - // hessian(x) * offset = -grad(x) // линейная система. можно решить специализированным решателем либо просто найти обратную матрицу гессиана и домножить на минус градиент - // offset = -hessian(x)^-1 * grad(x) - - cv::Vec3f offset; - if (!cv::solve(H, -g, offset, cv::DECOMP_LU)) - break; - - if (std::abs(offset[0]) < 0.5f && std::abs(offset[1]) < 0.5f && std::abs(offset[2]) < 0.5f) { - - // функцию респонза оптимизировали как параболу: D(x + offset) = D(x) + grad(x) * offset + 1/2 * offset_transposed * hessian(x) * offset - // подставляем hessian(x) * offset = -grad(x): D(x + offset) = D(x) + grad(x) * offset - 1/2 * offset_transposed * grad = D(x) + 1/2 * grad(x) * offset - float response_optimized = resp_center + 0.5f * g.dot(offset); - if (std::abs(response_optimized) < thresh) - break; - - // фильтрация по зацепистости - if (params.enable_edge_like_filtering) { - // из линейной алгебры, сумма диагональных элементов матрицы (след) равна сумме собственных чисел - // определитель матрицы равен произведению собственных чисел - // в случае гессиана (пространственной части: (dxx dxy, dxy, dyy)), собственные числа lambda1, lambda2 - силы кривизны в направлении максимальной кривизны и в ортогональном -// float trace = //TODO ; // = lambda1 + lambda2 -// float det = // TODO ; // = lambda1 * lambda2 -// if (det <= 0) -// break; // если произведение кривизн отрицательное, то мы находимся в седловой точке, а не в максимуме/минимуме. если нулевое, то это ровная граница вообще -// -// // если граница незацепистая = грань, то одна кривизна сильно больше чем другая. хотим, чтобы обе кривизны были примерно сопоставимы -// // тогда их отношение r = lambda1/lambda2 будет не очень большим -// // если расписать trace * trace / det через r, то получится (r + 1) ^ 2 / r -// // функция растущая по r, так что если наше фактическое значение trace * trace / det выше (r + 1) ^ 2 / r, то и наше отношение кривизн больше порога, значит плохая зацепистость -// // и просто как интуиция, при больших r это выражение просто до r сокращается -// -// // в итоге получается что порог edge_threshold в отличие от response_threshold наоборот, чем больше тем расслабленнее -// float r = edge_threshold; -// if (TODO) -// break; - } - - // скейлим координаты точек обратно до родных размеров картинки - // !!! если выбираем при даунскейле другую политику, с усреднением вместо ресемплинга, то надо здесь применять формулу со сдвигами на полпикселя - float scale = (real_octave >= 0) ? (float)(1 << real_octave) : (1.f / (float)(1 << (-real_octave))); - float real_x = (xi + offset[0]) * scale; - float real_y = (yi + offset[1]) * scale; - float real_layer = li + offset[2]; - - if (!params.enable_subpixel_localization) { - real_x = x * scale; - real_y = y * scale; - real_layer = layer; - } - - float kp_sigma = (float)(sigma0 * std::pow(2.0, (double)real_layer / s) * scale); - - cv::KeyPoint kp; - kp.pt.x = real_x; - kp.pt.y = real_y; - kp.size = kp_sigma * 2.f; // диаметр - kp.octave = real_octave; - kp.class_id = li; // в настоящей opencv имплементации и слой и октава запихиваются в поле octave битовыми операциями - kp.response = std::abs(response_optimized); - keypoints.push_back(kp); - break; - } - - // это на случай если не зашли в предыдущий if: если оптимизированная точка вылетела за границы нашего пикселя, то делаем еще шаг - // идея в том, что если максимум реально там (а у нас же неидеальная парабола), то оптимизировав в той клеточке еще раз, если мы получим маленький сдвиг, то подтвердим минимум и успокоимся - // а если снова вылетим из пикселя, то либо поищем минимум еще, либо устанем и забьем - xi += cvRound(offset[0]); - yi += cvRound(offset[1]); - li += cvRound(offset[2]); - - if (li < 1 || li > s || xi < border || xi >= cols - border || yi < border || yi >= rows - border) - break; - } - } - } - } - - if (verbose_level) - std::cout << "octave " << o << ": " << keypoints.size() << " keypoints so far" << std::endl; - } - - if (verbose_level) - std::cout << "total keypoints: " << keypoints.size() << std::endl; - - return keypoints; -} - -std::vector phg::computeOrientations(const std::vector& kpts, const std::vector& octaves, const phg::SIFTParams& params, int verbose_level) -{ - const int s = params.n_octave_layers; - const double sigma0 = params.sigma; - const int n_bins = params.orient_nbins; - const double peak_ratio = params.orient_peak_ratio; - - std::vector histogram(n_bins); - - std::vector oriented_kpts; - - const int first_octave = params.upscale_first ? -1 : 0; - - for (const cv::KeyPoint& kp : kpts) { - int layer = kp.class_id; - int real_octave = kp.octave; - int o = real_octave - first_octave; // индекс в массиве octaves - - const cv::Mat& img = octaves[o].layers[layer]; - - float scale = (real_octave >= 0) ? (float)(1 << real_octave) : (1.f / (float)(1 << (-real_octave))); - float x = kp.pt.x / scale; - float y = kp.pt.y / scale; - - // найдем радиус ключевой точки в координатах ее октавы - float kp_sigma_octave = (float)(sigma0 * std::pow(2.0, (double)layer / s)); - float sigma_win = 1.5f * kp_sigma_octave; // цитата из lowe: "Each sample added to the histogram is weighted by its gradient magnitude and by a Gaussian-weighted circular window with a σ that is 1.5 times that of the scale of the keypoint." - int radius = (int)std::round(3.f * sigma_win); - - int xi = (int)std::round(x); - int yi = (int)std::round(y); - - if (xi - radius <= 0 || xi + radius >= img.cols - 1 || yi - radius <= 0 || yi + radius >= img.rows - 1) - continue; - - histogram.assign(n_bins, 0.0); - - for (int dy = -radius; dy <= radius; dy++) { - for (int dx = -radius; dx <= radius; dx++) { -// 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 = 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; - } - } - - // немного сгладим гистограмму: сделаем несколько проходов box-blur (повторный box blur приближает гауссово размытие) - for (int iter = 0; iter < 6; iter++) { - float first = histogram[0]; - float prev = histogram[n_bins - 1]; - for (int i = 0; i < n_bins - 1; i++) { - float tmp = histogram[i]; - histogram[i] = (prev + histogram[i] + histogram[i + 1]) / 3.f; - prev = tmp; - } - histogram[n_bins - 1] = (prev + histogram[n_bins - 1] + first) / 3.f; - } - - // находим порог: все максимумы сильнее чем peak_ratio * max_val будут приняты и сгенерирована точка - // таким образом, на одну задетектированную точку может быть порождено несколько ориентированных точек, если сложно определить однозначно, куда она была направлена - float max_val = *std::max_element(histogram.begin(), histogram.end()); - - for (int i = 0; i < n_bins; i++) { - int prev = (i + n_bins - 1) % n_bins; - int next = (i + 1) % n_bins; - - // если локальный максимум и респонз больше порога - if (histogram[i] > histogram[prev] && histogram[i] > histogram[next] && histogram[i] >= peak_ratio * max_val) { - float left = histogram[prev]; - float center = histogram[i]; - float right = histogram[next]; - - // хотим найти угол дескриптора точнее = зафитить параболу по трем точкам (i-1, left), (i, center), (i+1, right) - // у параболы f(x) = ax^2 + bx + c, экстремум в точке x = offset = -b/(2a) - // f(-1) = left, f(0) = center, f(1) = right - // f(0) = c = center - // f(1) = a + b + c = right - // f(-1) = a - b + c = left - // f(1) + f(-1) = 2a + 2c -> 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); - } - } - } - - if (verbose_level) - std::cout << "orientations: " << kpts.size() << " -> " << oriented_kpts.size() << " keypoints" << std::endl; - - return oriented_kpts; -} - -// дескриптор подсчитывается по более широкой окрестности. если она выходит за границы изображения, точка может быть отброшена, в результате чего массив kpts может измениться -std::pair> phg::computeDescriptors(const std::vector& kpts, const std::vector& octaves, const phg::SIFTParams& params, int verbose_level) -{ - const int s = params.n_octave_layers; - const double sigma0 = params.sigma; - - // будем считать дескриптор внутри патча вокруг ключевой точки - // структура патча: 4x4 сетка, в каждой клетке гистограмма градиентов на 8 бинов - const int n_spatial_bins = 4; - const int n_orient_bins = 8; - const int n_dims = n_spatial_bins * n_spatial_bins * n_orient_bins; // 128 - - // размер одной клетки патча в сигмах. всего размер контекста для одного дексриптора = n_spatial_bins * spatial_bin_width_sigmas сигм - const float spatial_bin_width_sigmas = 3.f; // в сигмах - - const float mag_cap = 0.2f; - - std::vector valid_kpts; - cv::Mat descriptors; - - const int first_octave = params.upscale_first ? -1 : 0; - - for (const cv::KeyPoint& kp : kpts) { - int layer = kp.class_id; - int real_octave = kp.octave; - int o = real_octave - first_octave; // индекс в массиве octaves - - const cv::Mat& img = octaves[o].layers[layer]; - - float scale = (real_octave >= 0) ? (float)(1 << real_octave) : (1.f / (float)(1 << (-real_octave))); - float x = kp.pt.x / scale; - float y = kp.pt.y / scale; - - float kp_sigma_octave = (float)(sigma0 * std::pow(2.0, (double)layer / s)); - - // размер патча в котором считаем дескриптор в пикселях октавы - float spatial_bin_width = spatial_bin_width_sigmas * kp_sigma_octave; - - // изначально ширина дескриптора = spatial_bin_width * n_spatial_bins, но берем с запасом: - // * sqrt(2) для того, чтобы можно было посемплировать патч даже повернутый на 45 градусов ромбиком - // * +1 в скобках чтобы можно было семплировать градиенты (а еще зачем?) - float half_width = 0.5f * spatial_bin_width * (n_spatial_bins + 1) * std::sqrt(2.f); - int radius = (int)std::round(half_width); - - int xi = (int)std::round(x); - int yi = (int)std::round(y); - - if (xi - radius <= 0 || xi + radius >= img.cols - 1 || yi - radius <= 0 || yi + radius >= img.rows - 1) - continue; - - float kp_angle_rad = kp.angle * (float)CV_PI / 180.f; - float cos_a = std::cos(kp_angle_rad); - float sin_a = std::sin(kp_angle_rad); - - // для гауссового взвешивания: затухающий вклад градиентов с краев картинки - float sigma_desc = (float)n_spatial_bins * 0.5f; - - std::vector desc(n_dims, 0.f); - - // семплируем градиенты и кладем в гистограммы - for (int dy = -radius; dy <= radius; dy++) { - for (int dx = -radius; dx <= radius; dx++) { - int px = xi + dx; - int py = yi + dy; - - float rot_x = (cos_a * dx + sin_a * dy) / spatial_bin_width; - float rot_y = (-sin_a * dx + cos_a * dy) / spatial_bin_width; - - // подсчет пространственного бина - // бин 0 бин 1 | бин 2 бин 3 - // [-----------][-----------] [-----------][-----------] - // ^ - // центр ключевой точки (rot_x = rot_y = 0) - // центр нулевого бина в координатах rot находится в точке (-1.5, -1.5), а после сдвига перемещается в точку (0, 0), что удобно для индексирования - float bin_x = rot_x + n_spatial_bins * 0.5f - 0.5f; - float bin_y = rot_y + n_spatial_bins * 0.5f - 0.5f; - - if (bin_x < -1.f || bin_x >= (float)n_spatial_bins || bin_y < -1.f || bin_y >= (float)n_spatial_bins) - continue; - - // градиент (потом все равно будем все нормализовывать, так что можно не нормировать здесь) - 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); - - // инвариантность к повороту: повернем направление градиента на угол ключевой точки - float angle_invariant = angle - kp_angle_rad; - while (angle_invariant < 0.f) - angle_invariant += (float)CV_2PI; - while (angle_invariant >= (float)CV_2PI) - angle_invariant -= (float)CV_2PI; - - // подсчет бина в гистограммке градиентов внутри пространственного бина - float bin_o = angle_invariant * n_orient_bins / CV_2PI; - if (bin_o >= n_orient_bins) - 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; - - if (params.enable_descriptor_bin_interpolation) { - // размажем вклад weighted_mag по пространственным бинам и по бинам гистограммок трилинейной интерполяцией - - int ix0 = (int)std::floor(bin_x); - int iy0 = (int)std::floor(bin_y); - int io0 = (int)std::floor(bin_o); - - float fx = bin_x - ix0; - float fy = bin_y - iy0; - float fo = bin_o - io0; - - for (int diy = 0; diy <= 1; diy++) { - int iy = iy0 + diy; - if (iy < 0 || iy >= n_spatial_bins) - continue; - float wy = (diy == 0) ? (1.f - fy) : fy; - - for (int dix = 0; dix <= 1; dix++) { - int ix = ix0 + dix; - if (ix < 0 || ix >= n_spatial_bins) - continue; - float wx = (dix == 0) ? (1.f - fx) : fx; - - for (int dio = 0; dio <= 1; dio++) { - int io = (io0 + dio) % n_orient_bins; - if (io < 0) - io += n_orient_bins; - float wo = (dio == 0) ? (1.f - fo) : fo; - -// int idx = TODO; -// desc[idx] += TODO; - } - } - } - } else { - int ix_nearest = (int)std::round(bin_x); - int iy_nearest = (int)std::round(bin_y); - 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; - } - } - } - } - - // нормализуем дескриптор до единичной l2 длины - float norm = 0.f; - for (float v : desc) - norm += v * v; - norm = std::sqrt(norm) + 1e-7f; - for (float& v : desc) - v /= norm; - - // грохнем слишком большие градиенты и ренормализуем - // таким образом один выброс не потянет за собой весь дескриптор и в будущем расстояние с похожим соседом не вырастет сильно - for (float& v : desc) - v = std::min(v, mag_cap); - - norm = 0.f; - for (float v : desc) - norm += v * v; - norm = std::sqrt(norm) + 1e-7f; - for (float& v : desc) - v /= norm; - - if (descriptors.empty()) { - descriptors.create(0, n_dims, CV_32F); - } - - cv::Mat row(1, n_dims, CV_32F, desc.data()); - descriptors.push_back(row.clone()); - valid_kpts.push_back(kp); - } - - if (verbose_level) - std::cout << "descriptors: " << kpts.size() << " -> " << valid_kpts.size() << " keypoints (some discarded due to border)" << std::endl; - - return { descriptors, valid_kpts }; -} - -std::vector phg::selectTopKeypoints(const std::vector& kpts, const phg::SIFTParams& params, int verbose_level) -{ - if (params.nfeatures <= 0 || (int)kpts.size() <= params.nfeatures) { - return kpts; - } - - int nfeatures = params.nfeatures; - - std::vector idx(kpts.size()); - std::iota(idx.begin(), idx.end(), 0); - std::partial_sort(idx.begin(), idx.begin() + nfeatures, idx.end(), [&kpts](int a, int b) { return std::abs(kpts[a].response) > std::abs(kpts[b].response); }); - idx.resize(nfeatures); - std::sort(idx.begin(), idx.end()); - - std::vector sel_kpts(nfeatures); - for (int i = 0; i < nfeatures; ++i) { - sel_kpts[i] = kpts[idx[i]]; - } - - if (verbose_level) - std::cout << "retained top " << nfeatures << " keypoints by response" << std::endl; - - return sel_kpts; -} - -void phg::SIFT::detectAndCompute(const cv::Mat& img, const cv::Mat& mask, std::vector& kpts, cv::Mat& desc) const -{ - rassert(mask.empty(), 911738571854310); // not implemented, parameter added to match interface with opencv sift implementation - - saveImg("00_input.jpg", img); - - cv::Mat gray = toGray32F(img); - saveImg("01_gray.png", gray); - - if (p.upscale_first) { - auto prev_size = gray.size(); - gray = upsample2x(gray); - if (verbose_level) - std::cout << "upscaled image from " << prev_size.width << "x" << prev_size.height << " to " << gray.cols << "x" << gray.rows << std::endl; - saveImg("01b_gray_upscaled.png", gray); - } - - std::vector octaves = buildOctaves(gray, p, verbose_level); - savePyramid("pyramid/02_octave", octaves); - - std::vector dog = buildDoG(octaves, p, verbose_level); - savePyramid("pyramidDoG/03_dog_octave", dog, true); - - kpts = findScaleSpaceExtrema(dog, p, verbose_level); - // ориентация ключевых точек это довольно дорогая операция - // в случае если пользователь просит малое количество лучших точек (например, 1000, а без порога нашлось 20000), - // то по производительности очень оправдано сразу их здесь и выбрать, чтобы не тащить до самого конца где все равно отбросим - kpts = selectTopKeypoints(kpts, p, verbose_level); - - kpts = computeOrientations(kpts, octaves, p, verbose_level); - // после подсчета ориентаций количество могло возрасти (и скорее всего возросло) - // нужно снова выбрать лучшие точки чтобы уложиться в бюджет - kpts = selectTopKeypoints(kpts, p, verbose_level); - - if (verbose_level >= 2) { - cv::Mat kpts_img; - cv::drawKeypoints(img, kpts, kpts_img, cv::Scalar::all(-1), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS); - saveImg("04_keypoints.jpg", kpts_img); - } - - std::tie(desc, kpts) = computeDescriptors(kpts, octaves, p, verbose_level); - - // TODO всегда ли мы получаем ровно столько точек сколько запросили в параметре nfeatures? в каких случаях это не так и в какую сторону? - // как подкрутить алгоритм, чтобы всегда выдавать ровно запрошенное количество точек (когда это в принципе возможно) но не сильно просесть в производительности? -} - -void phg::SIFT::saveImg(const std::string& name, const cv::Mat& img) const -{ - if (verbose_level < 2 || debug_folder.empty()) { - return; - } - - cv::Mat out; - if (img.depth() == CV_32F) { - img.convertTo(out, CV_8U, 255.0); - } else { - out = img; - } - cv::imwrite(debug_folder + name, out); -} - -void phg::SIFT::savePyramid(const std::string& name, const std::vector& pyramid, bool normalize) const -{ - if (verbose_level < 2 || debug_folder.empty()) { - return; - } - - cv::Size size = pyramid.front().layers.front().size(); - - for (size_t o = 0; o < pyramid.size(); ++o) { - std::cout << "saving octave " << o << std::endl; - - const Octave& octave = pyramid[o]; - - for (size_t i = 0; i < octave.layers.size(); ++i) { - cv::Mat layer = octave.layers[i].clone(); - - cv::resize(layer, layer, size, 0, 0, cv::INTER_LINEAR); - - if (normalize) { - double mn, mx; - cv::minMaxLoc(layer, &mn, &mx); - if (mx - mn > 1e-8) { - layer = (layer - mn) / (mx - mn); - } else { - layer.setTo(0.5); - } - } - - std::stringstream ss; - ss << name << "_" << o << "_layer_" << i << ".png"; - saveImg(ss.str(), layer); - } - } -} +#include "sift.h" +#include "libutils/rasserts.h" + +#include +#include +#include +#include +#include + +// Ссылки: +// [lowe04] - Distinctive Image Features from Scale-Invariant Keypoints, David G. Lowe, 2004 +// +// Примеры реализаций (стоит обращаться только если совсем не понятны какие-то места): +// 1) https://github.com/robwhess/opensift/blob/master/src/sift.c +// 2) https://gist.github.com/lxc-xx/7088609 +// 3) https://github.com/opencv/opencv/blob/1834eed8098aa2c595f4d1099eeaa0992ce8b321/modules/features2d/src/sift.dispatch.cpp +// 4) https://github.com/opencv/opencv/blob/1834eed8098aa2c595f4d1099eeaa0992ce8b321/modules/features2d/src/sift.simd.hpp + +// My SIFT +namespace { + +cv::Mat upsample2x(const cv::Mat& src) +{ + cv::Mat dst; + cv::resize(src, dst, cv::Size(src.cols * 2, src.rows * 2), 0, 0, cv::INTER_LINEAR); + return dst; +} + +cv::Mat downsample2x(const cv::Mat& src) +{ + int dstW = src.cols / 2; + int dstH = src.rows / 2; + cv::Mat dst(dstH, dstW, src.type()); + const int ch = src.channels(); + + for (int y = 0; y < dstH; y++) { + const float* srcRow = src.ptr(y * 2); + float* dstRow = dst.ptr(y); + for (int x = 0; x < dstW; x++) { + std::copy(srcRow + x * 2 * ch, srcRow + x * 2 * ch + ch, dstRow + x * ch); + } + } + return dst; +} + +[[maybe_unused]] cv::Mat downsample2x_avg(const cv::Mat& src) +{ + int dstW = src.cols / 2; + int dstH = src.rows / 2; + cv::Mat dst(dstH, dstW, src.type()); + + for (int y = 0; y < dstH; y++) { + const float* r0 = src.ptr(y * 2); + const float* r1 = src.ptr(y * 2 + 1); + float* dstRow = dst.ptr(y); + for (int x = 0; x < dstW; x++) { + dstRow[x] = (r0[x * 2] + r0[x * 2 + 1] + r1[x * 2] + r1[x * 2 + 1]) * 0.25f; + } + } + return dst; +} +} + +cv::Mat phg::toGray32F(const cv::Mat& img) +{ + cv::Mat gray; + if (img.channels() == 3) { + cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY); + } else if (img.channels() == 4) { + cv::cvtColor(img, gray, cv::COLOR_BGRA2GRAY); + } else { + gray = img; + } + + cv::Mat gray_float; + gray.convertTo(gray_float, CV_32F, 1.0 / 255.0); + return gray_float; +} + +std::vector phg::buildOctaves(const cv::Mat& img, const phg::SIFTParams& p, int verbose_level) +{ + const int s = p.n_octave_layers; + const double sigma0 = p.sigma; + // взятое с потолка значение блюра который уже есть в картинке. используем для того, чтобы не так сильно блюрить базовую картинку и не терять лишний раз фичи + // upd: хотя llm не соглашается со "взятое с потолка": + // It is strictly not taken from the ceiling. sigma=0.5 is the theoretical minimum blur needed to prevent aliasing (Nyquist frequency) + // when sampling a continuous signal into a discrete grid. It is a mathematically grounded assumption for digital images. + // общая идея в том, что у нас есть какой-то сигнал реального мира, и есть входное изображение + // сигнал реального мира: потенциально высочайшего разрешения, можем зумиться почти до молекул, сигма почти нулевая + // сигнал с камеры, входное изображение: было произведено усреднение хотя бы по отдельным пикселям матрицы камеры, что соответствует сигме в полпикселя + const double sigma_nominal = p.upscale_first ? 1.0 /*2x от неапскейленного*/ : 0.5; + const int n_layers = s + 3; + // нужно +2 слоя для того чтобы крайних было по соседу для поиска максимума в scale space, + // и еще +1 слой, чтобы получить s DoG слоев (DoG = разность двух) + + int n_octaves = std::max(1, (int)std::round(std::log2(std::min(img.cols, img.rows))) - 3); // не даунскейлим дальше размера картинки в 16 пикселей, там уже не имеет смысла что-то детектировать + + cv::Mat base; + double sigma_base = std::sqrt(sigma0 * sigma0 - sigma_nominal * sigma_nominal); // можно использовать дальше как идею для инкрементального блюра слоев + cv::GaussianBlur(img, base, cv::Size(), sigma_base, sigma_base); + + std::vector octaves(n_octaves); + + + for (int o = 0; o < n_octaves; o++) { + phg::SIFT::Octave& oct = octaves[o]; + oct.layers.resize(n_layers); + + oct.layers[0] = base.clone(); + double sigma_layer = sigma0; + + // для простоты в каждой октаве будем каждый раз блюрить базовую картинку с полной сигмой + // можно подумать, как сделать эффективнее - для построения n+1 слоя доблюревать уже поблюренный n-ый слой, так чтобы в итоге получилась такая же сигма + // это будет немного быстрее, тк нужно более маленькое ядро свертки на каждый шаг + for (int i = 1; i < n_layers; i++) { + double k = std::pow(2.0, 1.0 / s); + double sigma_curr = sigma_layer * k; + double sigma_adjusted = std::sqrt(sigma_curr*sigma_curr - sigma_layer*sigma_layer ); // Incremental sigma + + oct.layers[i] = base.clone(); + cv::GaussianBlur(oct.layers[i - 1], oct.layers[i], cv::Size(), sigma_adjusted, sigma_adjusted); + sigma_layer = sigma_curr; + } + + // подготавливаем базовый слой для следующей октавы + if (o + 1 < n_octaves) { + + cv::Mat base_; + cv::resize(oct.layers[n_layers-3], base_, cv::Size(), 0.5, 0.5, cv::INTER_NEAREST); + base = base_; + // используется в opencv, формула для пересчета ключевых точек: pt_upscaled = 2^o * pt_downscaled + // T1ODO cv::resize(даунскейлим текущий слой в два раза, без интерполяции, просто сабсепмлинг); + + // можно использовать и downsample2x_avg(oct.layers[s]), это позволяет потом заапскейлить слои обратно до оригинального разрешения без сдвига + // но потребуется везде изменить формулу для пересчета ключевых точек: pt_upscaled = (pt_downscaled + 0.5) * 2^o - 0.5 + + if (verbose_level) + std::cout << "new octave base size: " << base.size().width << std::endl; + } + } + + return octaves; +} + +std::vector phg::buildDoG(const std::vector& octaves, const phg::SIFTParams& p, int verbose_level) +{ // difference of Gaissians + 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); + + for (int layer = 0; layer < octave.layers.size() - 1; layer++) { + dog[o].layers[layer] = octave.layers[layer + 1] - octave.layers[layer]; + } + + + // T1ODO каждый слой дога это разница n+1 и n-й гауссианы + } + + return dog; +} + +std::vector phg::findScaleSpaceExtrema(const std::vector& dog, const phg::SIFTParams& params, int verbose_level) +{ + const int s = params.n_octave_layers; + const double sigma0 = params.sigma; + const double contrast_threshold = params.contrast_threshold; + const double edge_threshold = params.edge_threshold; + + // чем больше слоев в октаве, тем меньше разница между ними -> компенсируем порог + const float thresh = (float)(contrast_threshold / s); + + const int border = 5; + + const int max_interp_steps = 5; + + const int first_octave = params.upscale_first ? -1 : 0; + + std::vector keypoints; + + for (int o = 0; o < (int)dog.size(); o++) { + int real_octave = o + first_octave; + + const std::vector& dog_layers = dog[o].layers; + const int n_dog_layers = (int)dog_layers.size(); + rassert(n_dog_layers == s + 2, 2138971238612312); + + // итерируемся по внутренним слоям пирамиды, у нас всегда есть предыдущий и следующий сосед + for (int layer = 1; layer <= s; layer++) { + const cv::Mat& dog_curr = dog_layers[layer]; + const cv::Mat& dog_prev = dog_layers[layer - 1]; + const cv::Mat& dog_next = dog_layers[layer + 1]; + + int rows = dog_curr.rows, cols = dog_curr.cols; + + for (int y = border; y < rows - border; y++) { + const float* c = dog_curr.ptr(y); + const float* cp = dog_curr.ptr(y - 1); + const float* cn = dog_curr.ptr(y + 1); + + const float* p = dog_prev.ptr(y); + const float* pp = dog_prev.ptr(y - 1); + const float* pn = dog_prev.ptr(y + 1); + + const float* n = dog_next.ptr(y); + const float* np = dog_next.ptr(y - 1); + const float* nn = dog_next.ptr(y + 1); + + for (int x = border; x < cols - border; x++) { + float val = c[x]; + + // cascade filtering: предварительная слабая проверка, отбрасываем точку если она не дает хотя бы + // половину от требуемого порога, + // в надежде что потом после оптимизации порог будет пробит + // так мы и отбрасываем кучу мусора и не слишком строго судим точки которые пока еще не оптимальные + if (std::abs(val) < thresh * 0.5f) + continue; + + bool is_max = true, is_min = true; + auto check = [&](float v) { + if (v >= val) + is_max = false; + if (v <= val) + is_min = false; + }; + + // check within current layer if val is max + check(c[x - 1]); + check(c[x + 1]); + check(cp[x]); + check(cp[x - 1]); + check(cp[x + 1]); + check(cn[x]); + check(cn[x - 1]); + check(cn[x + 1]); + + // std::cout << "val: " << val << ", is_max: " << is_max << ", is_min: " << is_min << std::endl; + // std::cout << "neighbors: " << c[x - 1] << ", " << c[x + 1] << ", " << cp[x - 1] << ", " << cp[x + 1] << ", " << cn[x - 1] << ", " << cn[x + 1] << std::endl; + + if (!is_max && !is_min) + continue; + + // T1ODO проверить локальный максимум на предыдущем скейле + check(p[x]); + check(p[x - 1]); + check(p[x + 1]); + check(pp[x]); + check(pp[x - 1]); + check(pp[x + 1]); + check(pn[x]); + check(pn[x - 1]); + check(pn[x + 1]); + + if (!is_max && !is_min) + continue; + + // T1ODO проверить локальный максимум на следующем скейле + check(n[x]); + check(n[x - 1]); + check(n[x + 1]); + check(np[x]); + check(np[x - 1]); + check(np[x + 1]); + check(nn[x]); + check(nn[x - 1]); + check(nn[x + 1]); + + + if (!is_max && !is_min) + continue; + + int xi = x, yi = y, li = layer; + + for (int step = 0; step < max_interp_steps; step++) { + const cv::Mat& cL = dog_layers[li]; + const cv::Mat& pL = dog_layers[li - 1]; + const cv::Mat& nL = dog_layers[li + 1]; + + float resp_center = cL.at(yi, xi); + + // градиент + float dx = (cL.at(yi, xi + 1) - cL.at(yi, xi - 1)) * 0.5f; + float dy = (cL.at(yi + 1, xi) - cL.at(yi - 1, xi)) * 0.5f; + float ds = (nL.at(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 = 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); + cv::Vec3f g(dx, dy, ds); + + // в нашей точке производная (градиент) еще не равна нулю (т.к. еще мы скорее всего не точно в оптимуме) + // хотим найти такой offset, где ноль производной. в предположении что оптимизируемая функция это парабола, + // ищем корни ее производной, линейной функции: + // grad(x + offset) = grad(x) + grad'(x) * offset = grad(x) + hessian(x) * offset = 0 + // hessian(x) * offset = -grad(x) // линейная система. можно решить специализированным решателем либо просто найти обратную матрицу гессиана и домножить на минус градиент + // offset = -hessian(x)^-1 * grad(x) + cv::Vec3f offset; + if (!cv::solve(H, -g, offset, cv::DECOMP_LU)) + break; + + if (std::abs(offset[0]) < 0.5f && std::abs(offset[1]) < 0.5f && std::abs(offset[2]) < 0.5f) { + + // функцию респонза оптимизировали как параболу: D(x + offset) = D(x) + grad(x) * offset + 1/2 * offset_transposed * hessian(x) * offset + // подставляем hessian(x) * offset = -grad(x): D(x + offset) = D(x) + grad(x) * offset - 1/2 * offset_transposed * grad = D(x) + 1/2 * grad(x) * offset + float response_optimized = resp_center + 0.5f * g.dot(offset); + if (std::abs(response_optimized) < thresh) + break; + + // фильтрация по зацепистости + if (params.enable_edge_like_filtering) { + // из линейной алгебры, сумма диагональных элементов матрицы (след) равна сумме собственных чисел + // определитель матрицы равен произведению собственных чисел + // в случае гессиана (пространственной части: (dxx dxy, dxy, dyy)), + // собственные числа lambda1, lambda2 - силы кривизны в направлении максимальной кривизны и в ортогональном + cv::Matx22f H_spatial(dxx, dxy, dxy, dyy); + // find eigen values for H_spatial: + cv::Vec2f eigen_values; + cv::eigen(H_spatial, eigen_values); + // std::cout << "eigen values: " << eigen_values[0] << ", " << eigen_values[1] << std::endl; + auto lambda1 = eigen_values[0]; + auto lambda2 = eigen_values[1]; + + float trace = dxx + dyy; + float det = dxx * dyy - dxy * dxy; + if (det <= 0) + break; // если произведение кривизн отрицательное, то мы находимся в седловой точке, а не в максимуме/минимуме. если нулевое, то это ровная граница вообще + + // если граница незацепистая = грань, то одна кривизна сильно больше чем другая. хотим, чтобы обе кривизны были примерно сопоставимы + // тогда их отношение r = lambda1/lambda2 будет не очень большим + // если расписать trace * trace / det через r, то получится (r + 1) ^ 2 / r + // функция растущая по r, так что если наше фактическое значение trace * trace / det выше (r + 1) ^ 2 / r, то и наше отношение кривизн больше порога, значит плохая зацепистость + // и просто как интуиция, при больших r это выражение просто до r сокращается + + // в итоге получается что порог edge_threshold в отличие от response_threshold наоборот, чем больше тем расслабленнее + float r = trace * trace / det; + if (r > (edge_threshold + 1) * (edge_threshold + 1) / edge_threshold) + break; + } + + // скейлим координаты точек обратно до родных размеров картинки + // !!! если выбираем при даунскейле другую политику, с усреднением вместо ресемплинга, то надо здесь применять формулу со сдвигами на полпикселя + float scale = (real_octave >= 0) ? (float)(1 << real_octave) : (1.f / (float)(1 << (-real_octave))); + float real_x = (xi + offset[0]) * scale; + float real_y = (yi + offset[1]) * scale; + float real_layer = li + offset[2]; + + if (!params.enable_subpixel_localization) { + real_x = x * scale; + real_y = y * scale; + real_layer = layer; + } + + float kp_sigma = (float)(sigma0 * std::pow(2.0, (double)real_layer / s) * scale); + + cv::KeyPoint kp; + kp.pt.x = real_x; + kp.pt.y = real_y; + kp.size = kp_sigma * 2.f; // диаметр + kp.octave = real_octave; + kp.class_id = li; // в настоящей opencv имплементации и слой и октава запихиваются в поле octave битовыми операциями + kp.response = std::abs(response_optimized); + keypoints.push_back(kp); + break; + } + + // это на случай если не зашли в предыдущий if: если оптимизированная точка вылетела за границы нашего пикселя, то делаем еще шаг + // идея в том, что если максимум реально там (а у нас же неидеальная парабола), то оптимизировав в той клеточке еще раз, если мы получим маленький сдвиг, то подтвердим минимум и успокоимся + // а если снова вылетим из пикселя, то либо поищем минимум еще, либо устанем и забьем + xi += cvRound(offset[0]); + yi += cvRound(offset[1]); + li += cvRound(offset[2]); + + if (li < 1 || li > s || xi < border || xi >= cols - border || yi < border || yi >= rows - border) + break; + } + } + } + } + + if (verbose_level) + std::cout << "octave " << o << ": " << keypoints.size() << " keypoints so far" << std::endl; + } + + if (verbose_level) + std::cout << "total keypoints: " << keypoints.size() << std::endl; + + return keypoints; +} + +std::vector phg::computeOrientations(const std::vector& kpts, const std::vector& octaves, const phg::SIFTParams& params, int verbose_level) +{ + // we wanna assign one or more dominant orientations to each keypoint detected in the scale-space + const int s = params.n_octave_layers; + const double sigma0 = params.sigma; + const int n_bins = params.orient_nbins; + const double peak_ratio = params.orient_peak_ratio; + + std::vector histogram(n_bins); + + std::vector oriented_kpts; + + const int first_octave = params.upscale_first ? -1 : 0; + + for (const cv::KeyPoint& kp : kpts) { + int layer = kp.class_id; + int real_octave = kp.octave; + int o = real_octave - first_octave; // индекс в массиве octaves + + const cv::Mat& img = octaves[o].layers[layer]; + + float scale = (real_octave >= 0) ? (float)(1 << real_octave) : (1.f / (float)(1 << (-real_octave))); + float x = kp.pt.x / scale; + float y = kp.pt.y / scale; + + // найдем радиус ключевой точки в координатах ее октавы + float kp_sigma_octave = (float)(sigma0 * std::pow(2.0, (double)layer / s)); + float sigma_win = 1.5f * kp_sigma_octave; + // цитата из lowe: "Each sample added to the histogram is weighted by its gradient magnitude + // and by a Gaussian-weighted circular window with a σ that is 1.5 times that of the scale of the keypoint." + + int radius = (int)std::round(3.f * sigma_win); + // radius of a point refers to the size of the region + // around the keypoint that is considered for further processing + + int xi = (int)std::round(x); + int yi = (int)std::round(y); + + if (xi - radius <= 0 || xi + radius >= img.cols - 1 || yi - radius <= 0 || yi + radius >= img.rows - 1) + continue; + + histogram.assign(n_bins, 0.0); + + for (int dy = -radius; dy <= radius; dy++) { + for (int dx = -radius; dx <= radius; dx++) { + int px = xi + dx; + int py = yi + dy; // we take a point within +-rad + // Illustration of gradient computation: + // The pixel (px, py) is at the center, and we compute the gradient + // in the x-direction (gx) and y-direction (gy) using neighboring pixels. + // + // gy (vertical gradient) + // ↑ + // | (py-1, px-1) (py-1, px) (py-1, px+1) + // | +------------+------------+ + // | | | | + // | | | | + // | +------------+------------+ + // | (py, px-1) (py, px) (py, px+1) + // | +------------+------------+ + // | | | | + // | | | | + // | +------------+------------+ + // | (py+1, px-1) (py+1, px) (py+1, px+1) + // | | | | + // | | | | + // | +------------+------------+ + // | + // +--------------------> gx (horizontal gradient) + // + // gx = img.at(py, px+1) - img.at(py, px-1) + // gy = img.at(py+1, px) - img.at(py-1, px) + // gx measures the horizontal change in intensity (difference between right and left neighbors). + // gy measures the vertical change in intensity (difference between top and bottom neighbors). + // Together, gx and gy form the gradient vector at (px, py), which points in the direction of the steepest intensity change. + + // градиент + 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; + if (!params.enable_orientation_gaussian_weighting) { + weight = 1.f; + } else { + weight = std::exp(-(dx*dx + dy*dy) / (2.f * sigma_win * sigma_win)); + } + + // голосуем в гистограмме направлений. находим два ближайших бина и гладко распределяем голос между ними + // в таком случае, голос попавший близко к границе между бинами, проголосует поровну за оба бина + // float bins_closest = angle_deg / (360./n_bins); + float bin = angle_deg / (360./n_bins); + 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] += weight * mag * (1. - frac); + histogram[bin1] += weight * mag * (frac); + } + } + + + + // немного сгладим гистограмму: сделаем несколько проходов box-blur (повторный box blur приближает гауссово размытие) + for (int iter = 0; iter < 6; iter++) { + float first = histogram[0]; + float prev = histogram[n_bins - 1]; + for (int i = 0; i < n_bins - 1; i++) { + float tmp = histogram[i]; + histogram[i] = (prev + histogram[i] + histogram[i + 1]) / 3.f; + prev = tmp; + } + histogram[n_bins - 1] = (prev + histogram[n_bins - 1] + first) / 3.f; + } + + // находим порог: все максимумы сильнее чем peak_ratio * max_val будут приняты и сгенерирована точка + // таким образом, на одну задетектированную точку может быть порождено несколько ориентированных точек, + // если сложно определить однозначно, куда она была направлена + + // allowing a keypoint to have multiple orientations обеспечивает устойчивость к повторяющимся структурам, + // например, рядам параллельных границ, где может быть несколько равно вероятных направлений градиента + float max_val = *std::max_element(histogram.begin(), histogram.end()); + + for (int i = 0; i < n_bins; i++) { + int prev = (i + n_bins - 1) % n_bins; + int next = (i + 1) % n_bins; + + // если локальный максимум и респонз больше порога + if (histogram[i] > histogram[prev] && histogram[i] > histogram[next] && histogram[i] >= peak_ratio * max_val) { + float left = histogram[prev]; + float center = histogram[i]; + float right = histogram[next]; + + // хотим найти угол дескриптора точнее = зафитить параболу по трем точкам (i-1, left), (i, center), (i+1, right) + // у параболы f(x) = ax^2 + bx + c, экстремум в точке x = offset = -b/(2a) + // f(-1) = left, f(0) = center, f(1) = right + + // f(0) = c = center + // f(1) = a + b + c = right + // f(-1) = a - b + c = left + + // f(1) + f(-1) = 2a + 2c -> a = (left + right - 2 * center) / 2 + // f(1) - f(-1) = 2b -> b = (right - left) / 2 + float a = (left + right - 2.f * center) / 2.f; + float b = (right - left) / 2.f; + float offset = -b / (2.f * a); + 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); + } + } + } + + if (verbose_level) + std::cout << "orientations: " << kpts.size() << " -> " << oriented_kpts.size() << " keypoints" << std::endl; + + return oriented_kpts; +} + +// дескриптор подсчитывается по более широкой окрестности. если она выходит за границы изображения, точка может быть отброшена, в результате чего массив kpts может измениться +std::pair> phg::computeDescriptors(const std::vector& kpts, const std::vector& octaves, const phg::SIFTParams& params, int verbose_level) +{ + const int s = params.n_octave_layers; + const double sigma0 = params.sigma; + + // будем считать дескриптор внутри патча вокруг ключевой точки + // структура патча: 4x4 сетка, в каждой клетке гистограмма градиентов на 8 бинов + const int n_spatial_bins = 4; + const int n_orient_bins = 8; + const int n_dims = n_spatial_bins * n_spatial_bins * n_orient_bins; // 128 + + // размер одной клетки патча в сигмах. всего размер контекста для одного дексриптора = n_spatial_bins * spatial_bin_width_sigmas сигм + const float spatial_bin_width_sigmas = 3.f; // в сигмах + + const float mag_cap = 0.2f; + + std::vector valid_kpts; + cv::Mat descriptors; + + const int first_octave = params.upscale_first ? -1 : 0; + + for (const cv::KeyPoint& kp : kpts) { + int layer = kp.class_id; + int real_octave = kp.octave; + int o = real_octave - first_octave; // индекс в массиве octaves + + const cv::Mat& img = octaves[o].layers[layer]; + + float scale = (real_octave >= 0) ? (float)(1 << real_octave) : (1.f / (float)(1 << (-real_octave))); + float x = kp.pt.x / scale; + float y = kp.pt.y / scale; + + float kp_sigma_octave = (float)(sigma0 * std::pow(2.0, (double)layer / s)); + + // размер патча в котором считаем дескриптор в пикселях октавы + float spatial_bin_width = spatial_bin_width_sigmas * kp_sigma_octave; + + // изначально ширина дескриптора = spatial_bin_width * n_spatial_bins, но берем с запасом: + // * sqrt(2) для того, чтобы можно было посемплировать патч даже повернутый на 45 градусов ромбиком + // * +1 в скобках чтобы можно было семплировать градиенты (а еще зачем?) + float half_width = 0.5f * spatial_bin_width * (n_spatial_bins + 1) * std::sqrt(2.f); + int radius = (int)std::round(half_width); + + int xi = (int)std::round(x); + int yi = (int)std::round(y); + + if (xi - radius <= 0 || xi + radius >= img.cols - 1 || yi - radius <= 0 || yi + radius >= img.rows - 1) + continue; + + float kp_angle_rad = kp.angle * (float)CV_PI / 180.f; + float cos_a = std::cos(kp_angle_rad); + float sin_a = std::sin(kp_angle_rad); + + // для гауссового взвешивания: затухающий вклад градиентов с краев картинки + float sigma_desc = (float)n_spatial_bins * 0.5f; + + std::vector desc(n_dims, 0.f); + + // семплируем градиенты и кладем в гистограммы + for (int dy = -radius; dy <= radius; dy++) { + for (int dx = -radius; dx <= radius; dx++) { + int px = xi + dx; + int py = yi + dy; + + float rot_x = (cos_a * dx + sin_a * dy) / spatial_bin_width; + float rot_y = (-sin_a * dx + cos_a * dy) / spatial_bin_width; + + // подсчет пространственного бина + // бин 0 бин 1 | бин 2 бин 3 + // [-----------][-----------] [-----------][-----------] + // ^ + // центр ключевой точки (rot_x = rot_y = 0) + // центр нулевого бина в координатах rot находится в точке (-1.5, -1.5), а после сдвига перемещается в точку (0, 0), что удобно для индексирования + float bin_x = rot_x + n_spatial_bins * 0.5f - 0.5f; + float bin_y = rot_y + n_spatial_bins * 0.5f - 0.5f; + + if (bin_x < -1.f || bin_x >= (float)n_spatial_bins || bin_y < -1.f || bin_y >= (float)n_spatial_bins) + continue; + + // градиент (потом все равно будем все нормализовывать, так что можно не нормировать здесь) + 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); + /* I had a question about why we need gradient both for computeOrientation and computeDescriptors: + + Why Can't We Reuse the Gradients and Angles? + Different Neighborhoods: + In computeOrientations, the gradients are computed over a circular region around the keypoint to determine the dominant orientation. + In computeDescriptors, the gradients are computed over a larger rectangular region divided into spatial bins to build the descriptor. + + Different Uses: + In computeOrientations, the gradients are used to build an orientation histogram. + In computeDescriptors, the gradients are used to populate the spatial and orientation bins of the descriptor. + + Rotation Adjustment: + In computeDescriptors, the gradient angles are adjusted relative to the keypoint's dominant orientation. This adjustment is not needed in computeOrientations. + Performance Considerations: + + */ + + // инвариантность к повороту: повернем направление градиента на угол ключевой точки + float angle_invariant = angle - kp_angle_rad; + while (angle_invariant < 0.f) + angle_invariant += (float)CV_2PI; + while (angle_invariant >= (float)CV_2PI) + angle_invariant -= (float)CV_2PI; + + // подсчет бина в гистограммке градиентов внутри пространственного бина + float bin_o = angle_invariant * n_orient_bins / CV_2PI; + if (bin_o >= n_orient_bins) + bin_o -= n_orient_bins; + + // семплы вблизи края патча взвешиваем с меньшим весом + + 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 по пространственным бинам и по бинам гистограммок трилинейной интерполяцией + + int ix0 = (int)std::floor(bin_x); + int iy0 = (int)std::floor(bin_y); + int io0 = (int)std::floor(bin_o); + + float fx = bin_x - ix0; + float fy = bin_y - iy0; + float fo = bin_o - io0; + + for (int diy = 0; diy <= 1; diy++) { + int iy = iy0 + diy; + if (iy < 0 || iy >= n_spatial_bins) + continue; + float wy = (diy == 0) ? (1.f - fy) : fy; + + for (int dix = 0; dix <= 1; dix++) { + int ix = ix0 + dix; + if (ix < 0 || ix >= n_spatial_bins) + continue; + float wx = (dix == 0) ? (1.f - fx) : fx; + + for (int dio = 0; dio <= 1; dio++) { + int io = (io0 + dio) % n_orient_bins; + if (io < 0) + io += n_orient_bins; + float wo = (dio == 0) ? (1.f - fo) : fo; + + int idx = (iy * n_spatial_bins + ix) * n_orient_bins + io; + desc[idx] += wo * wx * wy * weighted_mag; + } + } + } + } else { + int ix_nearest = (int)std::round(bin_x); + int iy_nearest = (int)std::round(bin_y); + 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) { + int idx = (iy_nearest * n_spatial_bins + ix_nearest) * n_orient_bins + io_nearest; + desc[idx] += weighted_mag; + } + } + } + } + + // нормализуем дескриптор до единичной l2 длины + float norm = 0.f; + for (float v : desc) + norm += v * v; + norm = std::sqrt(norm) + 1e-7f; + for (float& v : desc) + v /= norm; + + // грохнем слишком большие градиенты и ренормализуем + // таким образом один выброс не потянет за собой весь дескриптор и в будущем расстояние с похожим соседом не вырастет сильно + for (float& v : desc) + v = std::min(v, mag_cap); + + norm = 0.f; + for (float v : desc) + norm += v * v; + norm = std::sqrt(norm) + 1e-7f; + for (float& v : desc) + v /= norm; + + if (descriptors.empty()) { + descriptors.create(0, n_dims, CV_32F); + } + + cv::Mat row(1, n_dims, CV_32F, desc.data()); + descriptors.push_back(row.clone()); + valid_kpts.push_back(kp); + } + + if (verbose_level) + std::cout << "descriptors: " << kpts.size() << " -> " << valid_kpts.size() << " keypoints (some discarded due to border)" << std::endl; + + return { descriptors, valid_kpts }; +} + +std::vector phg::selectTopKeypoints(const std::vector& kpts, const phg::SIFTParams& params, int verbose_level) +{ + if (params.nfeatures <= 0 || (int)kpts.size() <= params.nfeatures) { + return kpts; + } + + int nfeatures = params.nfeatures; + + std::vector idx(kpts.size()); + std::iota(idx.begin(), idx.end(), 0); + std::partial_sort(idx.begin(), idx.begin() + nfeatures, idx.end(), [&kpts](int a, int b) { return std::abs(kpts[a].response) > std::abs(kpts[b].response); }); + idx.resize(nfeatures); + std::sort(idx.begin(), idx.end()); + + std::vector sel_kpts(nfeatures); + for (int i = 0; i < nfeatures; ++i) { + sel_kpts[i] = kpts[idx[i]]; + } + + if (verbose_level) + std::cout << "retained top " << nfeatures << " keypoints by response" << std::endl; + + return sel_kpts; +} + +void phg::SIFT::detectAndCompute(const cv::Mat& img, const cv::Mat& mask, std::vector& kpts, cv::Mat& desc) const +{ + rassert(mask.empty(), 911738571854310); // not implemented, parameter added to match interface with opencv sift implementation + + saveImg("00_input.jpg", img); + + cv::Mat gray = toGray32F(img); + saveImg("01_gray.png", gray); + + if (p.upscale_first) { + auto prev_size = gray.size(); + gray = upsample2x(gray); + if (verbose_level) + std::cout << "upscaled image from " << prev_size.width << "x" << prev_size.height << " to " << gray.cols << "x" << gray.rows << std::endl; + saveImg("01b_gray_upscaled.png", gray); + } + + std::vector octaves = buildOctaves(gray, p, verbose_level); + savePyramid("pyramid/02_octave", octaves); + + std::vector dog = buildDoG(octaves, p, verbose_level); + savePyramid("pyramidDoG/03_dog_octave", dog, true); + + kpts = findScaleSpaceExtrema(dog, p, verbose_level); + // ориентация ключевых точек это довольно дорогая операция + // в случае если пользователь просит малое количество лучших точек (например, 1000, а без порога нашлось 20000), + // то по производительности очень оправдано сразу их здесь и выбрать, чтобы не тащить до самого конца где все равно отбросим + kpts = selectTopKeypoints(kpts, p, verbose_level); + + kpts = computeOrientations(kpts, octaves, p, verbose_level); + // после подсчета ориентаций количество могло возрасти (и скорее всего возросло) + // нужно снова выбрать лучшие точки чтобы уложиться в бюджет + kpts = selectTopKeypoints(kpts, p, verbose_level); + + if (verbose_level >= 2) { + cv::Mat kpts_img; + cv::drawKeypoints(img, kpts, kpts_img, cv::Scalar::all(-1), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS); + saveImg("04_keypoints.jpg", kpts_img); + } + + std::tie(desc, kpts) = computeDescriptors(kpts, octaves, p, verbose_level); + + // TODO всегда ли мы получаем ровно столько точек сколько запросили в параметре nfeatures? + // в каких случаях это не так и в какую сторону? + // как подкрутить алгоритм, чтобы всегда выдавать ровно запрошенное количество точек (когда это в принципе возможно) но не сильно просесть в производительности? +} + +void phg::SIFT::saveImg(const std::string& name, const cv::Mat& img) const +{ + if (verbose_level < 2 || debug_folder.empty()) { + return; + } + + cv::Mat out; + if (img.depth() == CV_32F) { + img.convertTo(out, CV_8U, 255.0); + } else { + out = img; + } + cv::imwrite(debug_folder + name, out); +} + +void phg::SIFT::savePyramid(const std::string& name, const std::vector& pyramid, bool normalize) const +{ + if (verbose_level < 2 || debug_folder.empty()) { + return; + } + + cv::Size size = pyramid.front().layers.front().size(); + + for (size_t o = 0; o < pyramid.size(); ++o) { + std::cout << "saving octave " << o << std::endl; + + const Octave& octave = pyramid[o]; + + for (size_t i = 0; i < octave.layers.size(); ++i) { + cv::Mat layer = octave.layers[i].clone(); + + cv::resize(layer, layer, size, 0, 0, cv::INTER_LINEAR); + + if (normalize) { + double mn, mx; + cv::minMaxLoc(layer, &mn, &mx); + if (mx - mn > 1e-8) { + layer = (layer - mn) / (mx - mn); + } else { + layer.setTo(0.5); + } + } + + std::stringstream ss; + ss << name << "_" << o << "_layer_" << i << ".png"; + saveImg(ss.str(), layer); + } + } +} diff --git a/src/phg/sift/sift.h b/src/phg/sift/sift.h index 6e65f6d..66f3095 100755 --- a/src/phg/sift/sift.h +++ b/src/phg/sift/sift.h @@ -1,59 +1,59 @@ -#pragma once - -#include - -namespace phg { - -struct SIFTParams { - int nfeatures = 0; - int n_octave_layers = 3; - double contrast_threshold = 0.04; - double edge_threshold = 10; - double sigma = 1.6; - - double orient_peak_ratio = 0.8; - int orient_nbins = 36; - bool upscale_first = true; - - bool enable_subpixel_localization = true; - bool enable_edge_like_filtering = true; - bool enable_orientation_bin_interpolation = true; - bool enable_orientation_gaussian_weighting = true; - bool enable_orientation_subpixel_localization = true; - bool enable_descriptor_gaussian_weighting = true; - bool enable_descriptor_bin_interpolation = true; -}; - -class SIFT { -public: - struct Octave { - std::vector layers; - }; - - explicit SIFT(const SIFTParams& p = SIFTParams(), int verbose_level = 0, const std::string& debug_folder = "") - : p(p) - , verbose_level(verbose_level) - , debug_folder(debug_folder) - { - } - - void detectAndCompute(const cv::Mat& img, const cv::Mat& mask, std::vector& kpts, cv::Mat& desc) const; - void detectAndCompute(const cv::Mat& img, std::vector& kpts, cv::Mat& desc) const { detectAndCompute(img, { }, kpts, desc); } - -private: - void saveImg(const std::string& name, const cv::Mat& img) const; - void savePyramid(const std::string& name, const std::vector& pyramid, bool normalize = false) const; - - int verbose_level; - std::string debug_folder; - SIFTParams p; -}; - -cv::Mat toGray32F(const cv::Mat& img); -std::vector buildOctaves(const cv::Mat& img, const SIFTParams& p, int verbose_level = 0); -std::vector buildDoG(const std::vector& octaves, const SIFTParams& p, int verbose_level = 0); -std::vector findScaleSpaceExtrema(const std::vector& dog, const SIFTParams& params, int verbose_level = 0); -std::vector computeOrientations(const std::vector& kpts, const std::vector& octaves, const SIFTParams& params, int verbose_level = 0); -std::pair> computeDescriptors(const std::vector& kpts, const std::vector& octaves, const SIFTParams& params, int verbose_level = 0); -std::vector selectTopKeypoints(const std::vector& kpts, const SIFTParams& params, int verbose_level = 0); -} +#pragma once + +#include + +namespace phg { +// rotation 30 :: [ORB_OCV] average angle difference between matched points: 25.313 degrees +struct SIFTParams { + int nfeatures = 0; + int n_octave_layers = 3; + double contrast_threshold = 0.04; + double edge_threshold = 10; + double sigma = 1.6; + + double orient_peak_ratio = 0.8; + int orient_nbins = 36; + bool upscale_first = true; + + bool enable_subpixel_localization = true; + bool enable_edge_like_filtering = true; + bool enable_orientation_bin_interpolation = true; + bool enable_orientation_gaussian_weighting = true; + bool enable_orientation_subpixel_localization = true; + bool enable_descriptor_gaussian_weighting = true; + bool enable_descriptor_bin_interpolation = true; +}; + +class SIFT { +public: + struct Octave { + std::vector layers; + }; + + explicit SIFT(const SIFTParams& p = SIFTParams(), int verbose_level = 0, const std::string& debug_folder = "/mnt/c/Users/irady/GitHub/ITMO/2026-фотграмметрия/homeworks_wsl/task01/PhotogrammetryTasks2026/data/debug/test_sift/debug/") + : p(p) + , verbose_level(verbose_level) + , debug_folder(debug_folder) + { + } + + void detectAndCompute(const cv::Mat& img, const cv::Mat& mask, std::vector& kpts, cv::Mat& desc) const; + void detectAndCompute(const cv::Mat& img, std::vector& kpts, cv::Mat& desc) const { detectAndCompute(img, { }, kpts, desc); } + +private: + void saveImg(const std::string& name, const cv::Mat& img) const; + void savePyramid(const std::string& name, const std::vector& pyramid, bool normalize = false) const; + + int verbose_level; + std::string debug_folder; + SIFTParams p; +}; + +cv::Mat toGray32F(const cv::Mat& img); +std::vector buildOctaves(const cv::Mat& img, const SIFTParams& p, int verbose_level = 0); +std::vector buildDoG(const std::vector& octaves, const SIFTParams& p, int verbose_level = 0); +std::vector findScaleSpaceExtrema(const std::vector& dog, const SIFTParams& params, int verbose_level = 0); +std::vector computeOrientations(const std::vector& kpts, const std::vector& octaves, const SIFTParams& params, int verbose_level = 0); +std::pair> computeDescriptors(const std::vector& kpts, const std::vector& octaves, const SIFTParams& params, int verbose_level = 0); +std::vector selectTopKeypoints(const std::vector& kpts, const SIFTParams& params, int verbose_level = 0); +} diff --git a/tests/test_matching.cpp b/tests/test_matching.cpp index 4de5b71..0a29b18 100644 --- a/tests/test_matching.cpp +++ b/tests/test_matching.cpp @@ -1,842 +1,842 @@ -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - - -#include "utils/test_utils.h" - - -// 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 - -// TODO disable for local testing but do not commit -#define SERVER_TESTING 1 - -#if SERVER_TESTING && !defined(__linux__) -#undef ENABLE_GPU_BRUTEFORCE_MATCHER -#define ENABLE_GPU_BRUTEFORCE_MATCHER 0 -#endif - -#if ENABLE_MY_MATCHING -const double max_keypoints_rmse_px = 1.0; -#else -const double max_keypoints_rmse_px = 10.0; -#endif - -const double max_color_rmse_8u = 20; - -#define GAUSSIAN_NOISE_STDDEV 1.0 - - -namespace { - - void drawMatches(const cv::Mat &img1, - const cv::Mat &img2, - const std::vector &keypoints1, - const std::vector &keypoints2, - const std::vector &matches, - const std::string &path) - { - cv::Mat img_matches; - drawMatches( img1, keypoints1, img2, keypoints2, matches, img_matches, cv::Scalar::all(-1), - cv::Scalar::all(-1), std::vector(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS ); - - cv::imwrite(path, img_matches); - } - - cv::Mat getHomography(const cv::Mat &img1, const cv::Mat &img2) - { - using namespace cv; - - cv::Ptr detector = cv::SIFT::create(); - std::vector keypoints1, keypoints2; - Mat descriptors1, descriptors2; - detector->detectAndCompute( img1, noArray(), keypoints1, descriptors1 ); - detector->detectAndCompute( img2, noArray(), keypoints2, descriptors2 ); - - std::vector< std::vector > knn_matches; - -#if ENABLE_MY_MATCHING - phg::FlannMatcher matcher; - matcher.train(descriptors2); - matcher.knnMatch(descriptors1, knn_matches, 2); -#else - Ptr matcher = DescriptorMatcher::create(DescriptorMatcher::FLANNBASED); - matcher->knnMatch( descriptors1, descriptors2, knn_matches, 2 ); -#endif - - std::vector good_matches(knn_matches.size()); - for (int i = 0; i < (int) knn_matches.size(); ++i) { - good_matches[i] = knn_matches[i][0]; - } - -#if ENABLE_MY_MATCHING - phg::DescriptorMatcher::filterMatchesRatioTest(knn_matches, good_matches); - { - std::vector tmp; - phg::DescriptorMatcher::filterMatchesClusters(good_matches, keypoints1, keypoints2, tmp); - std::swap(tmp, good_matches); - } -#else - { - std::vector tmp; - phg::filterMatchesGMS(good_matches, keypoints1, keypoints2, img1.size(), img2.size(), tmp); - std::swap(tmp, good_matches); - } -#endif - - std::vector points1, points2; - for (const cv::DMatch &match : good_matches) { - points1.push_back(keypoints1[match.queryIdx].pt); - points2.push_back(keypoints2[match.trainIdx].pt); - } - -#if ENABLE_MY_MATCHING - cv::Mat H = phg::findHomography(points1, points2); -#else - cv::Mat H = phg::findHomographyCV(points1, points2); -#endif - - return H; - } - - void evaluateStitching(const cv::Mat &img1, const cv::Mat &img2, double &keypoints_rmse, double &color_rmse, - const std::vector &keypoints1, const std::vector &keypoints2, - const cv::Mat &descriptors1, const cv::Mat &descriptors2) - { - using namespace cv; - - std::vector> knn_matches; - -#if ENABLE_MY_MATCHING - phg::FlannMatcher matcher; - matcher.train(descriptors2); - matcher.knnMatch(descriptors1, knn_matches, 2); -#else - Ptr matcher = DescriptorMatcher::create(DescriptorMatcher::FLANNBASED); - matcher->knnMatch( descriptors1, descriptors2, knn_matches, 2 ); -#endif - - std::vector good_matches(knn_matches.size()); - for (int i = 0; i < (int) knn_matches.size(); ++i) { - good_matches[i] = knn_matches[i][0]; - } - -#if ENABLE_MY_MATCHING - phg::DescriptorMatcher::filterMatchesRatioTest(knn_matches, good_matches); - { - std::vector tmp; - phg::DescriptorMatcher::filterMatchesClusters(good_matches, keypoints1, keypoints2, tmp); - std::swap(tmp, good_matches); - } -#else - { - std::vector tmp; - phg::filterMatchesGMS(good_matches, keypoints1, keypoints2, img1.size(), img2.size(), tmp); - std::swap(tmp, good_matches); - } -#endif - - std::vector points1, points2; - for (const cv::DMatch &match : good_matches) { - points1.push_back(keypoints1[match.queryIdx].pt); - points2.push_back(keypoints2[match.trainIdx].pt); - } -#if ENABLE_MY_MATCHING - cv::Mat H = phg::findHomography(points1, points2); -#else - cv::Mat H = phg::findHomographyCV(points1, points2); -#endif - - if (good_matches.size() < 4) { - throw std::runtime_error("too few matches"); - } - - keypoints_rmse = 0; - for (int i = 0; i < (int) good_matches.size(); ++i) { -#if ENABLE_MY_MATCHING - cv::Point2f pt = phg::transformPoint(points1[i], H); -#else - cv::Point2f pt = phg::transformPointCV(points1[i], H); -#endif - cv::Point2f diff = pt - points2[i]; - keypoints_rmse += diff.x * diff.x + diff.y * diff.y; - } - keypoints_rmse /= good_matches.size(); - keypoints_rmse = std::sqrt(keypoints_rmse); - - color_rmse = 0; - int64_t count = 0; - for (int y = 0; y < img1.rows; ++y) { - for (int x = 0; x < img1.cols; ++x) { - cv::Vec3b col1 = img1.at(y, x); -#if ENABLE_MY_MATCHING - cv::Point2f pt = phg::transformPoint(cv::Point2f(x, y), H); -#else - cv::Point2f pt = phg::transformPointCV(cv::Point2f(x, y), H); -#endif - int pt_x = std::round(pt.x); - int pt_y = std::round(pt.y); - if (pt_x >= 0 && pt_x < img2.cols && pt_y >= 0 && pt_y < img2.rows) { - cv::Vec3b col2 = img2.at(pt_y, pt_x); - int dc0 = int(col2[0]) - int(col1[0]); - int dc1 = int(col2[1]) - int(col1[1]); - int dc2 = int(col2[2]) - int(col1[2]); - - color_rmse += dc0 * dc0 + dc1 * dc1 + dc2 * dc2; - ++count; - } - } - } - - if (count) { - color_rmse /= count; - color_rmse = std::sqrt(color_rmse); - } - } - -} - -namespace { - void testStitching(const cv::Mat &img1, const cv::Mat &img2, const std::vector &keypoints1, const std::vector &keypoints2, - const cv::Mat &descriptors1, const cv::Mat &descriptors2) - { - double rmse_kpts, rmse_color; - evaluateStitching(img1, img2, rmse_kpts, rmse_color, keypoints1, keypoints2, descriptors1, descriptors2); - - std::cout << "keypoints RMSE: " << rmse_kpts << ", color RMSE: " << rmse_color << std::endl; - - EXPECT_LT(rmse_kpts, max_keypoints_rmse_px); - EXPECT_LT(rmse_color, max_color_rmse_8u); - } - - void testStitchingMultipleDetectors(const cv::Mat &img1, const cv::Mat &img2) - { - { - std::cout << "testing sift detector/descriptor..." << std::endl; - cv::Ptr detector = cv::SIFT::create(); - std::vector keypoints1, keypoints2; - cv::Mat descriptors1, descriptors2; - detector->detectAndCompute( img1, cv::noArray(), keypoints1, descriptors1 ); - detector->detectAndCompute( img2, cv::noArray(), keypoints2, descriptors2 ); - - testStitching(img1, img2, keypoints1, keypoints2, descriptors1, descriptors2); - } - -#if ENABLE_MY_DESCRIPTOR - { - std::cout << "testing my detector/descriptor..." << std::endl; - std::vector keypoints1, keypoints2; - cv::Mat descriptors1, descriptors2; - phg::SIFT mySIFT; - mySIFT.detectAndCompute(img1, keypoints1, descriptors1); - mySIFT.detectAndCompute(img2, keypoints2, descriptors2); - - testStitching(img1, img2, keypoints1, keypoints2, descriptors1, descriptors2); - } -#endif - } -} - -TEST (MATCHING, SimpleStitching) { - - cv::Mat img1 = cv::imread("data/src/test_matching/hiking_left.JPG"); - cv::Mat img2 = cv::imread("data/src/test_matching/hiking_right.JPG"); - - testStitchingMultipleDetectors(img1, img2); -} - -namespace { - - bool matcheq(const cv::DMatch &lhs, const cv::DMatch &rhs) - { - return std::tie(lhs.trainIdx, lhs.queryIdx, lhs.imgIdx) == std::tie(rhs.trainIdx, rhs.queryIdx, rhs.imgIdx); - } - - void evaluateMatching(const cv::Mat &img1, const cv::Mat &img2, const std::vector &keypoints1, const std::vector &keypoints2, - const cv::Mat &descriptors1, const cv::Mat &descriptors2, - double &nn_score, double &nn2_score, double &nn_score_cv, double &nn2_score_cv, - double &time_my, double &time_cv, double &time_bruteforce, double &time_bruteforce_gpu, - double &good_nn, double &good_ratio, double &good_clusters, double &good_ratio_and_clusters, bool do_bruteforce) - { - using namespace cv; - - if (!descriptors1.rows || !descriptors2.rows) { - throw std::runtime_error("empty descriptors"); - } - - timer tm; - - std::vector> knn_matches_flann, knn_matches_flann_cv, knn_matches_bruteforce; - - std::cout << "flann matching..." << std::endl; - tm.restart(); - #if ENABLE_MY_MATCHING - { - phg::FlannMatcher matcher; - matcher.train(descriptors2); - matcher.knnMatch(descriptors1, knn_matches_flann, 2); - } - #endif - time_my = tm.elapsed(); - - std::cout << "cv flann matching..." << std::endl; - tm.restart(); - { - Ptr matcher = DescriptorMatcher::create(DescriptorMatcher::FLANNBASED); - matcher->knnMatch( descriptors1, descriptors2, knn_matches_flann_cv, 2 ); - - #if !ENABLE_MY_MATCHING - knn_matches_flann = knn_matches_flann_cv; - #endif - } - time_cv = tm.elapsed(); - - tm.restart(); - if (do_bruteforce) { - std::cout << "brute force matching" << std::endl; - phg::BruteforceMatcher matcher; - matcher.train(descriptors2); - matcher.knnMatch(descriptors1, knn_matches_bruteforce, 2); - } - time_bruteforce = tm.elapsed(); - - tm.restart(); - std::vector> knn_matches_bruteforce_gpu; -#if ENABLE_GPU_BRUTEFORCE_MATCHER - if (do_bruteforce) { - std::cout << "brute force GPU matching" << std::endl; - phg::BruteforceMatcherGPU matcher; - matcher.train(descriptors2); - matcher.knnMatch(descriptors1, knn_matches_bruteforce_gpu, 2); - } -#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])); - } - } -#endif - - nn_score = 0; - nn2_score = 0; - nn_score_cv = 0; - nn2_score_cv = 0; - if (do_bruteforce) { - for (int i = 0; i < descriptors1.rows; ++i) { - if (knn_matches_bruteforce[i][0].queryIdx != i) { - throw std::runtime_error("invalid DMatch queryIdx for knn_matches_bruteforce"); - } - if (knn_matches_flann[i][0].queryIdx != i) { - throw std::runtime_error("invalid DMatch queryIdx for knn_matches_flann"); - } - if (knn_matches_flann_cv[i][0].queryIdx != i) { - throw std::runtime_error("invalid DMatch queryIdx for knn_matches_flann_cv"); - } - - if (knn_matches_flann[i][0].trainIdx == knn_matches_bruteforce[i][0].trainIdx) { - ++nn_score; - } - - if (knn_matches_flann[i][1].trainIdx == knn_matches_bruteforce[i][1].trainIdx) { - ++nn2_score; - } - - if (knn_matches_flann_cv[i][0].trainIdx == knn_matches_bruteforce[i][0].trainIdx) { - ++nn_score_cv; - } - - if (knn_matches_flann_cv[i][1].trainIdx == knn_matches_bruteforce[i][1].trainIdx) { - ++nn2_score_cv; - } - } - - nn_score /= descriptors1.rows; - nn2_score /= descriptors1.rows; - nn_score_cv /= descriptors1.rows; - nn2_score_cv /= descriptors1.rows; - } - - std::vector good_matches_nn(knn_matches_flann.size()); - for (int i = 0; i < (int) knn_matches_flann.size(); ++i) { - good_matches_nn[i] = knn_matches_flann[i][0]; - } - drawMatches(img1, img2, keypoints1, keypoints2, good_matches_nn, "data/debug/test_matching/" + getTestSuiteName() + "_" + getTestName() + "_" + "00_matches_nn.png"); - - #if ENABLE_MY_MATCHING - std::cout << "filtering matches by ratio test..." << std::endl; - std::vector good_matches_ratio; - phg::DescriptorMatcher::filterMatchesRatioTest(knn_matches_flann, good_matches_ratio); - drawMatches(img1, img2, keypoints1, keypoints2, good_matches_ratio, "data/debug/test_matching/" + getTestSuiteName() + "_" + getTestName() + "_" + "01_matches_ratio.png"); - - std::cout << "filtering matches by clusters..." << std::endl; - std::vector good_matches_clusters_only; - phg::DescriptorMatcher::filterMatchesClusters(good_matches_nn, keypoints1, keypoints2, good_matches_clusters_only); - drawMatches(img1, img2, keypoints1, keypoints2, good_matches_clusters_only, "data/debug/test_matching/" + getTestSuiteName() + "_" + getTestName() + "_" + "03_matches_clusters_only.png"); - - std::cout << "filtering matches by ratio & clusters" << std::endl; - std::vector good_matches_clusters_and_ratio; - phg::DescriptorMatcher::filterMatchesClusters(good_matches_ratio, keypoints1, keypoints2, good_matches_clusters_and_ratio); - drawMatches(img1, img2, keypoints1, keypoints2, good_matches_clusters_and_ratio, "data/debug/test_matching/" + getTestSuiteName() + "_" + getTestName() + "_" + "04_matches_clusters_and_ratio.png"); - #else - std::vector good_matches_clusters_and_ratio; - phg::filterMatchesGMS(good_matches_nn, keypoints1, keypoints2, img1.size(), img2.size(), good_matches_clusters_and_ratio); - drawMatches(img1, img2, keypoints1, keypoints2, good_matches_clusters_and_ratio, "data/debug/test_matching/" + getTestSuiteName() + "_" + getTestName() + "_" + "04_matches_gms.png"); - #endif - - std::cout << "estimating homography..." << std::endl; - cv::Mat H; - { - std::vector points1, points2; - for (const cv::DMatch &match : good_matches_clusters_and_ratio) { - points1.push_back(keypoints1[match.queryIdx].pt); - points2.push_back(keypoints2[match.trainIdx].pt); - } - -#if ENABLE_MY_MATCHING - H = phg::findHomography(points1, points2); -#else - H = phg::findHomographyCV(points1, points2); -#endif - } - - good_nn = 0; - good_ratio = 0; - good_clusters = 0; - good_ratio_and_clusters = 0; - - std::cout << "evaluating homography..." << std::endl; - - #if ENABLE_MY_MATCHING - const int ntest = 4; - std::vector* arrs[ntest] = {&good_matches_nn, &good_matches_ratio, &good_matches_clusters_only, &good_matches_clusters_and_ratio}; - double* ptrs[ntest] = {&good_nn, &good_ratio, &good_clusters, &good_ratio_and_clusters}; - #else - const int ntest = 2; - std::vector* arrs[ntest] = {&good_matches_nn, &good_matches_clusters_and_ratio}; - double* ptrs[ntest] = {&good_nn, &good_ratio_and_clusters}; - #endif - - for (int i_test = 0; i_test < ntest; ++i_test) { - - const std::vector &arr = *arrs[i_test]; - - if (arr.size() < 50) { - std::cerr << "too few matches: " + std::to_string(arr.size()) << std::endl; - continue; - } - - std::vector points1, points2; - for (const cv::DMatch &match : arr) { - points1.push_back(keypoints1[match.queryIdx].pt); - points2.push_back(keypoints2[match.trainIdx].pt); - } - - (*ptrs[i_test]) = 0; - for (int i = 0; i < (int) arr.size(); ++i) { -#if ENABLE_MY_MATCHING - cv::Point2f pt = phg::transformPoint(points1[i], H); -#else - cv::Point2f pt = phg::transformPointCV(points1[i], H); -#endif - - cv::Point2f diff = pt - points2[i]; - float dist2 = diff.x * diff.x + diff.y * diff.y; - if (dist2 < max_keypoints_rmse_px * max_keypoints_rmse_px) { - ++(*ptrs[i_test]); - } - } - if (arr.size()) { - (*ptrs[i_test]) /= arr.size(); - } - } - - } - - void testMatching(const cv::Mat &img1, const cv::Mat &img2, const std::vector &keypoints1, const std::vector &keypoints2, - const cv::Mat &descriptors1, const cv::Mat &descriptors2, - double &nn_score, double &nn2_score, double &nn_score_cv, double &nn2_score_cv, - double &time_my, double &time_cv, double &time_bruteforce, double &time_bruteforce_gpu, - double &good_nn, double &good_ratio, double &good_clusters, double &good_ratio_and_clusters, - bool do_bruteforce - ) - { - evaluateMatching(img1, img2, keypoints1, keypoints2, descriptors1, descriptors2, - nn_score, nn2_score, nn_score_cv, nn2_score_cv, - time_my, time_cv, time_bruteforce, time_bruteforce_gpu, - good_nn, good_ratio, good_clusters, good_ratio_and_clusters, do_bruteforce); - - std::cout << "nn_score: " << nn_score << ", "; - std::cout << "nn2_score: " << nn2_score << ", "; - std::cout << "nn_score_cv: " << nn_score_cv << ", "; - std::cout << "nn2_score_cv: " << nn2_score_cv << ", "; - std::cout << "time_my: " << time_my << ", "; - 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 << ", "; -#endif - std::cout << "good_nn: " << good_nn << ", "; - std::cout << "good_ratio: " << good_ratio << ", "; - std::cout << "good_clusters: " << good_clusters << ", "; - std::cout << "good_ratio_and_clusters: " << good_ratio_and_clusters << std::endl; - } - - void testMatchingMultipleDetectors(const cv::Mat &img1, const cv::Mat &img2, - double &nn_score, double &nn2_score, double &nn_score_cv, double &nn2_score_cv, - double &time_my, double &time_cv, double &time_bruteforce, double &time_bruteforce_gpu, - double &good_nn, double &good_ratio, double &good_clusters, double &good_ratio_and_clusters, bool do_bruteforce = true) - { - { - std::cout << "testing sift detector/descriptor..." << std::endl; - cv::Ptr detector = cv::SIFT::create(); - std::vector keypoints1, keypoints2; - cv::Mat descriptors1, descriptors2; - detector->detectAndCompute( img1, cv::noArray(), keypoints1, descriptors1 ); - detector->detectAndCompute( img2, cv::noArray(), keypoints2, descriptors2 ); - - testMatching(img1, img2, keypoints1, keypoints2, descriptors1, descriptors2, - nn_score, nn2_score, nn_score_cv, nn2_score_cv, - time_my, time_cv, time_bruteforce, time_bruteforce_gpu, - good_nn, good_ratio, good_clusters, good_ratio_and_clusters, do_bruteforce); - } -#if ENABLE_MY_DESCRIPTOR - { - std::cout << "testing my detector/descriptor..." << std::endl; - std::vector keypoints1, keypoints2; - cv::Mat descriptors1, descriptors2; - phg::SIFT mySIFT; - mySIFT.detectAndCompute(img1, keypoints1, descriptors1); - mySIFT.detectAndCompute(img2, keypoints2, descriptors2); - - testMatching(img1, img2, keypoints1, keypoints2, descriptors1, descriptors2, - nn_score, nn2_score, nn_score_cv, nn2_score_cv, - time_my, time_cv, time_bruteforce, time_bruteforce_gpu, - good_nn, good_ratio, good_clusters, good_ratio_and_clusters, do_bruteforce); - } -#endif - } - -} - -TEST (MATCHING, SimpleMatching) { - - cv::Mat img1 = cv::imread("data/src/test_matching/hiking_left.JPG"); - cv::Mat img2 = cv::imread("data/src/test_matching/hiking_right.JPG"); - - - double nn_score, nn2_score, nn_score_cv, nn2_score_cv, - time_my, time_cv, time_bruteforce, time_bruteforce_gpu, good_nn, good_ratio, good_clusters, good_ratio_and_clusters; - - testMatchingMultipleDetectors(img1, img2, - nn_score, nn2_score, nn_score_cv, nn2_score_cv, - time_my, time_cv, time_bruteforce, time_bruteforce_gpu, - good_nn, good_ratio, good_clusters, good_ratio_and_clusters); - - - - EXPECT_GT(nn_score, 0.9 * nn_score_cv); - EXPECT_GT(nn2_score, 0.9 * nn2_score_cv); - - EXPECT_LT(time_my, 1.5 * time_cv); - EXPECT_LT(time_my, 0.1 * time_bruteforce); - -#if ENABLE_GPU_BRUTEFORCE_MATCHER - EXPECT_LT(time_bruteforce_gpu, time_bruteforce); -#endif - -#if ENABLE_MY_MATCHING - EXPECT_LT(good_nn, good_ratio); - EXPECT_LT(good_nn, good_clusters); -#endif - EXPECT_LT(good_nn, good_ratio_and_clusters); - - EXPECT_GT(good_nn, 0.2); -#if ENABLE_MY_MATCHING - EXPECT_GT(good_ratio, 0.9); - EXPECT_GT(good_clusters, 0.9); -#endif - EXPECT_GT(good_ratio_and_clusters, 0.9); -} - -namespace { - - cv::Mat transformImg(const cv::Mat &img2, double angleDegreesClockwise, double scale) - { - cv::Mat M = cv::getRotationMatrix2D(cv::Point(0, 0), 0, scale); - M.at(0, 2) = img2.cols * 0.25 * scale; - M.at(1, 2) = img2.rows * 0.25 * scale; - cv::Mat tmp; - cv::warpAffine(img2, tmp, M, cv::Size(1.5 * img2.cols * scale, 1.5 * img2.rows * scale)); - - cv::Mat transformedImage; - M = cv::getRotationMatrix2D(cv::Point(tmp.cols / 2, tmp.rows / 2), -angleDegreesClockwise, 1.0); - cv::warpAffine(tmp, transformedImage, M, cv::Size(tmp.cols, tmp.rows)); - - return transformedImage; - } - - void addNoise(cv::Mat &img2) - { - cv::Mat noise(cv::Size(img2.cols, img2.rows), CV_8UC3); - cv::setRNGSeed(125125); // фиксируем рандом для детерминизма (чтобы результат воспроизводился из раза в раз) - cv::randn(noise, cv::Scalar::all(0), cv::Scalar::all(GAUSSIAN_NOISE_STDDEV)); - cv::add(img2, noise, img2); // добавляем к преобразованной картинке гауссиан шума - } - - void testMatchingTransformWrapper(double angleDegreesClockwise, double scale) - { - cv::Mat img1 = cv::imread("data/src/test_matching/hiking_left.JPG"); - cv::Mat img2 = cv::imread("data/src/test_matching/hiking_right.JPG"); - - img2 = transformImg(img2, angleDegreesClockwise, scale); - addNoise(img2); - - cv::imwrite("data/debug/test_matching/" + getTestSuiteName() + "_" + getTestName() + "_" + "hiking_right_rotated_noise.png", img2); - - double nn_score, nn2_score, nn_score_cv, nn2_score_cv, - time_my, time_cv, time_bruteforce, time_bruteforce_gpu, good_nn, good_ratio, good_clusters, good_ratio_and_clusters; - - testMatchingMultipleDetectors(img1, img2, - nn_score, nn2_score, nn_score_cv, nn2_score_cv, - time_my, time_cv, time_bruteforce, time_bruteforce_gpu, - good_nn, good_ratio, good_clusters, good_ratio_and_clusters, false); - - EXPECT_LT(time_my, 1.5 * time_cv); - -#if ENABLE_MY_MATCHING - EXPECT_LT(good_nn, good_ratio); -#endif - EXPECT_LT(good_nn, good_ratio_and_clusters); - -#if ENABLE_MY_MATCHING - EXPECT_GT(good_ratio, 0.7); -#endif - EXPECT_GT(good_ratio_and_clusters, 0.7); - } - -} - -TEST (MATCHING, Rotate10) { - double angleDegreesClockwise = 10; - double scale = 1.0; - - testMatchingTransformWrapper(angleDegreesClockwise, scale); -} - -TEST (MATCHING, Rotate20) { - double angleDegreesClockwise = 20; - double scale = 1.0; - - testMatchingTransformWrapper(angleDegreesClockwise, scale); -} - -TEST (MATCHING, Rotate30) { - double angleDegreesClockwise = 30; - double scale = 1.0; - - testMatchingTransformWrapper(angleDegreesClockwise, scale); -} - -TEST (MATCHING, Rotate40) { - double angleDegreesClockwise = 40; - double scale = 1.0; - - testMatchingTransformWrapper(angleDegreesClockwise, scale); -} - -TEST (MATCHING, Rotate45) { - double angleDegreesClockwise = 45; - double scale = 1.0; - - testMatchingTransformWrapper(angleDegreesClockwise, scale); -} - -TEST (MATCHING, Rotate90) { - double angleDegreesClockwise = 90; - double scale = 1.0; - - testMatchingTransformWrapper(angleDegreesClockwise, scale); -} - -TEST (MATCHING, Scale50) { - // seems to be some issue with gms matcher and high downscale -#if ENABLE_MY_MATCHING - double angleDegreesClockwise = 0; - double scale = 0.5; - - testMatchingTransformWrapper(angleDegreesClockwise, scale); -#endif -} - -TEST (MATCHING, Scale70) { - double angleDegreesClockwise = 0; - double scale = 0.7; - - testMatchingTransformWrapper(angleDegreesClockwise, scale); -} - -TEST (MATCHING, Scale90) { - double angleDegreesClockwise = 0; - double scale = 0.9; - - testMatchingTransformWrapper(angleDegreesClockwise, scale); -} - -TEST (MATCHING, Scale110) { - double angleDegreesClockwise = 0; - double scale = 1.1; - - testMatchingTransformWrapper(angleDegreesClockwise, scale); -} - -TEST (MATCHING, Scale130) { - double angleDegreesClockwise = 0; - double scale = 1.3; - - testMatchingTransformWrapper(angleDegreesClockwise, scale); -} - -TEST (MATCHING, Scale150) { - double angleDegreesClockwise = 0; - double scale = 1.5; - - testMatchingTransformWrapper(angleDegreesClockwise, scale); -} - -TEST (MATCHING, Scale175) { - double angleDegreesClockwise = 0; - double scale = 1.75; - - testMatchingTransformWrapper(angleDegreesClockwise, scale); -} - -TEST (MATCHING, Scale200) { - double angleDegreesClockwise = 0; - double scale = 2.0; - - testMatchingTransformWrapper(angleDegreesClockwise, scale); -} - -TEST (MATCHING, Rotate10Scale90) { - double angleDegreesClockwise = 10; - double scale = 0.9; - - testMatchingTransformWrapper(angleDegreesClockwise, scale); -} - -TEST (MATCHING, Rotate30Scale75) { - double angleDegreesClockwise = 30; - double scale = 0.75; - - testMatchingTransformWrapper(angleDegreesClockwise, scale); -} - -TEST (STITCHING, SimplePanorama) { -#if ENABLE_MY_MATCHING - cv::Mat img1 = cv::imread("data/src/test_matching/hiking_left.JPG"); - cv::Mat img2 = cv::imread("data/src/test_matching/hiking_right.JPG"); - - std::function homography_builder = [](const cv::Mat &lhs, const cv::Mat &rhs){ return getHomography(lhs, rhs); }; - cv::Mat pano = phg::stitchPanorama({img1, img2}, {-1, 0}, homography_builder); - cv::imwrite("data/debug/test_matching/" + getTestSuiteName() + "_" + getTestName() + "_" + "panorama.png", pano); -#endif -} - -namespace { - - int getOrthoScore(const cv::Mat &ortho0, const cv::Mat &ortho1, int threshold_px) - { - using namespace cv; - - cv::Ptr detector = cv::SIFT::create(); - std::vector keypoints1, keypoints2; - cv::Mat descriptors1, descriptors2; - detector->detectAndCompute( ortho0, cv::noArray(), keypoints1, descriptors1 ); - detector->detectAndCompute( ortho1, cv::noArray(), keypoints2, descriptors2 ); - - std::vector< std::vector > knn_matches; - - phg::FlannMatcher matcher; - matcher.train(descriptors2); - matcher.knnMatch(descriptors1, knn_matches, 2); - - std::vector good_matches(knn_matches.size()); - for (int i = 0; i < (int) knn_matches.size(); ++i) { - good_matches[i] = knn_matches[i][0]; - } - - phg::DescriptorMatcher::filterMatchesRatioTest(knn_matches, good_matches); - - { - std::vector tmp; - phg::DescriptorMatcher::filterMatchesClusters(good_matches, keypoints1, keypoints2, tmp); - std::swap(tmp, good_matches); - } - - int score = 0; - for (const cv::DMatch &match : good_matches) { - cv::Point2f d = keypoints1[match.queryIdx].pt - keypoints2[match.trainIdx].pt; - if (d.x * d.x + d.y * d.y < threshold_px * threshold_px) { - ++score; - } - } - - drawMatches(ortho0, ortho1, keypoints1, keypoints2, good_matches, "data/debug/test_matching/" + getTestSuiteName() + "_" + getTestName() + "_" + "ortho_matches.png"); - - return score; - } - -} - -TEST (STITCHING, Orthophoto) { -#if ENABLE_MY_MATCHING - cv::Mat img1 = cv::imread("data/src/test_matching/ortho/IMG_160729_071349_0000_RGB.JPG"); - cv::Mat img2 = cv::imread("data/src/test_matching/ortho/IMG_160729_071351_0001_RGB.JPG"); - cv::Mat img3 = cv::imread("data/src/test_matching/ortho/IMG_160729_071353_0002_RGB.JPG"); - cv::Mat img4 = cv::imread("data/src/test_matching/ortho/IMG_160729_071356_0003_RGB.JPG"); - cv::Mat img5 = cv::imread("data/src/test_matching/ortho/IMG_160729_071358_0004_RGB.JPG"); - - { - std::function homography_builder = [](const cv::Mat &lhs, const cv::Mat &rhs){ return getHomography(lhs, rhs); }; - cv::Mat ortho2 = phg::stitchPanorama({img1, img2, img3, img4, img5}, {1, 2, -1, 2, 3}, homography_builder); - cv::imwrite("data/debug/test_matching/" + getTestSuiteName() + "_" + getTestName() + "_" + "ortho_root2.jpg", ortho2); - } - - int counter = 0; - std::function homography_builder = [&counter](const cv::Mat &lhs, const cv::Mat &rhs){ - ++counter; - return getHomography(lhs, rhs); - }; - - cv::Mat ortho = phg::stitchPanorama({img1, img2, img3, img4, img5}, {-1, 0, 1, 2, 3}, homography_builder); - cv::imwrite("data/debug/test_matching/" + getTestSuiteName() + "_" + getTestName() + "_" + "ortho_root0.jpg", ortho); - - // гомография должна быть посчитана для каждого ребра в графе по разу - EXPECT_EQ(counter, 4); - - int threshold_px = 250; - int score = getOrthoScore(ortho, cv::imread("data/src/test_matching/ortho/ortho_root0.jpg"), threshold_px); - std::cout << "n stable ortho kpts: : " << score << std::endl; - EXPECT_GT(score, 7500); -#endif -} \ No newline at end of file +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + + +#include "utils/test_utils.h" + + +// TODO enable both toggles for testing custom detector & matcher +#define ENABLE_MY_DESCRIPTOR 1 +#define ENABLE_MY_MATCHING 1 +#define ENABLE_GPU_BRUTEFORCE_MATCHER 0 + +// TODO disable for local testing but do not commit +#define SERVER_TESTING 1 + +#if SERVER_TESTING && !defined(__linux__) +#undef ENABLE_GPU_BRUTEFORCE_MATCHER +#define ENABLE_GPU_BRUTEFORCE_MATCHER 0 +#endif + +#if ENABLE_MY_MATCHING +const double max_keypoints_rmse_px = 1.0; +#else +const double max_keypoints_rmse_px = 10.0; +#endif + +const double max_color_rmse_8u = 20; + +#define GAUSSIAN_NOISE_STDDEV 1.0 + + +namespace { + + void drawMatches(const cv::Mat &img1, + const cv::Mat &img2, + const std::vector &keypoints1, + const std::vector &keypoints2, + const std::vector &matches, + const std::string &path) + { + cv::Mat img_matches; + drawMatches( img1, keypoints1, img2, keypoints2, matches, img_matches, cv::Scalar::all(-1), + cv::Scalar::all(-1), std::vector(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS ); + + cv::imwrite(path, img_matches); + } + + cv::Mat getHomography(const cv::Mat &img1, const cv::Mat &img2) + { + using namespace cv; + + cv::Ptr detector = cv::SIFT::create(); + std::vector keypoints1, keypoints2; + Mat descriptors1, descriptors2; + detector->detectAndCompute( img1, noArray(), keypoints1, descriptors1 ); + detector->detectAndCompute( img2, noArray(), keypoints2, descriptors2 ); + + std::vector< std::vector > knn_matches; + +#if ENABLE_MY_MATCHING + phg::FlannMatcher matcher; + matcher.train(descriptors2); + matcher.knnMatch(descriptors1, knn_matches, 2); +#else + Ptr matcher = DescriptorMatcher::create(DescriptorMatcher::FLANNBASED); + matcher->knnMatch( descriptors1, descriptors2, knn_matches, 2 ); +#endif + + std::vector good_matches(knn_matches.size()); + for (int i = 0; i < (int) knn_matches.size(); ++i) { + good_matches[i] = knn_matches[i][0]; + } + +#if ENABLE_MY_MATCHING + phg::DescriptorMatcher::filterMatchesRatioTest(knn_matches, good_matches); + { + std::vector tmp; + phg::DescriptorMatcher::filterMatchesClusters(good_matches, keypoints1, keypoints2, tmp); + std::swap(tmp, good_matches); + } +#else + { + std::vector tmp; + phg::filterMatchesGMS(good_matches, keypoints1, keypoints2, img1.size(), img2.size(), tmp); + std::swap(tmp, good_matches); + } +#endif + + std::vector points1, points2; + for (const cv::DMatch &match : good_matches) { + points1.push_back(keypoints1[match.queryIdx].pt); + points2.push_back(keypoints2[match.trainIdx].pt); + } + +#if ENABLE_MY_MATCHING + cv::Mat H = phg::findHomography(points1, points2); +#else + cv::Mat H = phg::findHomographyCV(points1, points2); +#endif + + return H; + } + + void evaluateStitching(const cv::Mat &img1, const cv::Mat &img2, double &keypoints_rmse, double &color_rmse, + const std::vector &keypoints1, const std::vector &keypoints2, + const cv::Mat &descriptors1, const cv::Mat &descriptors2) + { + using namespace cv; + + std::vector> knn_matches; + +#if ENABLE_MY_MATCHING + phg::FlannMatcher matcher; + matcher.train(descriptors2); + matcher.knnMatch(descriptors1, knn_matches, 2); +#else + Ptr matcher = DescriptorMatcher::create(DescriptorMatcher::FLANNBASED); + matcher->knnMatch( descriptors1, descriptors2, knn_matches, 2 ); +#endif + + std::vector good_matches(knn_matches.size()); + for (int i = 0; i < (int) knn_matches.size(); ++i) { + good_matches[i] = knn_matches[i][0]; + } + +#if ENABLE_MY_MATCHING + phg::DescriptorMatcher::filterMatchesRatioTest(knn_matches, good_matches); + { + std::vector tmp; + phg::DescriptorMatcher::filterMatchesClusters(good_matches, keypoints1, keypoints2, tmp); + std::swap(tmp, good_matches); + } +#else + { + std::vector tmp; + phg::filterMatchesGMS(good_matches, keypoints1, keypoints2, img1.size(), img2.size(), tmp); + std::swap(tmp, good_matches); + } +#endif + + std::vector points1, points2; + for (const cv::DMatch &match : good_matches) { + points1.push_back(keypoints1[match.queryIdx].pt); + points2.push_back(keypoints2[match.trainIdx].pt); + } +#if ENABLE_MY_MATCHING + cv::Mat H = phg::findHomography(points1, points2); +#else + cv::Mat H = phg::findHomographyCV(points1, points2); +#endif + + if (good_matches.size() < 4) { + throw std::runtime_error("too few matches"); + } + + keypoints_rmse = 0; + for (int i = 0; i < (int) good_matches.size(); ++i) { +#if ENABLE_MY_MATCHING + cv::Point2f pt = phg::transformPoint(points1[i], H); +#else + cv::Point2f pt = phg::transformPointCV(points1[i], H); +#endif + cv::Point2f diff = pt - points2[i]; + keypoints_rmse += diff.x * diff.x + diff.y * diff.y; + } + keypoints_rmse /= good_matches.size(); + keypoints_rmse = std::sqrt(keypoints_rmse); + + color_rmse = 0; + int64_t count = 0; + for (int y = 0; y < img1.rows; ++y) { + for (int x = 0; x < img1.cols; ++x) { + cv::Vec3b col1 = img1.at(y, x); +#if ENABLE_MY_MATCHING + cv::Point2f pt = phg::transformPoint(cv::Point2f(x, y), H); +#else + cv::Point2f pt = phg::transformPointCV(cv::Point2f(x, y), H); +#endif + int pt_x = std::round(pt.x); + int pt_y = std::round(pt.y); + if (pt_x >= 0 && pt_x < img2.cols && pt_y >= 0 && pt_y < img2.rows) { + cv::Vec3b col2 = img2.at(pt_y, pt_x); + int dc0 = int(col2[0]) - int(col1[0]); + int dc1 = int(col2[1]) - int(col1[1]); + int dc2 = int(col2[2]) - int(col1[2]); + + color_rmse += dc0 * dc0 + dc1 * dc1 + dc2 * dc2; + ++count; + } + } + } + + if (count) { + color_rmse /= count; + color_rmse = std::sqrt(color_rmse); + } + } + +} + +namespace { + void testStitching(const cv::Mat &img1, const cv::Mat &img2, const std::vector &keypoints1, const std::vector &keypoints2, + const cv::Mat &descriptors1, const cv::Mat &descriptors2) + { + double rmse_kpts, rmse_color; + evaluateStitching(img1, img2, rmse_kpts, rmse_color, keypoints1, keypoints2, descriptors1, descriptors2); + + std::cout << "keypoints RMSE: " << rmse_kpts << ", color RMSE: " << rmse_color << std::endl; + + EXPECT_LT(rmse_kpts, max_keypoints_rmse_px); + EXPECT_LT(rmse_color, max_color_rmse_8u); + } + + void testStitchingMultipleDetectors(const cv::Mat &img1, const cv::Mat &img2) + { + { + std::cout << "testing sift detector/descriptor..." << std::endl; + cv::Ptr detector = cv::SIFT::create(); + std::vector keypoints1, keypoints2; + cv::Mat descriptors1, descriptors2; + detector->detectAndCompute( img1, cv::noArray(), keypoints1, descriptors1 ); + detector->detectAndCompute( img2, cv::noArray(), keypoints2, descriptors2 ); + + testStitching(img1, img2, keypoints1, keypoints2, descriptors1, descriptors2); + } + +#if ENABLE_MY_DESCRIPTOR + { + std::cout << "testing my detector/descriptor..." << std::endl; + std::vector keypoints1, keypoints2; + cv::Mat descriptors1, descriptors2; + phg::SIFT mySIFT; + mySIFT.detectAndCompute(img1, keypoints1, descriptors1); + mySIFT.detectAndCompute(img2, keypoints2, descriptors2); + + testStitching(img1, img2, keypoints1, keypoints2, descriptors1, descriptors2); + } +#endif + } +} + +TEST (MATCHING, SimpleStitching) { + + cv::Mat img1 = cv::imread("data/src/test_matching/hiking_left.JPG"); + cv::Mat img2 = cv::imread("data/src/test_matching/hiking_right.JPG"); + + testStitchingMultipleDetectors(img1, img2); +} + +namespace { + + bool matcheq(const cv::DMatch &lhs, const cv::DMatch &rhs) + { + return std::tie(lhs.trainIdx, lhs.queryIdx, lhs.imgIdx) == std::tie(rhs.trainIdx, rhs.queryIdx, rhs.imgIdx); + } + + void evaluateMatching(const cv::Mat &img1, const cv::Mat &img2, const std::vector &keypoints1, const std::vector &keypoints2, + const cv::Mat &descriptors1, const cv::Mat &descriptors2, + double &nn_score, double &nn2_score, double &nn_score_cv, double &nn2_score_cv, + double &time_my, double &time_cv, double &time_bruteforce, double &time_bruteforce_gpu, + double &good_nn, double &good_ratio, double &good_clusters, double &good_ratio_and_clusters, bool do_bruteforce) + { + using namespace cv; + + if (!descriptors1.rows || !descriptors2.rows) { + throw std::runtime_error("empty descriptors"); + } + + timer tm; + + std::vector> knn_matches_flann, knn_matches_flann_cv, knn_matches_bruteforce; + + std::cout << "flann matching..." << std::endl; + tm.restart(); + #if ENABLE_MY_MATCHING + { + phg::FlannMatcher matcher; + matcher.train(descriptors2); + matcher.knnMatch(descriptors1, knn_matches_flann, 2); + } + #endif + time_my = tm.elapsed(); + + std::cout << "cv flann matching..." << std::endl; + tm.restart(); + { + Ptr matcher = DescriptorMatcher::create(DescriptorMatcher::FLANNBASED); + matcher->knnMatch( descriptors1, descriptors2, knn_matches_flann_cv, 2 ); + + #if !ENABLE_MY_MATCHING + knn_matches_flann = knn_matches_flann_cv; + #endif + } + time_cv = tm.elapsed(); + + tm.restart(); + if (do_bruteforce) { + std::cout << "brute force matching" << std::endl; + phg::BruteforceMatcher matcher; + matcher.train(descriptors2); + matcher.knnMatch(descriptors1, knn_matches_bruteforce, 2); + } + time_bruteforce = tm.elapsed(); + + tm.restart(); + std::vector> knn_matches_bruteforce_gpu; +#if ENABLE_GPU_BRUTEFORCE_MATCHER + if (do_bruteforce) { + std::cout << "brute force GPU matching" << std::endl; + phg::BruteforceMatcherGPU matcher; + matcher.train(descriptors2); + matcher.knnMatch(descriptors1, knn_matches_bruteforce_gpu, 2); + } +#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])); + } + } +#endif + + nn_score = 0; + nn2_score = 0; + nn_score_cv = 0; + nn2_score_cv = 0; + if (do_bruteforce) { + for (int i = 0; i < descriptors1.rows; ++i) { + if (knn_matches_bruteforce[i][0].queryIdx != i) { + throw std::runtime_error("invalid DMatch queryIdx for knn_matches_bruteforce"); + } + if (knn_matches_flann[i][0].queryIdx != i) { + throw std::runtime_error("invalid DMatch queryIdx for knn_matches_flann"); + } + if (knn_matches_flann_cv[i][0].queryIdx != i) { + throw std::runtime_error("invalid DMatch queryIdx for knn_matches_flann_cv"); + } + + if (knn_matches_flann[i][0].trainIdx == knn_matches_bruteforce[i][0].trainIdx) { + ++nn_score; + } + + if (knn_matches_flann[i][1].trainIdx == knn_matches_bruteforce[i][1].trainIdx) { + ++nn2_score; + } + + if (knn_matches_flann_cv[i][0].trainIdx == knn_matches_bruteforce[i][0].trainIdx) { + ++nn_score_cv; + } + + if (knn_matches_flann_cv[i][1].trainIdx == knn_matches_bruteforce[i][1].trainIdx) { + ++nn2_score_cv; + } + } + + nn_score /= descriptors1.rows; + nn2_score /= descriptors1.rows; + nn_score_cv /= descriptors1.rows; + nn2_score_cv /= descriptors1.rows; + } + + std::vector good_matches_nn(knn_matches_flann.size()); + for (int i = 0; i < (int) knn_matches_flann.size(); ++i) { + good_matches_nn[i] = knn_matches_flann[i][0]; + } + drawMatches(img1, img2, keypoints1, keypoints2, good_matches_nn, "data/debug/test_matching/" + getTestSuiteName() + "_" + getTestName() + "_" + "00_matches_nn.png"); + + #if ENABLE_MY_MATCHING + std::cout << "filtering matches by ratio test..." << std::endl; + std::vector good_matches_ratio; + phg::DescriptorMatcher::filterMatchesRatioTest(knn_matches_flann, good_matches_ratio); + drawMatches(img1, img2, keypoints1, keypoints2, good_matches_ratio, "data/debug/test_matching/" + getTestSuiteName() + "_" + getTestName() + "_" + "01_matches_ratio.png"); + + std::cout << "filtering matches by clusters..." << std::endl; + std::vector good_matches_clusters_only; + phg::DescriptorMatcher::filterMatchesClusters(good_matches_nn, keypoints1, keypoints2, good_matches_clusters_only); + drawMatches(img1, img2, keypoints1, keypoints2, good_matches_clusters_only, "data/debug/test_matching/" + getTestSuiteName() + "_" + getTestName() + "_" + "03_matches_clusters_only.png"); + + std::cout << "filtering matches by ratio & clusters" << std::endl; + std::vector good_matches_clusters_and_ratio; + phg::DescriptorMatcher::filterMatchesClusters(good_matches_ratio, keypoints1, keypoints2, good_matches_clusters_and_ratio); + drawMatches(img1, img2, keypoints1, keypoints2, good_matches_clusters_and_ratio, "data/debug/test_matching/" + getTestSuiteName() + "_" + getTestName() + "_" + "04_matches_clusters_and_ratio.png"); + #else + std::vector good_matches_clusters_and_ratio; + phg::filterMatchesGMS(good_matches_nn, keypoints1, keypoints2, img1.size(), img2.size(), good_matches_clusters_and_ratio); + drawMatches(img1, img2, keypoints1, keypoints2, good_matches_clusters_and_ratio, "data/debug/test_matching/" + getTestSuiteName() + "_" + getTestName() + "_" + "04_matches_gms.png"); + #endif + + std::cout << "estimating homography..." << std::endl; + cv::Mat H; + { + std::vector points1, points2; + for (const cv::DMatch &match : good_matches_clusters_and_ratio) { + points1.push_back(keypoints1[match.queryIdx].pt); + points2.push_back(keypoints2[match.trainIdx].pt); + } + +#if ENABLE_MY_MATCHING + H = phg::findHomography(points1, points2); +#else + H = phg::findHomographyCV(points1, points2); +#endif + } + + good_nn = 0; + good_ratio = 0; + good_clusters = 0; + good_ratio_and_clusters = 0; + + std::cout << "evaluating homography..." << std::endl; + + #if ENABLE_MY_MATCHING + const int ntest = 4; + std::vector* arrs[ntest] = {&good_matches_nn, &good_matches_ratio, &good_matches_clusters_only, &good_matches_clusters_and_ratio}; + double* ptrs[ntest] = {&good_nn, &good_ratio, &good_clusters, &good_ratio_and_clusters}; + #else + const int ntest = 2; + std::vector* arrs[ntest] = {&good_matches_nn, &good_matches_clusters_and_ratio}; + double* ptrs[ntest] = {&good_nn, &good_ratio_and_clusters}; + #endif + + for (int i_test = 0; i_test < ntest; ++i_test) { + + const std::vector &arr = *arrs[i_test]; + + if (arr.size() < 50) { + std::cerr << "too few matches: " + std::to_string(arr.size()) << std::endl; + continue; + } + + std::vector points1, points2; + for (const cv::DMatch &match : arr) { + points1.push_back(keypoints1[match.queryIdx].pt); + points2.push_back(keypoints2[match.trainIdx].pt); + } + + (*ptrs[i_test]) = 0; + for (int i = 0; i < (int) arr.size(); ++i) { +#if ENABLE_MY_MATCHING + cv::Point2f pt = phg::transformPoint(points1[i], H); +#else + cv::Point2f pt = phg::transformPointCV(points1[i], H); +#endif + + cv::Point2f diff = pt - points2[i]; + float dist2 = diff.x * diff.x + diff.y * diff.y; + if (dist2 < max_keypoints_rmse_px * max_keypoints_rmse_px) { + ++(*ptrs[i_test]); + } + } + if (arr.size()) { + (*ptrs[i_test]) /= arr.size(); + } + } + + } + + void testMatching(const cv::Mat &img1, const cv::Mat &img2, const std::vector &keypoints1, const std::vector &keypoints2, + const cv::Mat &descriptors1, const cv::Mat &descriptors2, + double &nn_score, double &nn2_score, double &nn_score_cv, double &nn2_score_cv, + double &time_my, double &time_cv, double &time_bruteforce, double &time_bruteforce_gpu, + double &good_nn, double &good_ratio, double &good_clusters, double &good_ratio_and_clusters, + bool do_bruteforce + ) + { + evaluateMatching(img1, img2, keypoints1, keypoints2, descriptors1, descriptors2, + nn_score, nn2_score, nn_score_cv, nn2_score_cv, + time_my, time_cv, time_bruteforce, time_bruteforce_gpu, + good_nn, good_ratio, good_clusters, good_ratio_and_clusters, do_bruteforce); + + std::cout << "nn_score: " << nn_score << ", "; + std::cout << "nn2_score: " << nn2_score << ", "; + std::cout << "nn_score_cv: " << nn_score_cv << ", "; + std::cout << "nn2_score_cv: " << nn2_score_cv << ", "; + std::cout << "time_my: " << time_my << ", "; + 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 << ", "; +#endif + std::cout << "good_nn: " << good_nn << ", "; + std::cout << "good_ratio: " << good_ratio << ", "; + std::cout << "good_clusters: " << good_clusters << ", "; + std::cout << "good_ratio_and_clusters: " << good_ratio_and_clusters << std::endl; + } + + void testMatchingMultipleDetectors(const cv::Mat &img1, const cv::Mat &img2, + double &nn_score, double &nn2_score, double &nn_score_cv, double &nn2_score_cv, + double &time_my, double &time_cv, double &time_bruteforce, double &time_bruteforce_gpu, + double &good_nn, double &good_ratio, double &good_clusters, double &good_ratio_and_clusters, bool do_bruteforce = true) + { + { + std::cout << "testing sift detector/descriptor..." << std::endl; + cv::Ptr detector = cv::SIFT::create(); + std::vector keypoints1, keypoints2; + cv::Mat descriptors1, descriptors2; + detector->detectAndCompute( img1, cv::noArray(), keypoints1, descriptors1 ); + detector->detectAndCompute( img2, cv::noArray(), keypoints2, descriptors2 ); + + testMatching(img1, img2, keypoints1, keypoints2, descriptors1, descriptors2, + nn_score, nn2_score, nn_score_cv, nn2_score_cv, + time_my, time_cv, time_bruteforce, time_bruteforce_gpu, + good_nn, good_ratio, good_clusters, good_ratio_and_clusters, do_bruteforce); + } +#if ENABLE_MY_DESCRIPTOR + { + std::cout << "testing my detector/descriptor..." << std::endl; + std::vector keypoints1, keypoints2; + cv::Mat descriptors1, descriptors2; + phg::SIFT mySIFT; + mySIFT.detectAndCompute(img1, keypoints1, descriptors1); + mySIFT.detectAndCompute(img2, keypoints2, descriptors2); + + testMatching(img1, img2, keypoints1, keypoints2, descriptors1, descriptors2, + nn_score, nn2_score, nn_score_cv, nn2_score_cv, + time_my, time_cv, time_bruteforce, time_bruteforce_gpu, + good_nn, good_ratio, good_clusters, good_ratio_and_clusters, do_bruteforce); + } +#endif + } + +} + +TEST (MATCHING, SimpleMatching) { + + cv::Mat img1 = cv::imread("data/src/test_matching/hiking_left.JPG"); + cv::Mat img2 = cv::imread("data/src/test_matching/hiking_right.JPG"); + + + double nn_score, nn2_score, nn_score_cv, nn2_score_cv, + time_my, time_cv, time_bruteforce, time_bruteforce_gpu, good_nn, good_ratio, good_clusters, good_ratio_and_clusters; + + testMatchingMultipleDetectors(img1, img2, + nn_score, nn2_score, nn_score_cv, nn2_score_cv, + time_my, time_cv, time_bruteforce, time_bruteforce_gpu, + good_nn, good_ratio, good_clusters, good_ratio_and_clusters); + + + + EXPECT_GT(nn_score, 0.9 * nn_score_cv); + EXPECT_GT(nn2_score, 0.9 * nn2_score_cv); + + EXPECT_LT(time_my, 1.5 * time_cv); + EXPECT_LT(time_my, 0.1 * time_bruteforce); + +#if ENABLE_GPU_BRUTEFORCE_MATCHER && !SERVER_TESTING + EXPECT_LT(time_bruteforce_gpu, time_bruteforce); +#endif + +#if ENABLE_MY_MATCHING + EXPECT_LT(good_nn, good_ratio); + EXPECT_LT(good_nn, good_clusters); +#endif + EXPECT_LT(good_nn, good_ratio_and_clusters); + + EXPECT_GT(good_nn, 0.2); +#if ENABLE_MY_MATCHING + EXPECT_GT(good_ratio, 0.9); + EXPECT_GT(good_clusters, 0.9); +#endif + EXPECT_GT(good_ratio_and_clusters, 0.9); +} + +namespace { + + cv::Mat transformImg(const cv::Mat &img2, double angleDegreesClockwise, double scale) + { + cv::Mat M = cv::getRotationMatrix2D(cv::Point(0, 0), 0, scale); + M.at(0, 2) = img2.cols * 0.25 * scale; + M.at(1, 2) = img2.rows * 0.25 * scale; + cv::Mat tmp; + cv::warpAffine(img2, tmp, M, cv::Size(1.5 * img2.cols * scale, 1.5 * img2.rows * scale)); + + cv::Mat transformedImage; + M = cv::getRotationMatrix2D(cv::Point(tmp.cols / 2, tmp.rows / 2), -angleDegreesClockwise, 1.0); + cv::warpAffine(tmp, transformedImage, M, cv::Size(tmp.cols, tmp.rows)); + + return transformedImage; + } + + void addNoise(cv::Mat &img2) + { + cv::Mat noise(cv::Size(img2.cols, img2.rows), CV_8UC3); + cv::setRNGSeed(125125); // фиксируем рандом для детерминизма (чтобы результат воспроизводился из раза в раз) + cv::randn(noise, cv::Scalar::all(0), cv::Scalar::all(GAUSSIAN_NOISE_STDDEV)); + cv::add(img2, noise, img2); // добавляем к преобразованной картинке гауссиан шума + } + + void testMatchingTransformWrapper(double angleDegreesClockwise, double scale) + { + cv::Mat img1 = cv::imread("data/src/test_matching/hiking_left.JPG"); + cv::Mat img2 = cv::imread("data/src/test_matching/hiking_right.JPG"); + + img2 = transformImg(img2, angleDegreesClockwise, scale); + addNoise(img2); + + cv::imwrite("data/debug/test_matching/" + getTestSuiteName() + "_" + getTestName() + "_" + "hiking_right_rotated_noise.png", img2); + + double nn_score, nn2_score, nn_score_cv, nn2_score_cv, + time_my, time_cv, time_bruteforce, time_bruteforce_gpu, good_nn, good_ratio, good_clusters, good_ratio_and_clusters; + + testMatchingMultipleDetectors(img1, img2, + nn_score, nn2_score, nn_score_cv, nn2_score_cv, + time_my, time_cv, time_bruteforce, time_bruteforce_gpu, + good_nn, good_ratio, good_clusters, good_ratio_and_clusters, false); + + EXPECT_LT(time_my, 1.5 * time_cv); + +#if ENABLE_MY_MATCHING + EXPECT_LT(good_nn, good_ratio); +#endif + EXPECT_LT(good_nn, good_ratio_and_clusters); + +#if ENABLE_MY_MATCHING + EXPECT_GT(good_ratio, 0.7); +#endif + EXPECT_GT(good_ratio_and_clusters, 0.7); + } + +} + +TEST (MATCHING, Rotate10) { + double angleDegreesClockwise = 10; + double scale = 1.0; + + testMatchingTransformWrapper(angleDegreesClockwise, scale); +} + +TEST (MATCHING, Rotate20) { + double angleDegreesClockwise = 20; + double scale = 1.0; + + testMatchingTransformWrapper(angleDegreesClockwise, scale); +} + +TEST (MATCHING, Rotate30) { + double angleDegreesClockwise = 30; + double scale = 1.0; + + testMatchingTransformWrapper(angleDegreesClockwise, scale); +} + +TEST (MATCHING, Rotate40) { + double angleDegreesClockwise = 40; + double scale = 1.0; + + testMatchingTransformWrapper(angleDegreesClockwise, scale); +} + +TEST (MATCHING, Rotate45) { + double angleDegreesClockwise = 45; + double scale = 1.0; + + testMatchingTransformWrapper(angleDegreesClockwise, scale); +} + +TEST (MATCHING, Rotate90) { + double angleDegreesClockwise = 90; + double scale = 1.0; + + testMatchingTransformWrapper(angleDegreesClockwise, scale); +} + +TEST (MATCHING, Scale50) { + // seems to be some issue with gms matcher and high downscale +#if ENABLE_MY_MATCHING + double angleDegreesClockwise = 0; + double scale = 0.5; + + testMatchingTransformWrapper(angleDegreesClockwise, scale); +#endif +} + +TEST (MATCHING, Scale70) { + double angleDegreesClockwise = 0; + double scale = 0.7; + + testMatchingTransformWrapper(angleDegreesClockwise, scale); +} + +TEST (MATCHING, Scale90) { + double angleDegreesClockwise = 0; + double scale = 0.9; + + testMatchingTransformWrapper(angleDegreesClockwise, scale); +} + +TEST (MATCHING, Scale110) { + double angleDegreesClockwise = 0; + double scale = 1.1; + + testMatchingTransformWrapper(angleDegreesClockwise, scale); +} + +TEST (MATCHING, Scale130) { + double angleDegreesClockwise = 0; + double scale = 1.3; + + testMatchingTransformWrapper(angleDegreesClockwise, scale); +} + +TEST (MATCHING, Scale150) { + double angleDegreesClockwise = 0; + double scale = 1.5; + + testMatchingTransformWrapper(angleDegreesClockwise, scale); +} + +TEST (MATCHING, Scale175) { + double angleDegreesClockwise = 0; + double scale = 1.75; + + testMatchingTransformWrapper(angleDegreesClockwise, scale); +} + +TEST (MATCHING, Scale200) { + double angleDegreesClockwise = 0; + double scale = 2.0; + + testMatchingTransformWrapper(angleDegreesClockwise, scale); +} + +TEST (MATCHING, Rotate10Scale90) { + double angleDegreesClockwise = 10; + double scale = 0.9; + + testMatchingTransformWrapper(angleDegreesClockwise, scale); +} + +TEST (MATCHING, Rotate30Scale75) { + double angleDegreesClockwise = 30; + double scale = 0.75; + + testMatchingTransformWrapper(angleDegreesClockwise, scale); +} + +TEST (STITCHING, SimplePanorama) { +#if ENABLE_MY_MATCHING + cv::Mat img1 = cv::imread("data/src/test_matching/hiking_left.JPG"); + cv::Mat img2 = cv::imread("data/src/test_matching/hiking_right.JPG"); + + std::function homography_builder = [](const cv::Mat &lhs, const cv::Mat &rhs){ return getHomography(lhs, rhs); }; + cv::Mat pano = phg::stitchPanorama({img1, img2}, {-1, 0}, homography_builder); + cv::imwrite("data/debug/test_matching/" + getTestSuiteName() + "_" + getTestName() + "_" + "panorama.png", pano); +#endif +} + +namespace { + + int getOrthoScore(const cv::Mat &ortho0, const cv::Mat &ortho1, int threshold_px) + { + using namespace cv; + + cv::Ptr detector = cv::SIFT::create(); + std::vector keypoints1, keypoints2; + cv::Mat descriptors1, descriptors2; + detector->detectAndCompute( ortho0, cv::noArray(), keypoints1, descriptors1 ); + detector->detectAndCompute( ortho1, cv::noArray(), keypoints2, descriptors2 ); + + std::vector< std::vector > knn_matches; + + phg::FlannMatcher matcher; + matcher.train(descriptors2); + matcher.knnMatch(descriptors1, knn_matches, 2); + + std::vector good_matches(knn_matches.size()); + for (int i = 0; i < (int) knn_matches.size(); ++i) { + good_matches[i] = knn_matches[i][0]; + } + + phg::DescriptorMatcher::filterMatchesRatioTest(knn_matches, good_matches); + + { + std::vector tmp; + phg::DescriptorMatcher::filterMatchesClusters(good_matches, keypoints1, keypoints2, tmp); + std::swap(tmp, good_matches); + } + + int score = 0; + for (const cv::DMatch &match : good_matches) { + cv::Point2f d = keypoints1[match.queryIdx].pt - keypoints2[match.trainIdx].pt; + if (d.x * d.x + d.y * d.y < threshold_px * threshold_px) { + ++score; + } + } + + drawMatches(ortho0, ortho1, keypoints1, keypoints2, good_matches, "data/debug/test_matching/" + getTestSuiteName() + "_" + getTestName() + "_" + "ortho_matches.png"); + + return score; + } + +} + +TEST (STITCHING, Orthophoto) { +#if ENABLE_MY_MATCHING + cv::Mat img1 = cv::imread("data/src/test_matching/ortho/IMG_160729_071349_0000_RGB.JPG"); + cv::Mat img2 = cv::imread("data/src/test_matching/ortho/IMG_160729_071351_0001_RGB.JPG"); + cv::Mat img3 = cv::imread("data/src/test_matching/ortho/IMG_160729_071353_0002_RGB.JPG"); + cv::Mat img4 = cv::imread("data/src/test_matching/ortho/IMG_160729_071356_0003_RGB.JPG"); + cv::Mat img5 = cv::imread("data/src/test_matching/ortho/IMG_160729_071358_0004_RGB.JPG"); + + { + std::function homography_builder = [](const cv::Mat &lhs, const cv::Mat &rhs){ return getHomography(lhs, rhs); }; + cv::Mat ortho2 = phg::stitchPanorama({img1, img2, img3, img4, img5}, {1, 2, -1, 2, 3}, homography_builder); + cv::imwrite("data/debug/test_matching/" + getTestSuiteName() + "_" + getTestName() + "_" + "ortho_root2.jpg", ortho2); + } + + int counter = 0; + std::function homography_builder = [&counter](const cv::Mat &lhs, const cv::Mat &rhs){ + ++counter; + return getHomography(lhs, rhs); + }; + + cv::Mat ortho = phg::stitchPanorama({img1, img2, img3, img4, img5}, {-1, 0, 1, 2, 3}, homography_builder); + cv::imwrite("data/debug/test_matching/" + getTestSuiteName() + "_" + getTestName() + "_" + "ortho_root0.jpg", ortho); + + // гомография должна быть посчитана для каждого ребра в графе по разу + EXPECT_EQ(counter, 4); + + int threshold_px = 250; + int score = getOrthoScore(ortho, cv::imread("data/src/test_matching/ortho/ortho_root0.jpg"), threshold_px); + std::cout << "n stable ortho kpts: : " << score << std::endl; + EXPECT_GT(score, 7500); +#endif +} diff --git a/tests/test_sfm.cpp b/tests/test_sfm.cpp index 4229b86..bfd2467 100644 --- a/tests/test_sfm.cpp +++ b/tests/test_sfm.cpp @@ -18,7 +18,7 @@ #include "utils/test_utils.h" -#define ENABLE_MY_SFM 0 +#define ENABLE_MY_SFM 1 namespace { @@ -237,7 +237,16 @@ TEST (SFM, EmatrixSimple) { pts0.push_back({(double) (std::rand() % calib.width()), (double) (std::rand() % calib.height())}); pts1.push_back({(double) (std::rand() % calib.width()), (double) (std::rand() % calib.height())}); } - + + // display points + std::cout << "pts0:\n"; + for (const auto &pt : pts0) { + std::cout << pt << std::endl; + } + std::cout << "pts1:\n"; + for (const auto &pt : pts1) { + std::cout << pt << std::endl; + } matrix3d F = phg::findFMatrix(pts0, pts1, 10); matrix3d E = phg::fmatrix2ematrix(F, calib, calib); @@ -532,6 +541,8 @@ TEST (SFM, Resection) { #endif using namespace cv; + // here we perform resection of two cameras using the same set of 3d points, + // which were obtained by triangulation in the previous test. const cv::Mat img1 = cv::imread("data/src/test_sfm/saharov/IMG_3023.JPG"); const cv::Mat img2 = cv::imread("data/src/test_sfm/saharov/IMG_3024.JPG"); @@ -543,6 +554,7 @@ TEST (SFM, Resection) { cv::Ptr detector = cv::SIFT::create(); std::vector keypoints1, keypoints2; cv::Mat descriptors1, descriptors2; + // step 0: detect points detector->detectAndCompute( img1, cv::noArray(), keypoints1, descriptors1 ); detector->detectAndCompute( img2, cv::noArray(), keypoints2, descriptors2 ); @@ -550,6 +562,7 @@ TEST (SFM, Resection) { std::vector> knn_matches; Ptr matcher = DescriptorMatcher::create(DescriptorMatcher::FLANNBASED); + // step 1: match points matcher->knnMatch( descriptors1, descriptors2, knn_matches, 2 ); std::vector good_matches(knn_matches.size()); @@ -558,6 +571,7 @@ TEST (SFM, Resection) { } std::cout << "filtering matches GMS..." << std::endl; + // step 2: filter matches using GMS std::vector good_matches_gms; phg::filterMatchesGMS(good_matches, keypoints1, keypoints2, img1.size(), img2.size(), good_matches_gms); @@ -569,16 +583,19 @@ TEST (SFM, Resection) { points2.push_back(pt2); } + // step 3: find F matrix, E matrix matrix3d F = phg::findFMatrix(points1, points2); matrix3d E = phg::fmatrix2ematrix(F, calib0, calib1); matrix34d P0, P1; + // step 4: when we know the E matrix, we can find the relative position of cameras up to scale, by decomposing E matrix. phg::decomposeEMatrix(P0, P1, E, points1, points2, calib0, calib1); matrix3d R0, R1; vector3d O0, O1; - phg::decomposeUndistortedPMatrix(R0, O0, P0); - phg::decomposeUndistortedPMatrix(R1, O1, P1); + // step 5: decompose P matrices to get rotation and camera center + phg::decomposeUndistortedPMatrix(R0, O0, P0); // P0 is the reference camera, so it should be close to [I | 0] + phg::decomposeUndistortedPMatrix(R1, O1, P1); // P1 should be close to the actual position of the second camera std::cout << "Camera positions: " << std::endl; std::cout << "R0:\n" << R0 << std::endl; @@ -586,11 +603,19 @@ TEST (SFM, Resection) { std::cout << "R1:\n" << R1 << std::endl; std::cout << "O1: " << O1.t() << std::endl; - std::vector Xs; - std::vector x0s; + // by this point we have obtained the relative position of two cameras. + // Now we can perform resection, i.e. find camera matrices using 3d-2d correspondences, + // and compare them with the original ones (P0 and P1, obtained by decomposing E matrix). + + std::vector Xs; // Xs are the 3d points obtained by triangulation of matched points. They are used as "tie points" for resection. + std::vector x0s; // x0s and x1s are the corresponding 2d points in the first and second images, respectively. std::vector x1s; - matrix34d Ps[2] = {P0, P1}; + // so we like roll back the process of triangulation, and wanna find the same 3d points using resection. + // If the resection is correct, the obtained camera matrices should be close to the original ones (P0 and P1). + + matrix34d Ps[2] = {P0, P1}; // Ps are the original camera matrices, obtained by decomposing E matrix. We will use them for triangulation of points, and then compare with the results of resection. + // step 6: triangulate points using the original camera matrices to get 3d points (Xs), and their corresponding 2d projections in both images (x0s and x1s) for (int i = 0; i < (int) good_matches_gms.size(); ++i) { vector3d ms[2] = {calib0.unproject(points1[i]), calib1.unproject(points2[i])}; vector4d X = phg::triangulatePoint(Ps, ms, 2); @@ -605,6 +630,7 @@ TEST (SFM, Resection) { x1s.push_back(points2[i]); } + // step 7: perform resection using the obtained 3d-2d correspondences, and compare the obtained camera matrices with the original ones. matrix34d P0res = phg::findCameraMatrix(calib0, Xs, x0s); matrix34d P1res = phg::findCameraMatrix(calib1, Xs, x1s); diff --git a/tests/test_sift.cpp b/tests/test_sift.cpp index cf3bd7d..8345c4f 100755 --- a/tests/test_sift.cpp +++ b/tests/test_sift.cpp @@ -1,940 +1,945 @@ -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include "utils/test_utils.h" - -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif - -#define SHOW_RESULTS 0 // если вам хочется сразу видеть результат в окошке - переключите в 1, но не забудьте выключить перед коммитом (иначе бот в CI будет ждать веками) -#define MAX_ACCEPTED_PIXEL_ERROR \ - 0.01 // максимальное расстояние в пикселях (процент от ширины картинки) между ключевыми точками чтобы их можно было зачесть как "почти совпавшие" (это очень завышенный порог, по-хорошему должно быть 0.005 например) -#define MAX_AVG_PIXEL_ERROR 0.075 - -#define GAUSSIAN_NOISE_STDDEV 1.0 - -// TODO ENABLE ME -// TODO ENABLE ME -// TODO ENABLE ME -#define ENABLE_MY_SIFT_TESTING 0 - -#define DENY_CREATE_REF_DATA 1 - -struct MatchingPairData { - size_t npoints1, npoints2, nmatches; -}; - -template MatchingPairData evaluateMatching(SIFT& sift, const cv::Mat& img1, const cv::Mat& img2, const std::string& output_matches_img_path) -{ - std::vector kpts1; - cv::Mat desc1; - sift.detectAndCompute(img1, { }, kpts1, desc1); - - std::vector kpts2; - cv::Mat desc2; - sift.detectAndCompute(img2, { }, kpts2, desc2); - - // Brute-force matching with ratio test - cv::BFMatcher matcher(cv::NORM_L2); - std::vector> knnMatches; - matcher.knnMatch(desc1, desc2, knnMatches, 2); - - const float ratioThresh = 0.75f; - std::vector goodMatches; - for (const auto& m : knnMatches) { - if (m.size() == 2 && m[0].distance < ratioThresh * m[1].distance) { - goodMatches.push_back(m[0]); - } - } - - // RANSAC filtering with fundamental matrix - std::vector inlierMatches; - if (goodMatches.size() >= 15) { - std::vector pts1, pts2; - for (const auto& m : goodMatches) { - pts1.push_back(kpts1[m.queryIdx].pt); - pts2.push_back(kpts2[m.trainIdx].pt); - } - - std::vector inlierMask; - cv::findFundamentalMat(pts1, pts2, cv::FM_RANSAC, 3.0, 0.99, inlierMask); - - for (size_t i = 0; i < goodMatches.size(); i++) { - if (inlierMask[i]) { - inlierMatches.push_back(goodMatches[i]); - } - } - } - - std::cout << "N keypoints: left " << kpts1.size() << ", right " << kpts2.size() << std::endl; - std::cout << "Good matches: " << goodMatches.size() << std::endl; - std::cout << "Inlier matches: " << inlierMatches.size() << std::endl; - - if (!output_matches_img_path.empty()) { - cv::Mat imgMatches; - cv::drawMatches(img1, kpts1, img2, kpts2, inlierMatches, imgMatches); - cv::imwrite(output_matches_img_path, imgMatches); - } - - return { kpts1.size(), kpts2.size(), inlierMatches.size() }; -} - -// функция рисует кружки случайного цвета вокруг точек, но если для точки не нашлось сопоставления - кружок будет толстый и ярко красный -void drawKeyPoints(cv::Mat& img, const std::vector& kps, const std::vector& is_not_matched) -{ - cv::RNG r(124124); - for (size_t i = 0; i < kps.size(); ++i) { - int thickness = 1; - cv::Scalar color; - if (is_not_matched[i]) { - color = CV_RGB(255, 0, 0); // OpenCV использует BGR схему вместо RGB, но можно использовать этот макрос вместо BGR - cv::Scalar(blue=0, green=0, red=255) - thickness = 2; - } else { - color = cv::Scalar(r.uniform(0, 255), r.uniform(0, 255), 0); - } - int radius = std::max(2, (int)(kps[i].size / 5.0f)); - float angle = kps[i].angle; - cv::circle(img, kps[i].pt, radius, color, thickness); - if (angle != -1.0) { - cv::line(img, kps[i].pt, cv::Point((int)std::round(kps[i].pt.x + radius * sin(angle * M_PI / 180.0)), (int)std::round(kps[i].pt.y + radius * cos(angle * M_PI / 180.0))), color); - } - } -} - -// Функция ищет знаковый угол между двумя направлениями (по кратчайшему пути, т.е. результат от -180 до 180) -double diffAngles(double angle0, double angle1) -{ - if (angle0 != -1.0 && angle1 != -1.0) { - rassert(angle0 >= 0.0 && angle0 < 360.0, 1235612352151); - rassert(angle1 >= 0.0 && angle1 < 360.0, 4645315415); - float diff; - if ((angle1 <= angle0 + 180 && angle0 + 180 <= 360) || (angle1 >= angle0 - 180 && angle0 - 180 >= 0)) { - diff = angle1 - angle0; - } else if (angle1 > angle0 + 180 && angle0 + 180 <= 360) { - diff = -(angle0 + (360 - angle1)); - } else if (angle1 <= angle0 - 180 && angle0 - 180 >= 0) { - diff = (360 - angle0) + angle1; - } else { - rassert(false, 1234124125125135); - } - rassert(diff >= -180 && diff <= 180, 233536136131); - return diff; - } else { - return 0.0; - } -} - -// На вход передается матрица описывающая преобразование картинки (сдвиг, поворот, масштабирование или их комбинация), допустимый процент Recall, и опционально можно тестировать другую картинку -void evaluateDetection(const cv::Mat& M, double minRecall, cv::Mat img0 = cv::Mat()) -{ - if (img0.empty()) { - img0 = cv::imread("data/src/test_sift/unicorn.png"); // грузим картинку по умолчанию - } - - ASSERT_FALSE(img0.empty()); // проверка что картинка была загружена - // убедитесь что рабочая папка (Edit Configurations...->Working directory) указывает на корневую папку проекта (и тогда картинка по умолчанию найдется по относительному пути - data/src/test_sift/unicorn.png) - - size_t width = img0.cols; - size_t height = img0.rows; - cv::Mat transformedImage; - cv::warpAffine(img0, transformedImage, M, cv::Size(width, height)); // строим img1 - преобразованная исходная картинка в соответствии с закодированным в матрицу M искажением пространства - cv::Mat noise(cv::Size(width, height), CV_8UC3); - cv::setRNGSeed(125125); // фиксируем рандом для детерминизма (чтобы результат воспроизводился из раза в раз) - cv::randn(noise, cv::Scalar::all(0), cv::Scalar::all(GAUSSIAN_NOISE_STDDEV)); - cv::add(transformedImage, noise, transformedImage); // добавляем к преобразованной картинке гауссиан шума - cv::Mat img1 = transformedImage; - - { - for (int method = 0; method < 3; ++method) { // тестируем три метода: OpenCV ORB, OpenCV SIFT, ваш SIFT - std::vector kps0; - std::vector kps1; - - cv::Mat desc0; - cv::Mat desc1; - - timer t; // очень удобно встраивать профилирование вашего кода по мере его написания, тогда полную картину видеть гораздо проще (особенно это помогает со старым кодом) - std::string method_name; - std::string log_prefix; - - phg::SIFTParams p; - p.nfeatures = 500; - if (method == 0) { - method_name = "ORB"; - log_prefix = "[ORB_OCV] "; - // ORB - один из видов ключевых дескрипторов, отличается высокой скоростью и относительно неплохим качеством - cv::Ptr detector = cv::ORB::create(p.nfeatures); // здесь можно было бы поиграть с его параметрами, например выделять больше чем 500 точек, строить большее число ступеней пирамиды и т.п. - detector->detect(img0, kps0); // детектируем ключевые точки на исходной картинке - detector->detect(img1, kps1); // детектируем ключевые точки на преобразованной картинке - - detector->compute(img0, kps0, desc0); - detector->compute(img1, kps1, desc1); - } else if (method == 1) { - method_name = "SIFTOCV"; - log_prefix = "[SIFTOCV] "; - cv::Ptr detector = cv::SIFT::create( - p.nfeatures, p.n_octave_layers, p.contrast_threshold, p.edge_threshold); // здесь можно было бы поиграть с его параметрами, например выделять больше чем 500 точек, строить большее число ступеней пирамиды и т.п. - detector->detect(img0, kps0); // детектируем ключевые точки на исходной картинке - detector->detect(img1, kps1); // детектируем ключевые точки на преобразованной картинке - - detector->compute(img0, kps0, desc0); - detector->compute(img1, kps1, desc1); - } else if (method == 2) { -#if ENABLE_MY_SIFT_TESTING - method_name = "SIFT_MY"; - log_prefix = "[SIFT_MY] "; - phg::SIFT mySIFT(p); - mySIFT.detectAndCompute(img0, kps0, desc0); - mySIFT.detectAndCompute(img1, kps1, desc1); -#else - return; -#endif - } else { - rassert(false, 13532513412); // это не проверка как часть тестирования, это проверка что число итераций в цикле и if-else ветки все еще согласованы и не разошлись - } - - std::cout << log_prefix << "Points detected: " << kps0.size() << " -> " << kps1.size() << " (in " << t.elapsed() << " sec)" << std::endl; - - std::vector ps01(kps0.size()); // давайте построим эталон - найдем куда бы должны были сместиться ключевые точки с исходного изображения с учетом нашей матрицы трансформации M - { - std::vector ps0(kps0.size()); // здесь мы сейчас расположим детектированные ключевые точки (каждую нужно преобразовать из типа КлючеваяТочка в Точка2Дэ) - for (size_t i = 0; i < kps0.size(); ++i) { - ps0[i] = kps0[i].pt; - } - cv::transform(ps0, ps01, M); // преобразовываем все точки с исходного изображения в систему координат его искаженной версии с учетом матрицы M, эти точки - эталон - } - - double error_sum = 0.0; // считаем суммарную ошибку координат сопоставлений точек чтобы найти среднюю ошибку (в пикселях) - double size_ratio_sum = 0.0; // хотим найти среднее соотношение размера сопоставленных ключевых точек (чтобы сверить эту пропорцию с тестируемым перепадом масштаба) - double angle_diff_sum = 0.0; // хотим найти среднее отличие угла наклона сопоставленных ключевых точек (чтобы сверить этот угол с тестируемым в тестах поворотом) - double desc_dist_sum = 0.0; // хотим найти среднее расстояние между дескрипторами сопоставленных ключевых точек - double desc_rand_dist_sum = 0.0; // найдем среднее расстояние между случайными парами ключевых точек (чтобы было с чем сравнить расстояние сопоставленных точек) - size_t n_matched = 0; // число успешно сопоставившихся исходных точек - size_t n_in_bounds = 0; // число исходных точек которые после преобразования координат не вышли за пределы картинки (т.е. в целом имели шансы на успешное сопоставление) - std::vector is_not_matched0(kps0.size(), true); // для каждой исходной точки хотим понять сопоставилась ли она - std::vector is_not_matched1(kps1.size(), true); // для каждой точки с результирующей картинки хотим понять сопоставился ли с ней хоть кто-то - - // эта прагма - способ распараллелить цикл на все ядра процессора (см. OpenMP parallel for) - // reduction позволяет сказать OpenMP что нужно провести редукцию суммированием для каждой из переменных: error_sum, n_matched, n_in_bounds, ... - // мы ведь хотим найти сумму по всем потокам - #pragma omp parallel for reduction(+ : error_sum, n_matched, n_in_bounds, size_ratio_sum, angle_diff_sum, desc_dist_sum, desc_rand_dist_sum) - for (ptrdiff_t i = 0; i < kps0.size(); ++i) { - cv::Point2f p01 = ps01[i]; // взяли ожидаемую координату куда должна была перейти точка - if (p01.x > 0 && p01.x < width && p01.y > 0 && p01.y < height) { - n_in_bounds += 1; // засчитали точку как "не вышла за пределы картинки - имеет шансы на успешное сопоставление" - } else { - continue; - } - - ptrdiff_t closest_j = -1; // будем искать ближайшую точку детектированную на искаженном изображении - double min_error = std::numeric_limits::max(); - for (ptrdiff_t j = 0; j < kps1.size(); ++j) { - double error = cv::norm(kps1[j].pt - p01); - if (error < min_error) { - min_error = error; - closest_j = j; - } - } - if (closest_j != -1 && min_error <= MAX_ACCEPTED_PIXEL_ERROR * width) { - // мы нашли что-то достаточно близкое - успех! - #pragma omp critical - { - is_not_matched0[i] = false; - is_not_matched1[closest_j] = false; - }; - ++n_matched; - error_sum += min_error; - if (kps0[i].size != 0.0) { - size_ratio_sum += kps1[closest_j].size / kps0[i].size; - } - angle_diff_sum += diffAngles(kps0[i].angle, kps1[closest_j].angle); - - cv::Mat d0 = desc0.rowRange(cv::Range(i, i + 1)); - cv::Mat d1 = desc1.rowRange(cv::Range(closest_j, closest_j + 1)); - size_t random_j = (239017 * i + 1232142) % kps1.size(); - cv::Mat random_d1 = desc1.rowRange(cv::Range(random_j, random_j + 1)); - ; - if (method_name == "ORB") { - desc_rand_dist_sum += cv::norm(d0, random_d1, cv::NORM_HAMMING); - - desc_dist_sum += cv::norm(d0, d1, cv::NORM_HAMMING); - } else if (method_name == "SIFTOCV" || method_name == "SIFT_MY") { - desc_rand_dist_sum += cv::norm(d0, random_d1, cv::NORM_L2); - - desc_dist_sum += cv::norm(d0, d1, cv::NORM_L2); - - // Это способ заглянуть в черную коробку, так вы можете визуально посмотреть на то - // что за числа в дескрипторах двух сопоставленных точек, насколько они похожи, - // и сверить что расстояние между дескрипторами - это действительно расстояние - // между точками в пространстве высокой размерности: -#if 0 - if (i % 100 == 0) { - #pragma omp critical - { - std::cout << "d0: " << d0 << std::endl; - std::cout << "d1: " << d1 << std::endl; - std::cout << "d1-d0: " << d1-d0 << std::endl; - cv::Mat mul; - cv::multiply((d1-d0), (d1-d0), mul); - std::cout << "(d1-d0)^2: " << mul << std::endl; - std::cout << "sum((d1-d0)^2): " << cv::sum(mul) << std::endl; - std::cout << "sqrt(sum((d1-d0)^2)): " << sqrt(cv::sum(mul)[0]) << std::endl; - std::cout << "norm: " << cv::norm(d0, d1, cv::NORM_L2) << std::endl; - } - } -#endif - } - } - } - rassert(n_matched > 0, 2319241421512); // это не проверка как часть тестирования, это проверка что я не набагал и что дальше не будет деления на ноль :) - double recall = n_matched * 1.0 / n_in_bounds; - double avg_error = error_sum / n_matched; - std::cout << log_prefix << n_matched << "/" << n_in_bounds << " (recall=" << recall << ") with average error=" << avg_error << std::endl; - std::cout << log_prefix << "average size ratio between matched points: " << (size_ratio_sum / n_matched) << std::endl; - if (angle_diff_sum != 0.0) { - std::cout << log_prefix << "average angle difference between matched points: " << (angle_diff_sum / n_matched) << " degrees" << std::endl; - // TODO почему SIFT менее точно угадывает средний угол отклонения? изменяется ли ситуация если выкрутить параметр ORIENTATION_VOTES_PEAK_RATIO=0.999? почему? - } - if (desc_dist_sum != 0.0 && desc_rand_dist_sum != 0.0) { - std::cout << log_prefix << "average descriptor distance between matched points: " << (desc_dist_sum / n_matched) << " (random distance: " << (desc_rand_dist_sum / n_matched) - << ") => differentiability=" << (desc_dist_sum / desc_rand_dist_sum) << std::endl; - } - - // а вот это проверка качества, самая важная часть теста, проверяем насколько часто одни и те же характерные точки детектируются - // несмотря на несущественное искажение изображения - // т.е. мы по сути проверяем что "ключевые точки детектируются инвариантно к смещению, повороту и масштабу" - EXPECT_GT(recall, minRecall); - // и проверяем среднюю ошибку в пикселях - EXPECT_LT(avg_error, MAX_AVG_PIXEL_ERROR * width); - - cv::Mat result0 = img0.clone(); - cv::Mat result1 = img1.clone(); - // рисует отладочные картинки, это удобно делать по коду вообще везде, чтобы легко и удобно всегда было заглянуть в черную коробку чтобы попробовать понять - // где проблемы, или где можно что-то улучшить - drawKeyPoints(result0, kps0, is_not_matched0); - drawKeyPoints(result1, kps1, is_not_matched1); - - cv::Mat result = concatenateImagesLeftRight(result0, result1); - cv::putText(result, log_prefix + " recall=" + to_string(recall), cv::Point(10, 30), cv::FONT_HERSHEY_DUPLEX, 0.75, CV_RGB(255, 255, 0)); - cv::putText(result, "avgPixelsError=" + to_string(avg_error), cv::Point(10, 60), cv::FONT_HERSHEY_DUPLEX, 0.75, CV_RGB(255, 255, 0)); - - // отладочную визуализацию сохраняем в папку чтобы легко было посмотреть на любой промежуточный результат - // или в данном случае - на любой результат любого теста - cv::imwrite("data/debug/test_sift/" + getTestSuiteName() + "/" + getTestName() + "_" + method_name + ".png", result); - - if (SHOW_RESULTS) { - // показать результат сразу в диалоге удобно если вы запускаете один и тот же тест раз за разом - // и хотите сразу видеть результат чтобы его оценить, вместо того чтобы идти в папочку и кликать по файлу - cv::imshow("Red thick circles - not matched", result); - cv::waitKey(); - } - } - } -} - -// создаем матрицу описывающую преобразование пространства "сдвиг на вектор" -cv::Mat createTranslationMatrix(double dx, double dy) -{ - // [1, 0, dx] - // [0, 1, dy] - cv::Mat M = cv::Mat(2, 3, CV_64FC1, 0.0); - M.at(0, 0) = 1.0; - M.at(1, 1) = 1.0; - M.at(0, 2) = dx; - M.at(1, 2) = dy; - return M; -} - -TEST(SIFT, MovedTheSameImage) -{ - double minRecall = 0.75; - evaluateDetection(createTranslationMatrix(0.0, 0.0), minRecall); -} - -TEST(SIFT, MovedImageRight) -{ - double minRecall = 0.75; - evaluateDetection(createTranslationMatrix(50.0, 0.0), minRecall); -} - -TEST(SIFT, MovedImageLeft) -{ - double minRecall = 0.75; - evaluateDetection(createTranslationMatrix(-50.0, 0.0), minRecall); -} - -TEST(SIFT, MovedImageUpHalfPixel) -{ - double minRecall = 0.75; - evaluateDetection(createTranslationMatrix(0.0, -50.5), minRecall); -} - -TEST(SIFT, MovedImageDownHalfPixel) -{ - double minRecall = 0.75; - evaluateDetection(createTranslationMatrix(0.0, 50.5), minRecall); -} - -TEST(SIFT, Rotate10) -{ - double angleDegreesClockwise = 10; - double scale = 1.0; - double minRecall = 0.60; - evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); -} - -TEST(SIFT, Rotate20) -{ - double angleDegreesClockwise = 20; - double scale = 1.0; - double minRecall = 0.60; - evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); -} - -TEST(SIFT, Rotate30) -{ - double angleDegreesClockwise = 30; - double scale = 1.0; - double minRecall = 0.60; - evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); -} - -TEST(SIFT, Rotate40) -{ - double angleDegreesClockwise = 40; - double scale = 1.0; - double minRecall = 0.60; - evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); -} - -TEST(SIFT, Rotate45) -{ - double angleDegreesClockwise = 45; - double scale = 1.0; - double minRecall = 0.60; - evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); -} - -TEST(SIFT, Rotate90) -{ - double angleDegreesClockwise = 90; - double scale = 1.0; - double minRecall = 0.75; - evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); -} - -TEST(SIFT, Scale50) -{ - double angleDegreesClockwise = 0; - double scale = 0.5; - double minRecall = 0.40; - evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); -} - -TEST(SIFT, Scale70) -{ - double angleDegreesClockwise = 0; - double scale = 0.7; - double minRecall = 0.40; - evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); -} - -TEST(SIFT, Scale90) -{ - double angleDegreesClockwise = 0; - double scale = 0.9; - double minRecall = 0.60; - evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); -} - -TEST(SIFT, Scale110) -{ - double angleDegreesClockwise = 0; - double scale = 1.1; - double minRecall = 0.60; - evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); -} - -TEST(SIFT, Scale130) -{ - double angleDegreesClockwise = 0; - double scale = 1.3; - double minRecall = 0.50; - evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); -} - -TEST(SIFT, Scale150) -{ - double angleDegreesClockwise = 0; - double scale = 1.5; - double minRecall = 0.50; - evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); -} - -TEST(SIFT, Scale175) -{ - double angleDegreesClockwise = 0; - double scale = 1.75; - double minRecall = 0.40; - evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); -} - -TEST(SIFT, Scale200) -{ - double angleDegreesClockwise = 0; - double scale = 2.0; - double minRecall = 0.20; - evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); -} - -TEST(SIFT, Rotate10Scale90) -{ - double angleDegreesClockwise = 10; - double scale = 0.9; - double minRecall = 0.65; - evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); -} - -TEST(SIFT, Rotate30Scale75) -{ - double angleDegreesClockwise = 30; - double scale = 0.75; - double minRecall = 0.50; - evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); -} - -TEST(SIFT, HerzJesu19RotateM40) -{ - cv::Mat jesu19 = cv::imread("data/src/test_sift/herzjesu19.png"); - - ASSERT_FALSE(jesu19.empty()); // проверка что картинка была загружена - // убедитесь что рабочая папка (Edit Configurations...->Working directory) указывает на корневую папку проекта - - double angleDegreesClockwise = -40; - double scale = 1.0; - double minRecall = 0.75; - evaluateDetection(cv::getRotationMatrix2D(cv::Point(jesu19.cols / 2, jesu19.rows / 2), -angleDegreesClockwise, scale), minRecall, jesu19); -} - -TEST(SIFT, DetectionSmokeTest) -{ -#if ENABLE_MY_SIFT_TESTING - phg::SIFTParams p; - phg::SIFT sift(p, 2, "data/debug/test_sift/debug/"); - - cv::Mat img = cv::imread("data/src/test_sift/mysh1.jpg"); - cv::resize(img, img, img.size() / 4, 0, 0, cv::INTER_AREA); - - std::vector kpts; - cv::Mat desc; - sift.detectAndCompute(img, kpts, desc); -#else - std::cout << "ENABLE_MY_SIFT_TESTING is disabled, test skipped" << std::endl; -#endif -} - -namespace fs = std::filesystem; - -namespace { - -const std::string kDataDir = "data/src/test_sift/test_steps_data/"; -const double kRelEps = 0.05; - -// ── helpers ──────────────────────────────────────────────────────────────── - -// Compare two matrices: same size/type, all float values within relative eps. -// Returns empty string on success, or a description of the first mismatch. -std::string compareMats(const cv::Mat& a, const cv::Mat& b, const std::string& label, double relEps) -{ - if (a.size() != b.size()) { - std::ostringstream ss; - ss << label << ": size mismatch " << a.size() << " vs " << b.size(); - return ss.str(); - } - if (a.type() != b.type()) { - std::ostringstream ss; - ss << label << ": type mismatch " << a.type() << " vs " << b.type(); - return ss.str(); - } - if (a.empty() && b.empty()) - return { }; - - // Convert to float64 for comparison - cv::Mat af, bf; - a.reshape(1).convertTo(af, CV_64F); - b.reshape(1).convertTo(bf, CV_64F); - - for (int r = 0; r < af.rows; ++r) { - for (int c = 0; c < af.cols; ++c) { - double va = af.at(r, c); - double vb = bf.at(r, c); - double denom = std::max({ std::abs(va), std::abs(vb), 1e-3 }); - double rel = std::abs(va - vb) / denom; - if (rel > relEps) { - std::ostringstream ss; - ss << label << ": mismatch at (" << r << "," << c << "): " << va << " vs " << vb << " (rel=" << rel << ")"; - return ss.str(); - } - } - } - return { }; -} - -// ── Octave serialization ────────────────────────────────────────────────── - -void saveOctaves(const std::string& path, const std::vector& octaves) -{ - - if (DENY_CREATE_REF_DATA) - throw std::runtime_error("saving reference data is denied"); - - cv::FileStorage fs(path, cv::FileStorage::WRITE); - fs << "num_octaves" << (int)octaves.size(); - for (size_t i = 0; i < octaves.size(); ++i) { - std::string prefix = "octave_" + std::to_string(i); - fs << (prefix + "_num_layers") << (int)octaves[i].layers.size(); - for (size_t j = 0; j < octaves[i].layers.size(); ++j) { - fs << (prefix + "_layer_" + std::to_string(j)) << octaves[i].layers[j]; - } - } -} - -std::vector loadOctaves(const std::string& path) -{ - cv::FileStorage fs(path, cv::FileStorage::READ); - int numOctaves; - fs["num_octaves"] >> numOctaves; - std::vector octaves(numOctaves); - for (int i = 0; i < numOctaves; ++i) { - std::string prefix = "octave_" + std::to_string(i); - int numLayers; - fs[prefix + "_num_layers"] >> numLayers; - octaves[i].layers.resize(numLayers); - for (int j = 0; j < numLayers; ++j) { - fs[prefix + "_layer_" + std::to_string(j)] >> octaves[i].layers[j]; - } - } - return octaves; -} - -void compareOctaves(const std::vector& a, const std::vector& b, const std::string& stepName) -{ - ASSERT_EQ(a.size(), b.size()) << stepName << ": octave count mismatch"; - for (size_t i = 0; i < a.size(); ++i) { - ASSERT_EQ(a[i].layers.size(), b[i].layers.size()) << stepName << ": octave " << i << " layer count mismatch"; - for (size_t j = 0; j < a[i].layers.size(); ++j) { - std::string label = stepName + " oct" + std::to_string(i) + " layer" + std::to_string(j); - std::string err = compareMats(a[i].layers[j], b[i].layers[j], label, kRelEps); - EXPECT_TRUE(err.empty()) << err; - } - } -} - -// ── KeyPoint serialization ──────────────────────────────────────────────── - -void saveKeypoints(const std::string& path, const std::vector& kpts) -{ - if (DENY_CREATE_REF_DATA) - throw std::runtime_error("saving reference data is denied"); - cv::FileStorage fs(path, cv::FileStorage::WRITE); - fs << "keypoints" << kpts; -} - -std::vector loadKeypoints(const std::string& path) -{ - cv::FileStorage fs(path, cv::FileStorage::READ); - std::vector kpts; - fs["keypoints"] >> kpts; - return kpts; -} - -// Compare two descriptor rows (as double). Returns true if all elements are -// within relative epsilon. -bool descriptorRowsSimilar(const double* a, const double* b, int cols, double relEps) -{ - double diffSq = 0.0, normASq = 0.0, normBSq = 0.0; - for (int c = 0; c < cols; ++c) { - double d = a[c] - b[c]; - diffSq += d * d; - normASq += a[c] * a[c]; - normBSq += b[c] * b[c]; - } - double denom = std::max({ std::sqrt(normASq), std::sqrt(normBSq), 1e-3 }); - return std::sqrt(diffSq) / denom <= relEps; -} - -// Check if two keypoints are "similar" — all continuous fields within relative -// epsilon, and integer fields (octave, class_id) match exactly. -// If descriptor rows are provided (non-null), they must also be similar. -bool keypointsSimilar(const cv::KeyPoint& a, const cv::KeyPoint& b, double relEps, const double* descRowA = nullptr, const double* descRowB = nullptr, int descCols = 0) -{ - auto relClose = [relEps](double va, double vb) { - double denom = std::max({ std::abs(va), std::abs(vb), 1e-3 }); - return std::abs(va - vb) / denom <= relEps; - }; - // Angle wraps around 360, so handle the wraparound case - auto angleDist = [](double a, double b) { - double d = std::abs(a - b); - return std::min(d, 360.0 - d); - }; - if (a.octave != b.octave) - return false; - if (a.class_id != b.class_id) - return false; - if (!relClose(a.pt.x, b.pt.x)) - return false; - if (!relClose(a.pt.y, b.pt.y)) - return false; - if (!relClose(a.size, b.size)) - return false; - if (!relClose(a.response, b.response)) - return false; - // For angle: use absolute threshold (relEps * 360) to handle near-zero angles - if (angleDist(a.angle, b.angle) > relEps * 360.0) - return false; - // If descriptors are provided, they must also match - if (descRowA && descRowB && descCols > 0 && !descriptorRowsSimilar(descRowA, descRowB, descCols, 4 * relEps)) - return false; - return true; -} - -// For each detected keypoint, check that a similar keypoint exists in the -// reference set. When descriptors are provided, similarity includes the -// descriptor row. Test passes if: -// 1) total detected count is within 20% of reference count -// 2) at least 80% of detected keypoints have a similar reference keypoint -void compareKeypoints(const std::vector& ref, const std::vector& detected, const std::string& stepName, const cv::Mat& refDesc = cv::Mat(), const cv::Mat& detDesc = cv::Mat()) -{ - const double kCountTolerance = 0.20; // 20% - const double kMinMatchRate = 0.80; // 80% - - // Validate descriptor dimensions if provided - bool useDesc = !refDesc.empty() && !detDesc.empty(); - if (useDesc) { - ASSERT_EQ(refDesc.rows, (int)ref.size()) << stepName << ": ref descriptor row count != ref keypoint count"; - ASSERT_EQ(detDesc.rows, (int)detected.size()) << stepName << ": det descriptor row count != det keypoint count"; - ASSERT_EQ(refDesc.cols, detDesc.cols) << stepName << ": descriptor column count mismatch"; - } - - // Convert descriptors to CV_64F for uniform comparison - cv::Mat refDescF, detDescF; - int descCols = 0; - if (useDesc) { - refDesc.convertTo(refDescF, CV_64F); - detDesc.convertTo(detDescF, CV_64F); - descCols = refDescF.cols; - } - - // 1) Check that counts are within 10% - double refCount = (double)ref.size(); - double detCount = (double)detected.size(); - double countRatio = (refCount > 0) ? std::abs(detCount - refCount) / refCount : detCount; - EXPECT_LE(countRatio, kCountTolerance) << stepName << ": keypoint count out of tolerance — detected " << detected.size() << ", reference " << ref.size() << " (diff " << (countRatio * 100.0) << "%)"; - - int goodCount = 0; - for (size_t i = 0; i < detected.size(); ++i) { - const double* detRow = useDesc ? detDescF.ptr((int)i) : nullptr; - - bool found = false; - for (size_t j = 0; j < ref.size(); ++j) { - const double* refRow = useDesc ? refDescF.ptr((int)j) : nullptr; - if (keypointsSimilar(detected[i], ref[j], kRelEps, detRow, refRow, descCols)) { - found = true; - break; - } - } - if (found) - ++goodCount; - } - - double matchRate = (detected.empty()) ? 1.0 : (double)goodCount / detCount; - EXPECT_GE(matchRate, kMinMatchRate) << stepName << ": only " << goodCount << " / " << detected.size() << " detected keypoints (" << (matchRate * 100.0) << "%) matched a reference keypoint (need " << (kMinMatchRate * 100.0) << "%)" - << (useDesc ? " [with descriptors]" : ""); - - std::cout << "[ MATCH ] " << stepName << ": " << goodCount << "/" << detected.size() << " matched (" << (matchRate * 100.0) << "%), ref count=" << ref.size() << (useDesc ? " [with descriptors]" : "") << std::endl; -} - -// ── Generic "check or create" wrappers ──────────────────────────────────── - -void checkOrCreateOctaves(const std::string& filename, const std::vector& octaves, const std::string& stepName) -{ - std::string path = kDataDir + filename; - if (fs::exists(path)) { - auto ref = loadOctaves(path); - compareOctaves(ref, octaves, stepName); - std::cout << "[ CHECK ] " << stepName << ": matched reference " << filename << std::endl; - } else { - saveOctaves(path, octaves); - std::cout << "[ CREATE ] " << stepName << ": saved reference " << filename << std::endl; - } -} - -void saveDescriptors(const std::string& path, const cv::Mat& desc) -{ - - if (DENY_CREATE_REF_DATA) - throw std::runtime_error("saving reference data is denied"); - - cv::FileStorage fs(path, cv::FileStorage::WRITE); - fs << "descriptors" << desc; -} - -cv::Mat loadDescriptors(const std::string& path) -{ - cv::FileStorage fs(path, cv::FileStorage::READ); - cv::Mat desc; - fs["descriptors"] >> desc; - return desc; -} - -// Check or create keypoints, with optional descriptors. -// When descFilename is non-empty and desc is non-empty, descriptors are -// saved/loaded alongside keypoints and included in the similarity check. -void checkOrCreateKeypoints(const std::string& filename, const std::vector& kpts, const std::string& stepName, const std::string& descFilename = "", const cv::Mat& desc = cv::Mat()) -{ - std::string path = kDataDir + filename; - bool withDesc = !descFilename.empty() && !desc.empty(); - std::string descPath = withDesc ? (kDataDir + descFilename) : ""; - - if (fs::exists(path)) { - auto refKpts = loadKeypoints(path); - - cv::Mat refDesc; - if (withDesc && fs::exists(descPath)) { - refDesc = loadDescriptors(descPath); - } - - compareKeypoints(refKpts, kpts, stepName, refDesc, desc); - } else { - saveKeypoints(path, kpts); - if (withDesc) { - saveDescriptors(descPath, desc); - } - std::cout << "[ CREATE ] " << stepName << ": saved reference " << filename << (withDesc ? " + " + descFilename : "") << std::endl; - } -} - -} // namespace - -TEST(SIFT, DetectionDescriptionSteps) -{ -#if ENABLE_MY_SIFT_TESTING - ASSERT_TRUE(fs::exists(kDataDir)) << "Test data directory not found: " << kDataDir; - - phg::SIFTParams p; - p.upscale_first = false; - - cv::Mat img = cv::imread("data/src/test_sift/mysh1.jpg"); - ASSERT_FALSE(img.empty()) << "Failed to load test image mysh1.jpg"; - cv::resize(img, img, img.size() / 8, 0, 0, cv::INTER_AREA); - - std::cout << "loaded image of size: " << img.size() << std::endl; - - cv::Mat gray = phg::toGray32F(img); - - // Step 1: Build octaves (Gaussian scale-space) - std::vector octaves = buildOctaves(gray, p); - checkOrCreateOctaves("step1_octaves.yml.gz", octaves, "buildOctaves"); - if (::testing::Test::HasFatalFailure()) - return; - - // Step 2: Build Difference-of-Gaussians - std::vector dog = buildDoG(octaves, p); - checkOrCreateOctaves("step2_dog.yml.gz", dog, "buildDoG"); - if (::testing::Test::HasFatalFailure()) - return; - - // Step 3: Find scale-space extrema - std::vector kpts = findScaleSpaceExtrema(dog, p); - checkOrCreateKeypoints("step3_extrema.yml.gz", kpts, "findScaleSpaceExtrema"); - if (::testing::Test::HasFatalFailure()) - return; - std::cout << "detected n keypoints: " << kpts.size() << std::endl; - - // Step 4: Select top keypoints (first pass) - kpts = selectTopKeypoints(kpts, p); - checkOrCreateKeypoints("step4_top_kpts1.yml.gz", kpts, "selectTopKeypoints_1"); - if (::testing::Test::HasFatalFailure()) - return; - - std::cout << "selected n keypoints: " << kpts.size() << std::endl; - - // Step 5: Compute orientations - kpts = computeOrientations(kpts, octaves, p); - checkOrCreateKeypoints("step5_orientations.yml.gz", kpts, "computeOrientations"); - if (::testing::Test::HasFatalFailure()) - return; - - std::cout << "oriented n keypoints: " << kpts.size() << std::endl; - - // Step 6: Select top keypoints (second pass) - kpts = selectTopKeypoints(kpts, p); - checkOrCreateKeypoints("step6_top_kpts2.yml.gz", kpts, "selectTopKeypoints_2"); - if (::testing::Test::HasFatalFailure()) - return; - - std::cout << "selected n keypoints: " << kpts.size() << std::endl; - - // Step 7: Compute descriptors - cv::Mat desc; - std::tie(desc, kpts) = computeDescriptors(kpts, octaves, p); - checkOrCreateKeypoints("step7_desc_kpts.yml.gz", kpts, "computeDescriptors", "step7_descriptors.yml.gz", desc); - if (::testing::Test::HasFatalFailure()) - return; - - std::cout << "described n keypoints: " << kpts.size() << std::endl; -#else - std::cout << "ENABLE_MY_SIFT_TESTING is disabled, test skipped" << std::endl; -#endif -} - -TEST(SIFT, PairMatching) -{ -#if ENABLE_MY_SIFT_TESTING - cv::Mat img1 = cv::imread("data/src/test_sift/mysh2.jpg"); - ASSERT_FALSE(img1.empty()); - - cv::Mat img2 = cv::imread("data/src/test_sift/mysh3.jpg"); - ASSERT_FALSE(img2.empty()); - - cv::resize(img1, img1, img1.size() / 2, 0, 0, cv::INTER_AREA); - cv::resize(img2, img2, img2.size() / 2, 0, 0, cv::INTER_AREA); - std::cout << "image sizes: " << img1.size() << ", " << img2.size() << std::endl; - - phg::SIFTParams params; - params.nfeatures = 10000; - - std::cout << "matching using opencv orb..." << std::endl; - auto orb_cv = cv::ORB::create(params.nfeatures); - evaluateMatching(*orb_cv, img1, img2, "data/debug/test_sift/SIFT/Matches_ORB.jpg"); - - std::cout << "matching using opencv sift..." << std::endl; - auto sift_cv = cv::SIFT::create(params.nfeatures, params.n_octave_layers, params.contrast_threshold, params.edge_threshold); - MatchingPairData data_cv = evaluateMatching(*sift_cv, img1, img2, "data/debug/test_sift/SIFT/Matches_SIFTOCV.jpg"); - - std::cout << "matching using my sift..." << std::endl; - phg::SIFT sift(params); - MatchingPairData data = evaluateMatching(sift, img1, img2, "data/debug/test_sift/SIFT/Matches_SIFT_MY.jpg"); - - double thresh = 0.8; // expect at least 80% of opencv sift points & matches - EXPECT_GE(data.npoints1, thresh * data_cv.npoints1); - EXPECT_GE(data.npoints2, thresh * data_cv.npoints2); - EXPECT_GE(data.nmatches, thresh * data_cv.nmatches); - - std::cout << "Final score: " << data.nmatches << std::endl; -#else - std::cout << "ENABLE_MY_SIFT_TESTING is disabled, test skipped" << std::endl; - std::cout << "Final score: UNKNOWN" << std::endl; -#endif -} +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "utils/test_utils.h" + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +#define SHOW_RESULTS 0 // если вам хочется сразу видеть результат в окошке - переключите в 1, но не забудьте выключить перед коммитом (иначе бот в CI будет ждать веками) +#define MAX_ACCEPTED_PIXEL_ERROR \ + 0.01 // максимальное расстояние в пикселях (процент от ширины картинки) между ключевыми точками чтобы их можно было зачесть как "почти совпавшие" (это очень завышенный порог, по-хорошему должно быть 0.005 например) +#define MAX_AVG_PIXEL_ERROR 0.075 + +#define GAUSSIAN_NOISE_STDDEV 1.0 + + +#define ENABLE_MY_SIFT_TESTING 1 + +#define DENY_CREATE_REF_DATA 1 + + +// [ORB_OCV] average angle difference between matched points: 25.313 degrees +// [SIFTOCV] average angle difference between matched points: 9.58379 degrees with SIFT_ORI_PEAK_RATIO 0.8 +// [SIFTOCV] average angle difference between matched points: 24.1679 degrees with SIFT_ORI_PEAK_RATIO 0.999 +struct MatchingPairData { + size_t npoints1, npoints2, nmatches; +}; + +template MatchingPairData evaluateMatching(SIFT& sift, const cv::Mat& img1, const cv::Mat& img2, const std::string& output_matches_img_path) +{ + std::vector kpts1; + cv::Mat desc1; + sift.detectAndCompute(img1, { }, kpts1, desc1); + + std::vector kpts2; + cv::Mat desc2; + sift.detectAndCompute(img2, { }, kpts2, desc2); + + // Brute-force matching with ratio test + cv::BFMatcher matcher(cv::NORM_L2); + std::vector> knnMatches; + matcher.knnMatch(desc1, desc2, knnMatches, 2); + + const float ratioThresh = 0.75f; + std::vector goodMatches; + for (const auto& m : knnMatches) { + if (m.size() == 2 && m[0].distance < ratioThresh * m[1].distance) { + goodMatches.push_back(m[0]); + } + } + + // RANSAC filtering with fundamental matrix + std::vector inlierMatches; + if (goodMatches.size() >= 15) { + std::vector pts1, pts2; + for (const auto& m : goodMatches) { + pts1.push_back(kpts1[m.queryIdx].pt); + pts2.push_back(kpts2[m.trainIdx].pt); + } + + std::vector inlierMask; + cv::findFundamentalMat(pts1, pts2, cv::FM_RANSAC, 3.0, 0.99, inlierMask); + + for (size_t i = 0; i < goodMatches.size(); i++) { + if (inlierMask[i]) { + inlierMatches.push_back(goodMatches[i]); + } + } + } + + std::cout << "N keypoints: left " << kpts1.size() << ", right " << kpts2.size() << std::endl; + std::cout << "Good matches: " << goodMatches.size() << std::endl; + std::cout << "Inlier matches: " << inlierMatches.size() << std::endl; + + if (!output_matches_img_path.empty()) { + cv::Mat imgMatches; + cv::drawMatches(img1, kpts1, img2, kpts2, inlierMatches, imgMatches); + cv::imwrite(output_matches_img_path, imgMatches); + } + + return { kpts1.size(), kpts2.size(), inlierMatches.size() }; +} + +// функция рисует кружки случайного цвета вокруг точек, но если для точки не нашлось сопоставления - кружок будет толстый и ярко красный +void drawKeyPoints(cv::Mat& img, const std::vector& kps, const std::vector& is_not_matched) +{ + cv::RNG r(124124); + for (size_t i = 0; i < kps.size(); ++i) { + int thickness = 1; + cv::Scalar color; + if (is_not_matched[i]) { + color = CV_RGB(255, 0, 0); // OpenCV использует BGR схему вместо RGB, но можно использовать этот макрос вместо BGR - cv::Scalar(blue=0, green=0, red=255) + thickness = 2; + } else { + color = cv::Scalar(r.uniform(0, 255), r.uniform(0, 255), 0); + } + int radius = std::max(2, (int)(kps[i].size / 5.0f)); + float angle = kps[i].angle; + cv::circle(img, kps[i].pt, radius, color, thickness); + if (angle != -1.0) { + cv::line(img, kps[i].pt, cv::Point((int)std::round(kps[i].pt.x + radius * sin(angle * M_PI / 180.0)), (int)std::round(kps[i].pt.y + radius * cos(angle * M_PI / 180.0))), color); + } + } +} + +// Функция ищет знаковый угол между двумя направлениями (по кратчайшему пути, т.е. результат от -180 до 180) +double diffAngles(double angle0, double angle1) +{ + if (angle0 != -1.0 && angle1 != -1.0) { + rassert(angle0 >= 0.0 && angle0 < 360.0, 1235612352151); + rassert(angle1 >= 0.0 && angle1 < 360.0, 4645315415); + float diff; + if ((angle1 <= angle0 + 180 && angle0 + 180 <= 360) || (angle1 >= angle0 - 180 && angle0 - 180 >= 0)) { + diff = angle1 - angle0; + } else if (angle1 > angle0 + 180 && angle0 + 180 <= 360) { + diff = -(angle0 + (360 - angle1)); + } else if (angle1 <= angle0 - 180 && angle0 - 180 >= 0) { + diff = (360 - angle0) + angle1; + } else { + rassert(false, 1234124125125135); + } + rassert(diff >= -180 && diff <= 180, 233536136131); + return diff; + } else { + return 0.0; + } +} + +// На вход передается матрица описывающая преобразование картинки (сдвиг, поворот, масштабирование или их комбинация), допустимый процент Recall, и опционально можно тестировать другую картинку +void evaluateDetection(const cv::Mat& M, double minRecall, cv::Mat img0 = cv::Mat()) +{ + if (img0.empty()) { + img0 = cv::imread("data/src/test_sift/unicorn.png"); // грузим картинку по умолчанию + } + + ASSERT_FALSE(img0.empty()); // проверка что картинка была загружена + // убедитесь что рабочая папка (Edit Configurations...->Working directory) указывает на корневую папку проекта (и тогда картинка по умолчанию найдется по относительному пути - data/src/test_sift/unicorn.png) + + size_t width = img0.cols; + size_t height = img0.rows; + cv::Mat transformedImage; + cv::warpAffine(img0, transformedImage, M, cv::Size(width, height)); // строим img1 - преобразованная исходная картинка в соответствии + // с закодированным в матрицу M искажением пространства + cv::Mat noise(cv::Size(width, height), CV_8UC3); + cv::setRNGSeed(125125); // фиксируем рандом для детерминизма (чтобы результат воспроизводился из раза в раз) + cv::randn(noise, cv::Scalar::all(0), cv::Scalar::all(GAUSSIAN_NOISE_STDDEV)); + cv::add(transformedImage, noise, transformedImage); // добавляем к преобразованной картинке гауссиан шума + cv::Mat img1 = transformedImage; + + { + for (int method = 0; method < 3; ++method) { // тестируем три метода: OpenCV ORB, OpenCV SIFT, ваш SIFT + std::vector kps0; + std::vector kps1; + // size ~ diameter of the meaningful neighborhood around that point + + cv::Mat desc0; + cv::Mat desc1; + + timer t; // очень удобно встраивать профилирование вашего кода по мере его написания, тогда полную картину видеть гораздо проще (особенно это помогает со старым кодом) + std::string method_name; + std::string log_prefix; + + phg::SIFTParams p; + p.nfeatures = 500; + if (method == 0) { + method_name = "ORB"; + log_prefix = "[ORB_OCV] "; + // ORB - один из видов ключевых дескрипторов, отличается высокой скоростью и относительно неплохим качеством + cv::Ptr detector = cv::ORB::create(p.nfeatures); // здесь можно было бы поиграть с его параметрами, например выделять больше чем 500 точек, строить большее число ступеней пирамиды и т.п. + detector->detect(img0, kps0); // детектируем ключевые точки на исходной картинке + detector->detect(img1, kps1); // детектируем ключевые точки на преобразованной картинке + + detector->compute(img0, kps0, desc0); // compute descriptors for kpts + detector->compute(img1, kps1, desc1); + } else if (method == 1) { + method_name = "SIFTOCV"; + log_prefix = "[SIFTOCV] "; + cv::Ptr detector = cv::SIFT::create( + p.nfeatures, p.n_octave_layers, p.contrast_threshold, p.edge_threshold); // здесь можно было бы поиграть с его параметрами, например выделять больше чем 500 точек, строить большее число ступеней пирамиды и т.п. + detector->detect(img0, kps0); // детектируем ключевые точки на исходной картинке + detector->detect(img1, kps1); // детектируем ключевые точки на преобразованной картинке + + detector->compute(img0, kps0, desc0); + detector->compute(img1, kps1, desc1); + } else if (method == 2) { +#if ENABLE_MY_SIFT_TESTING + method_name = "SIFT_MY"; + log_prefix = "[SIFT_MY] "; + phg::SIFT mySIFT(p); + mySIFT.detectAndCompute(img0, kps0, desc0); + mySIFT.detectAndCompute(img1, kps1, desc1); +#else + return; +#endif + } else { + rassert(false, 13532513412); // это не проверка как часть тестирования, это проверка что число итераций в цикле и if-else ветки все еще согласованы и не разошлись + } + + std::cout << log_prefix << "Points detected: " << kps0.size() << " -> " << kps1.size() << " (in " << t.elapsed() << " sec)" << std::endl; + + std::vector ps01(kps0.size()); // давайте построим эталон - найдем куда бы должны были сместиться ключевые точки с исходного изображения с учетом нашей матрицы трансформации M + { + std::vector ps0(kps0.size()); // здесь мы сейчас расположим детектированные ключевые точки (каждую нужно преобразовать из типа КлючеваяТочка в Точка2Дэ) + for (size_t i = 0; i < kps0.size(); ++i) { + ps0[i] = kps0[i].pt; + } + cv::transform(ps0, ps01, M); // преобразовываем все точки с исходного изображения в систему координат его искаженной версии с учетом матрицы M, эти точки - эталон + } + + double error_sum = 0.0; // считаем суммарную ошибку координат сопоставлений точек чтобы найти среднюю ошибку (в пикселях) + double size_ratio_sum = 0.0; // хотим найти среднее соотношение размера сопоставленных ключевых точек (чтобы сверить эту пропорцию с тестируемым перепадом масштаба) + double angle_diff_sum = 0.0; // хотим найти среднее отличие угла наклона сопоставленных ключевых точек (чтобы сверить этот угол с тестируемым в тестах поворотом) + double desc_dist_sum = 0.0; // хотим найти среднее расстояние между дескрипторами сопоставленных ключевых точек + double desc_rand_dist_sum = 0.0; // найдем среднее расстояние между случайными парами ключевых точек (чтобы было с чем сравнить расстояние сопоставленных точек) + size_t n_matched = 0; // число успешно сопоставившихся исходных точек + size_t n_in_bounds = 0; // число исходных точек которые после преобразования координат не вышли за пределы картинки (т.е. в целом имели шансы на успешное сопоставление) + std::vector is_not_matched0(kps0.size(), true); // для каждой исходной точки хотим понять сопоставилась ли она + std::vector is_not_matched1(kps1.size(), true); // для каждой точки с результирующей картинки хотим понять сопоставился ли с ней хоть кто-то + + // эта прагма - способ распараллелить цикл на все ядра процессора (см. OpenMP parallel for) + // reduction позволяет сказать OpenMP что нужно провести редукцию суммированием для каждой из переменных: error_sum, n_matched, n_in_bounds, ... + // мы ведь хотим найти сумму по всем потокам + #pragma omp parallel for reduction(+ : error_sum, n_matched, n_in_bounds, size_ratio_sum, angle_diff_sum, desc_dist_sum, desc_rand_dist_sum) + for (ptrdiff_t i = 0; i < kps0.size(); ++i) { + cv::Point2f p01 = ps01[i]; // взяли ожидаемую координату куда должна была перейти точка + if (p01.x > 0 && p01.x < width && p01.y > 0 && p01.y < height) { + n_in_bounds += 1; // засчитали точку как "не вышла за пределы картинки - имеет шансы на успешное сопоставление" + } else { + continue; + } + + ptrdiff_t closest_j = -1; // будем искать ближайшую точку детектированную на искаженном изображении + double min_error = std::numeric_limits::max(); + for (ptrdiff_t j = 0; j < kps1.size(); ++j) { + double error = cv::norm(kps1[j].pt - p01); + if (error < min_error) { + min_error = error; + closest_j = j; + } + } + if (closest_j != -1 && min_error <= MAX_ACCEPTED_PIXEL_ERROR * width) { + // мы нашли что-то достаточно близкое - успех! + #pragma omp critical + { + is_not_matched0[i] = false; + is_not_matched1[closest_j] = false; + }; + ++n_matched; + error_sum += min_error; + if (kps0[i].size != 0.0) { + size_ratio_sum += kps1[closest_j].size / kps0[i].size; + } + angle_diff_sum += diffAngles(kps0[i].angle, kps1[closest_j].angle); + + cv::Mat d0 = desc0.rowRange(cv::Range(i, i + 1)); + cv::Mat d1 = desc1.rowRange(cv::Range(closest_j, closest_j + 1)); + size_t random_j = (239017 * i + 1232142) % kps1.size(); + cv::Mat random_d1 = desc1.rowRange(cv::Range(random_j, random_j + 1)); + ; + if (method_name == "ORB") { + desc_rand_dist_sum += cv::norm(d0, random_d1, cv::NORM_HAMMING); + + desc_dist_sum += cv::norm(d0, d1, cv::NORM_HAMMING); + } else if (method_name == "SIFTOCV" || method_name == "SIFT_MY") { + desc_rand_dist_sum += cv::norm(d0, random_d1, cv::NORM_L2); + + desc_dist_sum += cv::norm(d0, d1, cv::NORM_L2); + + // Это способ заглянуть в черную коробку, так вы можете визуально посмотреть на то + // что за числа в дескрипторах двух сопоставленных точек, насколько они похожи, + // и сверить что расстояние между дескрипторами - это действительно расстояние + // между точками в пространстве высокой размерности: +#if 0 + if (i % 100 == 0) { + #pragma omp critical + { + std::cout << "d0: " << d0 << std::endl; + std::cout << "d1: " << d1 << std::endl; + std::cout << "d1-d0: " << d1-d0 << std::endl; + cv::Mat mul; + cv::multiply((d1-d0), (d1-d0), mul); + std::cout << "(d1-d0)^2: " << mul << std::endl; + std::cout << "sum((d1-d0)^2): " << cv::sum(mul) << std::endl; + std::cout << "sqrt(sum((d1-d0)^2)): " << sqrt(cv::sum(mul)[0]) << std::endl; + std::cout << "norm: " << cv::norm(d0, d1, cv::NORM_L2) << std::endl; + } + } +#endif + } + } + } + rassert(n_matched > 0, 2319241421512); // это не проверка как часть тестирования, это проверка что я не набагал и что дальше не будет деления на ноль :) + double recall = n_matched * 1.0 / n_in_bounds; + double avg_error = error_sum / n_matched; + std::cout << log_prefix << n_matched << "/" << n_in_bounds << " (recall=" << recall << ") with average error=" << avg_error << std::endl; + std::cout << log_prefix << "average size ratio between matched points: " << (size_ratio_sum / n_matched) << std::endl; + if (angle_diff_sum != 0.0) { + std::cout << log_prefix << "average angle difference between matched points: " << (angle_diff_sum / n_matched) << " degrees" << std::endl; + // TODO почему SIFT менее точно угадывает средний угол отклонения? + // изменяется ли ситуация если выкрутить параметр ORIENTATION_VOTES_PEAK_RATIO=0.999? почему? + } + if (desc_dist_sum != 0.0 && desc_rand_dist_sum != 0.0) { + std::cout << log_prefix << "average descriptor distance between matched points: " << (desc_dist_sum / n_matched) << " (random distance: " << (desc_rand_dist_sum / n_matched) + << ") => differentiability=" << (desc_dist_sum / desc_rand_dist_sum) << std::endl; + } + + // а вот это проверка качества, самая важная часть теста, проверяем насколько часто одни и те же характерные точки детектируются + // несмотря на несущественное искажение изображения + // т.е. мы по сути проверяем что "ключевые точки детектируются инвариантно к смещению, повороту и масштабу" + EXPECT_GT(recall, minRecall); + // и проверяем среднюю ошибку в пикселях + EXPECT_LT(avg_error, MAX_AVG_PIXEL_ERROR * width); + + cv::Mat result0 = img0.clone(); + cv::Mat result1 = img1.clone(); + // рисует отладочные картинки, это удобно делать по коду вообще везде, чтобы легко и удобно всегда было заглянуть в черную коробку чтобы попробовать понять + // где проблемы, или где можно что-то улучшить + drawKeyPoints(result0, kps0, is_not_matched0); + drawKeyPoints(result1, kps1, is_not_matched1); + + cv::Mat result = concatenateImagesLeftRight(result0, result1); + cv::putText(result, log_prefix + " recall=" + to_string(recall), cv::Point(10, 30), cv::FONT_HERSHEY_DUPLEX, 0.75, CV_RGB(255, 255, 0)); + cv::putText(result, "avgPixelsError=" + to_string(avg_error), cv::Point(10, 60), cv::FONT_HERSHEY_DUPLEX, 0.75, CV_RGB(255, 255, 0)); + + // отладочную визуализацию сохраняем в папку чтобы легко было посмотреть на любой промежуточный результат + // или в данном случае - на любой результат любого теста + cv::imwrite("data/debug/test_sift/" + getTestSuiteName() + "/" + getTestName() + "_" + method_name + ".png", result); + + if (SHOW_RESULTS) { + // показать результат сразу в диалоге удобно если вы запускаете один и тот же тест раз за разом + // и хотите сразу видеть результат чтобы его оценить, вместо того чтобы идти в папочку и кликать по файлу + cv::imshow("Red thick circles - not matched", result); + cv::waitKey(); + } + } + } +} + +// создаем матрицу описывающую преобразование пространства "сдвиг на вектор" +cv::Mat createTranslationMatrix(double dx, double dy) +{ + // [1, 0, dx] + // [0, 1, dy] + cv::Mat M = cv::Mat(2, 3, CV_64FC1, 0.0); + M.at(0, 0) = 1.0; + M.at(1, 1) = 1.0; + M.at(0, 2) = dx; + M.at(1, 2) = dy; + return M; +} + +TEST(SIFT, MovedTheSameImage) +{ + double minRecall = 0.75; + evaluateDetection(createTranslationMatrix(0.0, 0.0), minRecall); +} + +TEST(SIFT, MovedImageRight) +{ + double minRecall = 0.75; + evaluateDetection(createTranslationMatrix(50.0, 0.0), minRecall); +} + +TEST(SIFT, MovedImageLeft) +{ + double minRecall = 0.75; + evaluateDetection(createTranslationMatrix(-50.0, 0.0), minRecall); +} + +TEST(SIFT, MovedImageUpHalfPixel) +{ + double minRecall = 0.75; + evaluateDetection(createTranslationMatrix(0.0, -50.5), minRecall); +} + +TEST(SIFT, MovedImageDownHalfPixel) +{ + double minRecall = 0.75; + evaluateDetection(createTranslationMatrix(0.0, 50.5), minRecall); +} + +TEST(SIFT, Rotate10) +{ + double angleDegreesClockwise = 10; + double scale = 1.0; + double minRecall = 0.60; + evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); +} + +TEST(SIFT, Rotate20) +{ + double angleDegreesClockwise = 20; + double scale = 1.0; + double minRecall = 0.60; + evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); +} + +TEST(SIFT, Rotate30) +{ + double angleDegreesClockwise = 30; + double scale = 1.0; + double minRecall = 0.60; + evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); +} + +TEST(SIFT, Rotate40) +{ + double angleDegreesClockwise = 40; + double scale = 1.0; + double minRecall = 0.60; + evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); +} + +TEST(SIFT, Rotate45) +{ + double angleDegreesClockwise = 45; + double scale = 1.0; + double minRecall = 0.60; + evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); +} + +TEST(SIFT, Rotate90) +{ + double angleDegreesClockwise = 90; + double scale = 1.0; + double minRecall = 0.75; + evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); +} + +TEST(SIFT, Scale50) +{ + double angleDegreesClockwise = 0; + double scale = 0.5; + double minRecall = 0.40; + evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); +} + +TEST(SIFT, Scale70) +{ + double angleDegreesClockwise = 0; + double scale = 0.7; + double minRecall = 0.40; + evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); +} + +TEST(SIFT, Scale90) +{ + double angleDegreesClockwise = 0; + double scale = 0.9; + double minRecall = 0.60; + evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); +} + +TEST(SIFT, Scale110) +{ + double angleDegreesClockwise = 0; + double scale = 1.1; + double minRecall = 0.60; + evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); +} + +TEST(SIFT, Scale130) +{ + double angleDegreesClockwise = 0; + double scale = 1.3; + double minRecall = 0.50; + evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); +} + +TEST(SIFT, Scale150) +{ + double angleDegreesClockwise = 0; + double scale = 1.5; + double minRecall = 0.50; + evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); +} + +TEST(SIFT, Scale175) +{ + double angleDegreesClockwise = 0; + double scale = 1.75; + double minRecall = 0.40; + evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); +} + +TEST(SIFT, Scale200) +{ + double angleDegreesClockwise = 0; + double scale = 2.0; + double minRecall = 0.20; + evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); +} + +TEST(SIFT, Rotate10Scale90) +{ + double angleDegreesClockwise = 10; + double scale = 0.9; + double minRecall = 0.65; + evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); +} + +TEST(SIFT, Rotate30Scale75) +{ + double angleDegreesClockwise = 30; + double scale = 0.75; + double minRecall = 0.50; + evaluateDetection(cv::getRotationMatrix2D(cv::Point(200, 256), -angleDegreesClockwise, scale), minRecall); +} + +TEST(SIFT, HerzJesu19RotateM40) +{ + cv::Mat jesu19 = cv::imread("data/src/test_sift/herzjesu19.png"); + + ASSERT_FALSE(jesu19.empty()); // проверка что картинка была загружена + // убедитесь что рабочая папка (Edit Configurations...->Working directory) указывает на корневую папку проекта + + double angleDegreesClockwise = -40; + double scale = 1.0; + double minRecall = 0.75; + evaluateDetection(cv::getRotationMatrix2D(cv::Point(jesu19.cols / 2, jesu19.rows / 2), -angleDegreesClockwise, scale), minRecall, jesu19); +} + +TEST(SIFT, DetectionSmokeTest) +{ +#if ENABLE_MY_SIFT_TESTING + phg::SIFTParams p; + phg::SIFT sift(p, 2, "data/debug/test_sift/debug/"); + + cv::Mat img = cv::imread("data/src/test_sift/mysh1.jpg"); + cv::resize(img, img, img.size() / 4, 0, 0, cv::INTER_AREA); + + std::vector kpts; + cv::Mat desc; + sift.detectAndCompute(img, kpts, desc); +#else + std::cout << "ENABLE_MY_SIFT_TESTING is disabled, test skipped" << std::endl; +#endif +} + +namespace fs = std::filesystem; + +namespace { + +const std::string kDataDir = "data/src/test_sift/test_steps_data/"; +const double kRelEps = 0.05; + +// ── helpers ──────────────────────────────────────────────────────────────── + +// Compare two matrices: same size/type, all float values within relative eps. +// Returns empty string on success, or a description of the first mismatch. +std::string compareMats(const cv::Mat& a, const cv::Mat& b, const std::string& label, double relEps) +{ + if (a.size() != b.size()) { + std::ostringstream ss; + ss << label << ": size mismatch " << a.size() << " vs " << b.size(); + return ss.str(); + } + if (a.type() != b.type()) { + std::ostringstream ss; + ss << label << ": type mismatch " << a.type() << " vs " << b.type(); + return ss.str(); + } + if (a.empty() && b.empty()) + return { }; + + // Convert to float64 for comparison + cv::Mat af, bf; + a.reshape(1).convertTo(af, CV_64F); + b.reshape(1).convertTo(bf, CV_64F); + + for (int r = 0; r < af.rows; ++r) { + for (int c = 0; c < af.cols; ++c) { + double va = af.at(r, c); + double vb = bf.at(r, c); + double denom = std::max({ std::abs(va), std::abs(vb), 1e-3 }); + double rel = std::abs(va - vb) / denom; + if (rel > relEps) { + std::ostringstream ss; + ss << label << ": mismatch at (" << r << "," << c << "): " << va << " vs " << vb << " (rel=" << rel << ")"; + return ss.str(); + } + } + } + return { }; +} + +// ── Octave serialization ────────────────────────────────────────────────── + +void saveOctaves(const std::string& path, const std::vector& octaves) +{ + + if (DENY_CREATE_REF_DATA) + throw std::runtime_error("saving reference data is denied"); + + cv::FileStorage fs(path, cv::FileStorage::WRITE); + fs << "num_octaves" << (int)octaves.size(); + for (size_t i = 0; i < octaves.size(); ++i) { + std::string prefix = "octave_" + std::to_string(i); + fs << (prefix + "_num_layers") << (int)octaves[i].layers.size(); + for (size_t j = 0; j < octaves[i].layers.size(); ++j) { + fs << (prefix + "_layer_" + std::to_string(j)) << octaves[i].layers[j]; + } + } +} + +std::vector loadOctaves(const std::string& path) +{ + cv::FileStorage fs(path, cv::FileStorage::READ); + int numOctaves; + fs["num_octaves"] >> numOctaves; + std::vector octaves(numOctaves); + for (int i = 0; i < numOctaves; ++i) { + std::string prefix = "octave_" + std::to_string(i); + int numLayers; + fs[prefix + "_num_layers"] >> numLayers; + octaves[i].layers.resize(numLayers); + for (int j = 0; j < numLayers; ++j) { + fs[prefix + "_layer_" + std::to_string(j)] >> octaves[i].layers[j]; + } + } + return octaves; +} + +void compareOctaves(const std::vector& a, const std::vector& b, const std::string& stepName) +{ + ASSERT_EQ(a.size(), b.size()) << stepName << ": octave count mismatch"; + for (size_t i = 0; i < a.size(); ++i) { + ASSERT_EQ(a[i].layers.size(), b[i].layers.size()) << stepName << ": octave " << i << " layer count mismatch"; + for (size_t j = 0; j < a[i].layers.size(); ++j) { + std::string label = stepName + " oct" + std::to_string(i) + " layer" + std::to_string(j); + std::string err = compareMats(a[i].layers[j], b[i].layers[j], label, kRelEps); + EXPECT_TRUE(err.empty()) << err; + } + } +} + +// ── KeyPoint serialization ──────────────────────────────────────────────── + +void saveKeypoints(const std::string& path, const std::vector& kpts) +{ + if (DENY_CREATE_REF_DATA) + throw std::runtime_error("saving reference data is denied"); + cv::FileStorage fs(path, cv::FileStorage::WRITE); + fs << "keypoints" << kpts; +} + +std::vector loadKeypoints(const std::string& path) +{ + cv::FileStorage fs(path, cv::FileStorage::READ); + std::vector kpts; + fs["keypoints"] >> kpts; + return kpts; +} + +// Compare two descriptor rows (as double). Returns true if all elements are +// within relative epsilon. +bool descriptorRowsSimilar(const double* a, const double* b, int cols, double relEps) +{ + double diffSq = 0.0, normASq = 0.0, normBSq = 0.0; + for (int c = 0; c < cols; ++c) { + double d = a[c] - b[c]; + diffSq += d * d; + normASq += a[c] * a[c]; + normBSq += b[c] * b[c]; + } + double denom = std::max({ std::sqrt(normASq), std::sqrt(normBSq), 1e-3 }); + return std::sqrt(diffSq) / denom <= relEps; +} + +// Check if two keypoints are "similar" — all continuous fields within relative +// epsilon, and integer fields (octave, class_id) match exactly. +// If descriptor rows are provided (non-null), they must also be similar. +bool keypointsSimilar(const cv::KeyPoint& a, const cv::KeyPoint& b, double relEps, const double* descRowA = nullptr, const double* descRowB = nullptr, int descCols = 0) +{ + auto relClose = [relEps](double va, double vb) { + double denom = std::max({ std::abs(va), std::abs(vb), 1e-3 }); + return std::abs(va - vb) / denom <= relEps; + }; + // Angle wraps around 360, so handle the wraparound case + auto angleDist = [](double a, double b) { + double d = std::abs(a - b); + return std::min(d, 360.0 - d); + }; + if (a.octave != b.octave) + return false; + if (a.class_id != b.class_id) + return false; + if (!relClose(a.pt.x, b.pt.x)) + return false; + if (!relClose(a.pt.y, b.pt.y)) + return false; + if (!relClose(a.size, b.size)) + return false; + if (!relClose(a.response, b.response)) + return false; + // For angle: use absolute threshold (relEps * 360) to handle near-zero angles + if (angleDist(a.angle, b.angle) > relEps * 360.0) + return false; + // If descriptors are provided, they must also match + if (descRowA && descRowB && descCols > 0 && !descriptorRowsSimilar(descRowA, descRowB, descCols, 4 * relEps)) + return false; + return true; +} + +// For each detected keypoint, check that a similar keypoint exists in the +// reference set. When descriptors are provided, similarity includes the +// descriptor row. Test passes if: +// 1) total detected count is within 20% of reference count +// 2) at least 80% of detected keypoints have a similar reference keypoint +void compareKeypoints(const std::vector& ref, const std::vector& detected, const std::string& stepName, const cv::Mat& refDesc = cv::Mat(), const cv::Mat& detDesc = cv::Mat()) +{ + const double kCountTolerance = 0.20; // 20% + const double kMinMatchRate = 0.80; // 80% + + // Validate descriptor dimensions if provided + bool useDesc = !refDesc.empty() && !detDesc.empty(); + if (useDesc) { + ASSERT_EQ(refDesc.rows, (int)ref.size()) << stepName << ": ref descriptor row count != ref keypoint count"; + ASSERT_EQ(detDesc.rows, (int)detected.size()) << stepName << ": det descriptor row count != det keypoint count"; + ASSERT_EQ(refDesc.cols, detDesc.cols) << stepName << ": descriptor column count mismatch"; + } + + // Convert descriptors to CV_64F for uniform comparison + cv::Mat refDescF, detDescF; + int descCols = 0; + if (useDesc) { + refDesc.convertTo(refDescF, CV_64F); + detDesc.convertTo(detDescF, CV_64F); + descCols = refDescF.cols; + } + + // 1) Check that counts are within 10% + double refCount = (double)ref.size(); + double detCount = (double)detected.size(); + double countRatio = (refCount > 0) ? std::abs(detCount - refCount) / refCount : detCount; + EXPECT_LE(countRatio, kCountTolerance) << stepName << ": keypoint count out of tolerance — detected " << detected.size() << ", reference " << ref.size() << " (diff " << (countRatio * 100.0) << "%)"; + + int goodCount = 0; + for (size_t i = 0; i < detected.size(); ++i) { + const double* detRow = useDesc ? detDescF.ptr((int)i) : nullptr; + + bool found = false; + for (size_t j = 0; j < ref.size(); ++j) { + const double* refRow = useDesc ? refDescF.ptr((int)j) : nullptr; + if (keypointsSimilar(detected[i], ref[j], kRelEps, detRow, refRow, descCols)) { + found = true; + break; + } + } + if (found) + ++goodCount; + } + + double matchRate = (detected.empty()) ? 1.0 : (double)goodCount / detCount; + EXPECT_GE(matchRate, kMinMatchRate) << stepName << ": only " << goodCount << " / " << detected.size() << " detected keypoints (" << (matchRate * 100.0) << "%) matched a reference keypoint (need " << (kMinMatchRate * 100.0) << "%)" + << (useDesc ? " [with descriptors]" : ""); + + std::cout << "[ MATCH ] " << stepName << ": " << goodCount << "/" << detected.size() << " matched (" << (matchRate * 100.0) << "%), ref count=" << ref.size() << (useDesc ? " [with descriptors]" : "") << std::endl; +} + +// ── Generic "check or create" wrappers ──────────────────────────────────── + +void checkOrCreateOctaves(const std::string& filename, const std::vector& octaves, const std::string& stepName) +{ + std::string path = kDataDir + filename; + if (fs::exists(path)) { + auto ref = loadOctaves(path); + compareOctaves(ref, octaves, stepName); + std::cout << "[ CHECK ] " << stepName << ": matched reference " << filename << std::endl; + } else { + saveOctaves(path, octaves); + std::cout << "[ CREATE ] " << stepName << ": saved reference " << filename << std::endl; + } +} + +void saveDescriptors(const std::string& path, const cv::Mat& desc) +{ + + if (DENY_CREATE_REF_DATA) + throw std::runtime_error("saving reference data is denied"); + + cv::FileStorage fs(path, cv::FileStorage::WRITE); + fs << "descriptors" << desc; +} + +cv::Mat loadDescriptors(const std::string& path) +{ + cv::FileStorage fs(path, cv::FileStorage::READ); + cv::Mat desc; + fs["descriptors"] >> desc; + return desc; +} + +// Check or create keypoints, with optional descriptors. +// When descFilename is non-empty and desc is non-empty, descriptors are +// saved/loaded alongside keypoints and included in the similarity check. +void checkOrCreateKeypoints(const std::string& filename, const std::vector& kpts, const std::string& stepName, const std::string& descFilename = "", const cv::Mat& desc = cv::Mat()) +{ + std::string path = kDataDir + filename; + bool withDesc = !descFilename.empty() && !desc.empty(); + std::string descPath = withDesc ? (kDataDir + descFilename) : ""; + + if (fs::exists(path)) { + auto refKpts = loadKeypoints(path); + + cv::Mat refDesc; + if (withDesc && fs::exists(descPath)) { + refDesc = loadDescriptors(descPath); + } + + compareKeypoints(refKpts, kpts, stepName, refDesc, desc); + } else { + saveKeypoints(path, kpts); + if (withDesc) { + saveDescriptors(descPath, desc); + } + std::cout << "[ CREATE ] " << stepName << ": saved reference " << filename << (withDesc ? " + " + descFilename : "") << std::endl; + } +} + +} // namespace + +TEST(SIFT, DetectionDescriptionSteps) +{ +#if ENABLE_MY_SIFT_TESTING + ASSERT_TRUE(fs::exists(kDataDir)) << "Test data directory not found: " << kDataDir; + + phg::SIFTParams p; + p.upscale_first = false; + + cv::Mat img = cv::imread("data/src/test_sift/mysh1.jpg"); + ASSERT_FALSE(img.empty()) << "Failed to load test image mysh1.jpg"; + cv::resize(img, img, img.size() / 8, 0, 0, cv::INTER_AREA); + + std::cout << "loaded image of size: " << img.size() << std::endl; + + cv::Mat gray = phg::toGray32F(img); + + // Step 1: Build octaves (Gaussian scale-space) + std::vector octaves = buildOctaves(gray, p); + checkOrCreateOctaves("step1_octaves.yml.gz", octaves, "buildOctaves"); + if (::testing::Test::HasFatalFailure()) + return; + + // Step 2: Build Difference-of-Gaussians + std::vector dog = buildDoG(octaves, p); + checkOrCreateOctaves("step2_dog.yml.gz", dog, "buildDoG"); + if (::testing::Test::HasFatalFailure()) + return; + + // Step 3: Find scale-space extrema + std::vector kpts = findScaleSpaceExtrema(dog, p); + checkOrCreateKeypoints("step3_extrema.yml.gz", kpts, "findScaleSpaceExtrema"); + if (::testing::Test::HasFatalFailure()) + return; + std::cout << "detected n keypoints: " << kpts.size() << std::endl; + + // Step 4: Select top keypoints (first pass) + kpts = selectTopKeypoints(kpts, p); + checkOrCreateKeypoints("step4_top_kpts1.yml.gz", kpts, "selectTopKeypoints_1"); + if (::testing::Test::HasFatalFailure()) + return; + + std::cout << "selected n keypoints: " << kpts.size() << std::endl; + + // Step 5: Compute orientations + kpts = computeOrientations(kpts, octaves, p); + checkOrCreateKeypoints("step5_orientations.yml.gz", kpts, "computeOrientations"); + if (::testing::Test::HasFatalFailure()) + return; + + std::cout << "oriented n keypoints: " << kpts.size() << std::endl; + + // Step 6: Select top keypoints (second pass) + kpts = selectTopKeypoints(kpts, p); + checkOrCreateKeypoints("step6_top_kpts2.yml.gz", kpts, "selectTopKeypoints_2"); + if (::testing::Test::HasFatalFailure()) + return; + + std::cout << "selected n keypoints: " << kpts.size() << std::endl; + + // Step 7: Compute descriptors + cv::Mat desc; + std::tie(desc, kpts) = computeDescriptors(kpts, octaves, p); + checkOrCreateKeypoints("step7_desc_kpts.yml.gz", kpts, "computeDescriptors", "step7_descriptors.yml.gz", desc); + if (::testing::Test::HasFatalFailure()) + return; + + std::cout << "described n keypoints: " << kpts.size() << std::endl; +#else + std::cout << "ENABLE_MY_SIFT_TESTING is disabled, test skipped" << std::endl; +#endif +} + +TEST(SIFT, PairMatching) +{ +#if ENABLE_MY_SIFT_TESTING + cv::Mat img1 = cv::imread("data/src/test_sift/mysh2.jpg"); + ASSERT_FALSE(img1.empty()); + + cv::Mat img2 = cv::imread("data/src/test_sift/mysh3.jpg"); + ASSERT_FALSE(img2.empty()); + + cv::resize(img1, img1, img1.size() / 2, 0, 0, cv::INTER_AREA); + cv::resize(img2, img2, img2.size() / 2, 0, 0, cv::INTER_AREA); + std::cout << "image sizes: " << img1.size() << ", " << img2.size() << std::endl; + + phg::SIFTParams params; + params.nfeatures = 10000; + + std::cout << "matching using opencv orb..." << std::endl; + auto orb_cv = cv::ORB::create(params.nfeatures); + evaluateMatching(*orb_cv, img1, img2, "data/debug/test_sift/SIFT/Matches_ORB.jpg"); + + std::cout << "matching using opencv sift..." << std::endl; + auto sift_cv = cv::SIFT::create(params.nfeatures, params.n_octave_layers, params.contrast_threshold, params.edge_threshold); + MatchingPairData data_cv = evaluateMatching(*sift_cv, img1, img2, "data/debug/test_sift/SIFT/Matches_SIFTOCV.jpg"); + + std::cout << "matching using my sift..." << std::endl; + phg::SIFT sift(params); + MatchingPairData data = evaluateMatching(sift, img1, img2, "data/debug/test_sift/SIFT/Matches_SIFT_MY.jpg"); + + double thresh = 0.8; // expect at least 80% of opencv sift points & matches + EXPECT_GE(data.npoints1, thresh * data_cv.npoints1); + EXPECT_GE(data.npoints2, thresh * data_cv.npoints2); + EXPECT_GE(data.nmatches, thresh * data_cv.nmatches); + + std::cout << "Final score: " << data.nmatches << std::endl; +#else + std::cout << "ENABLE_MY_SIFT_TESTING is disabled, test skipped" << std::endl; + std::cout << "Final score: UNKNOWN" << std::endl; +#endif +}