diff --git a/src/phg/matching/descriptor_matcher.cpp b/src/phg/matching/descriptor_matcher.cpp index f4bcd87..4545c5d 100644 --- a/src/phg/matching/descriptor_matcher.cpp +++ b/src/phg/matching/descriptor_matcher.cpp @@ -1,16 +1,46 @@ #include "descriptor_matcher.h" -#include #include "flann_factory.h" +#include + +#include +#include +#include +#include +#include + void phg::DescriptorMatcher::filterMatchesRatioTest(const std::vector> &matches, std::vector &filtered_matches) { + const float ratio_thresh = 0.75f; + filtered_matches.clear(); + filtered_matches.reserve(matches.size()); - throw std::runtime_error("not implemented yet"); -} + for (const auto &m : matches) { + if (m.size() < 2) { + continue; + } + + const cv::DMatch &m0 = m[0]; + const cv::DMatch &m1 = m[1]; + if (m0.queryIdx < 0 || m0.trainIdx < 0) { + continue; + } + if (!std::isfinite(m0.distance) || !std::isfinite(m1.distance)) { + continue; + } + if (m1.distance <= 0.0f) { + continue; + } + + if (m0.distance < ratio_thresh * m1.distance) { + filtered_matches.push_back(m0); + } + } +} void phg::DescriptorMatcher::filterMatchesClusters(const std::vector &matches, const std::vector keypoints_query, @@ -19,58 +49,100 @@ void phg::DescriptorMatcher::filterMatchesClusters(const std::vector { filtered_matches.clear(); - const size_t total_neighbours = 5; // total number of neighbours to test (including candidate) - const size_t consistent_matches = 3; // minimum number of consistent matches (including candidate) - const float radius_limit_scale = 2.f; // limit search radius by scaled median - - const int n_matches = matches.size(); + const int total_neighbours = 5; + const int consistent_matches = 3; + const float radius_limit_scale = 2.f; + const int n_matches = (int)matches.size(); if (n_matches < total_neighbours) { - throw std::runtime_error("DescriptorMatcher::filterMatchesClusters : too few matches"); + filtered_matches = matches; + return; } cv::Mat points_query(n_matches, 2, CV_32FC1); cv::Mat points_train(n_matches, 2, CV_32FC1); for (int i = 0; i < n_matches; ++i) { - points_query.at(i) = keypoints_query[matches[i].queryIdx].pt; - points_train.at(i) = keypoints_train[matches[i].trainIdx].pt; + const int qi = matches[i].queryIdx; + const int ti = matches[i].trainIdx; + + if (qi < 0 || qi >= (int)keypoints_query.size() || ti < 0 || ti >= (int)keypoints_train.size()) { + points_query.at(i, 0) = 0.0f; + points_query.at(i, 1) = 0.0f; + points_train.at(i, 0) = 0.0f; + points_train.at(i, 1) = 0.0f; + continue; + } + + points_query.at(i, 0) = keypoints_query[qi].pt.x; + points_query.at(i, 1) = keypoints_query[qi].pt.y; + points_train.at(i, 0) = keypoints_train[ti].pt.x; + points_train.at(i, 1) = keypoints_train[ti].pt.y; + } + + const int ntrees = 1; + const int nchecks = 32; + std::shared_ptr index_params = flannKdTreeIndexParams(ntrees); + std::shared_ptr search_params = flannKsTreeSearchParams(nchecks); + + std::shared_ptr index_query = flannKdTreeIndex(points_query, index_params); + std::shared_ptr index_train = flannKdTreeIndex(points_train, index_params); + + 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); + + float radius2_query = 0.0f; + float radius2_train = 0.0f; + { + std::vector max_d2_q(n_matches); + std::vector max_d2_t(n_matches); + for (int i = 0; i < n_matches; ++i) { + max_d2_q[i] = distances2_query.at(i, total_neighbours - 1); + max_d2_t[i] = distances2_train.at(i, total_neighbours - 1); + } + + const int median_pos = n_matches / 2; + std::nth_element(max_d2_q.begin(), max_d2_q.begin() + median_pos, max_d2_q.end()); + std::nth_element(max_d2_t.begin(), max_d2_t.begin() + median_pos, max_d2_t.end()); + + radius2_query = max_d2_q[median_pos] * radius_limit_scale * radius_limit_scale; + radius2_train = max_d2_t[median_pos] * radius_limit_scale * radius_limit_scale; + if (!(radius2_query > 0.0f)) radius2_query = std::numeric_limits::infinity(); + if (!(radius2_train > 0.0f)) radius2_train = std::numeric_limits::infinity(); + } + + filtered_matches.reserve(n_matches); + + for (int i = 0; i < n_matches; ++i) { + std::array neigh_q{}; + int n_q = 0; + for (int k = 0; k < total_neighbours; ++k) { + const float d2 = distances2_query.at(i, k); + if (d2 <= radius2_query) { + neigh_q[(size_t)n_q++] = indices_query.at(i, k); + } + } + + int inter = 0; + for (int k = 0; k < total_neighbours; ++k) { + const float d2 = distances2_train.at(i, k); + if (d2 <= radius2_train) { + const int id = indices_train.at(i, k); + for (int j = 0; j < n_q; ++j) { + if (neigh_q[(size_t)j] == id) { + ++inter; + break; + } + } + } + } + + if (inter >= consistent_matches) { + filtered_matches.push_back(matches[i]); + } } -// -// // размерность всего 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 } diff --git a/src/phg/matching/descriptor_matcher.h b/src/phg/matching/descriptor_matcher.h index d7600bc..abc2d99 100644 --- a/src/phg/matching/descriptor_matcher.h +++ b/src/phg/matching/descriptor_matcher.h @@ -1,6 +1,9 @@ #pragma once #include +#include + +#include namespace phg { @@ -17,4 +20,4 @@ namespace phg { std::vector &filtered_matches); }; -} \ No newline at end of file +} diff --git a/src/phg/matching/flann_matcher.cpp b/src/phg/matching/flann_matcher.cpp index 9e9f518..e12c1c4 100644 --- a/src/phg/matching/flann_matcher.cpp +++ b/src/phg/matching/flann_matcher.cpp @@ -1,21 +1,81 @@ #include +#include +#include +#include + #include "flann_matcher.h" #include "flann_factory.h" - phg::FlannMatcher::FlannMatcher() { - // параметры для приближенного поиска -// index_params = flannKdTreeIndexParams(TODO); -// search_params = flannKsTreeSearchParams(TODO); + const int ntrees = 4; + const int nchecks = 32; + + index_params = flannKdTreeIndexParams(ntrees); + search_params = flannKsTreeSearchParams(nchecks); } void phg::FlannMatcher::train(const cv::Mat &train_desc) { - flann_index = flannKdTreeIndex(train_desc, index_params); + if (train_desc.rows < 2) { + throw std::runtime_error("FlannMatcher::train : needed at least 2 train descriptors"); + } + + if (train_desc.type() != CV_32F) { + train_desc.convertTo(train_desc_, CV_32F); + } else { + train_desc_ = train_desc; + } + + flann_index = flannKdTreeIndex(train_desc_, index_params); } -void phg::FlannMatcher::knnMatch(const cv::Mat &query_desc, std::vector> &matches, int k) const +void phg::FlannMatcher::knnMatch(const cv::Mat &query_desc, + std::vector> &matches, + int k) const { - throw std::runtime_error("not implemented yet"); + if (!flann_index) { + throw std::runtime_error("FlannMatcher::knnMatch : matcher is not trained"); + } + if (!search_params) { + throw std::runtime_error("FlannMatcher::knnMatch : invalid search_params"); + } + if (k <= 0) { + throw std::runtime_error("FlannMatcher::knnMatch : invalid k"); + } + + cv::Mat query32f; + if (query_desc.type() != CV_32F) { + query_desc.convertTo(query32f, CV_32F); + } else { + query32f = query_desc; + } + + const int n_query = query32f.rows; + matches.assign(n_query, {}); + + cv::Mat indices(n_query, k, CV_32SC1); + cv::Mat dists2(n_query, k, CV_32FC1); + + flann_index->knnSearch(query32f, indices, dists2, k, *search_params); + + for (int qi = 0; qi < n_query; ++qi) { + std::vector &dst = matches[qi]; + dst.resize(k); + + for (int j = 0; j < k; ++j) { + cv::DMatch m; + m.queryIdx = qi; + m.trainIdx = indices.at(qi, j); + m.imgIdx = 0; + + float d2 = dists2.at(qi, j); + m.distance = (d2 > 0.0f) ? std::sqrt(d2) : 0.0f; + + dst[j] = m; + } + + std::sort(dst.begin(), dst.end(), + [](const cv::DMatch &a, const cv::DMatch &b) { return a.distance < b.distance; }); + } } diff --git a/src/phg/matching/flann_matcher.h b/src/phg/matching/flann_matcher.h index 6f0c092..f9732f0 100644 --- a/src/phg/matching/flann_matcher.h +++ b/src/phg/matching/flann_matcher.h @@ -3,6 +3,8 @@ #include "descriptor_matcher.h" #include +#include + namespace phg { struct FlannMatcher : DescriptorMatcher { @@ -15,6 +17,8 @@ namespace phg { private: + cv::Mat train_desc_; + std::shared_ptr index_params; std::shared_ptr search_params; std::shared_ptr flann_index; diff --git a/src/phg/sfm/homography.cpp b/src/phg/sfm/homography.cpp index 5cbc780..981018d 100644 --- a/src/phg/sfm/homography.cpp +++ b/src/phg/sfm/homography.cpp @@ -3,6 +3,12 @@ #include #include +#include +#include +#include +#include +#include + namespace { // источник: https://e-maxx.ru/algo/linear_systems_gauss @@ -56,7 +62,6 @@ namespace { return 1; } - // см. Hartley, Zisserman: Multiple View Geometry in Computer Vision. Second Edition 4.1, 4.1.2 cv::Mat estimateHomography4Points(const cv::Point2f &l0, const cv::Point2f &l1, const cv::Point2f &l2, const cv::Point2f &l3, const cv::Point2f &r0, const cv::Point2f &r1, @@ -73,8 +78,6 @@ namespace { 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]; @@ -83,38 +86,31 @@ namespace { 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}); + (void)w1; + + A.push_back({ + x0, y0, w0, 0.0, 0.0, 0.0, + -x1 * x0, -x1 * y0, + x1 * w0 + }); + A.push_back({ + 0.0, 0.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()) { + } else if (res == 1) { + // ok + } 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 - { + } 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); @@ -122,7 +118,6 @@ namespace { return H_mat; } - // pseudorandom number generator inline uint64_t xorshift64(uint64_t *state) { if (*state == 0) { @@ -139,86 +134,158 @@ namespace { 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; + int v = (int)(xorshift64(state) % (uint64_t)max_id); if (dst.empty() || std::find(dst.begin(), dst.end(), v) == dst.end()) { dst.push_back(v); break; } } - if (dst.size() < i + 1) { + if ((int)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) + cv::Mat estimateHomographyRANSAC(const std::vector &points_lhs, + const std::vector &points_rhs) { if (points_lhs.size() != points_rhs.size()) { throw std::runtime_error("findHomography: points_lhs.size() != points_rhs.size()"); } + if (points_lhs.size() < 4) { + throw std::runtime_error("estimateHomographyRANSAC : too few points"); + } + + const int n_matches = (int)points_lhs.size(); + + const double reprojection_error_threshold_px = 3.0; + const double reprojection_error_threshold2 = reprojection_error_threshold_px * reprojection_error_threshold_px; + const double confidence = 0.999; + const int sample_size = 4; + int max_trials = 2000; + + auto isDegenerate = [&](const std::vector &s) -> bool { + auto area2 = [](const cv::Point2f &a, const cv::Point2f &b, const cv::Point2f &c) { + cv::Point2f ab = b - a; + cv::Point2f ac = c - a; + return std::abs(ab.x * ac.y - ab.y * ac.x); + }; + const double eps = 1e-3; + + const cv::Point2f &p0 = points_lhs[s[0]]; + const cv::Point2f &p1 = points_lhs[s[1]]; + const cv::Point2f &p2 = points_lhs[s[2]]; + const cv::Point2f &p3 = points_lhs[s[3]]; + const cv::Point2f &q0 = points_rhs[s[0]]; + const cv::Point2f &q1 = points_rhs[s[1]]; + const cv::Point2f &q2 = points_rhs[s[2]]; + const cv::Point2f &q3 = points_rhs[s[3]]; - // 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; + if (area2(p0, p1, p2) < eps) return true; + if (area2(p0, p1, p3) < eps) return true; + if (area2(p0, p2, p3) < eps) return true; + if (area2(p1, p2, p3) < eps) return true; + + if (area2(q0, q1, q2) < eps) return true; + if (area2(q0, q1, q3) < eps) return true; + if (area2(q0, q2, q3) < eps) return true; + if (area2(q1, q2, q3) < eps) return true; + + return false; + }; + + uint64_t seed = 1; + int best_support = 0; + cv::Mat best_H; + std::vector best_inliers_mask(n_matches, 0); + + int trials = max_trials; + std::vector sample; + std::vector inliers_mask(n_matches); + + for (int i_trial = 0; i_trial < trials; ++i_trial) { + randomSample(sample, n_matches, sample_size, &seed); + if (isDegenerate(sample)) { + continue; + } + + cv::Mat H; + try { + H = estimateHomography4Points(points_lhs[sample[0]], points_lhs[sample[1]], points_lhs[sample[2]], points_lhs[sample[3]], + points_rhs[sample[0]], points_rhs[sample[1]], points_rhs[sample[2]], points_rhs[sample[3]]); + } catch (...) { + continue; + } + + int support = 0; + for (int i_point = 0; i_point < n_matches; ++i_point) { + bool inl = false; + try { + cv::Point2d proj = phg::transformPoint(points_lhs[i_point], H); + cv::Point2d diff = proj - cv::Point2d(points_rhs[i_point]); + double d2 = diff.x * diff.x + diff.y * diff.y; + inl = (d2 <= reprojection_error_threshold2); + } catch (...) { + inl = false; + } + + inliers_mask[i_point] = (char)inl; + if (inl) ++support; + } + + if (support > best_support) { + best_support = support; + best_H = H; + best_inliers_mask = inliers_mask; + + double w = (double)best_support / (double)n_matches; + w = std::clamp(w, 1e-6, 1.0 - 1e-6); + double p_no_outliers = std::pow(w, sample_size); + p_no_outliers = std::clamp(p_no_outliers, 1e-12, 1.0 - 1e-12); + double new_trials = std::log(1.0 - confidence) / std::log(1.0 - p_no_outliers); + if (std::isfinite(new_trials)) { + trials = std::min(trials, (int)std::ceil(new_trials)); + trials = std::clamp(trials, 50, max_trials); + } + + if (best_support == n_matches) { + break; + } + } + } + + if (best_support < 4) { + throw std::runtime_error("estimateHomographyRANSAC : failed to estimate homography"); + } + + std::vector inl_lhs; + std::vector inl_rhs; + inl_lhs.reserve((size_t)best_support); + inl_rhs.reserve((size_t)best_support); + + for (int i = 0; i < n_matches; ++i) { + if (best_inliers_mask[i]) { + inl_lhs.push_back(points_lhs[i]); + inl_rhs.push_back(points_rhs[i]); + } + } + + if (inl_lhs.size() >= 4) { + cv::Mat refined = cv::findHomography(inl_lhs, inl_rhs, 0); + if (!refined.empty()) { + double s = refined.at(2, 2); + if (std::abs(s) > 1e-12) { + refined /= s; + } + return refined; + } + } + + return best_H; } } @@ -228,21 +295,51 @@ cv::Mat phg::findHomography(const std::vector &points_lhs, const st 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"); + if (T.empty() || T.rows != 3 || T.cols != 3) { + throw std::runtime_error("transformPoint: expected 3x3 matrix"); + } + + double a00, a01, a02; + double a10, a11, a12; + double a20, a21, a22; + + if (T.type() == CV_64FC1) { + a00 = T.at(0, 0); a01 = T.at(0, 1); a02 = T.at(0, 2); + a10 = T.at(1, 0); a11 = T.at(1, 1); a12 = T.at(1, 2); + a20 = T.at(2, 0); a21 = T.at(2, 1); a22 = T.at(2, 2); + } else if (T.type() == CV_32FC1) { + a00 = T.at(0, 0); a01 = T.at(0, 1); a02 = T.at(0, 2); + a10 = T.at(1, 0); a11 = T.at(1, 1); a12 = T.at(1, 2); + a20 = T.at(2, 0); a21 = T.at(2, 1); a22 = T.at(2, 2); + } else { + cv::Mat Td; + T.convertTo(Td, CV_64F); + a00 = Td.at(0, 0); a01 = Td.at(0, 1); a02 = Td.at(0, 2); + a10 = Td.at(1, 0); a11 = Td.at(1, 1); a12 = Td.at(1, 2); + a20 = Td.at(2, 0); a21 = Td.at(2, 1); a22 = Td.at(2, 2); + } + + const double x = pt.x; + const double y = pt.y; + + const double w = a20 * x + a21 * y + a22; + if (!std::isfinite(w) || std::abs(w) < 1e-12) { + throw std::runtime_error("transformPoint: invalid homogeneous coordinate"); + } + + const double xp = (a00 * x + a01 * y + a02) / w; + const double yp = (a10 * x + a11 * y + a12) / w; + return {xp, yp}; } 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); diff --git a/src/phg/sfm/homography.h b/src/phg/sfm/homography.h index 9127f23..732d4c9 100644 --- a/src/phg/sfm/homography.h +++ b/src/phg/sfm/homography.h @@ -2,6 +2,8 @@ #include +#include + namespace phg { cv::Mat findHomography(const std::vector &points_lhs, @@ -12,4 +14,4 @@ namespace phg { cv::Point2d transformPoint(const cv::Point2d &pt, const cv::Mat &T); cv::Point2d transformPointCV(const cv::Point2d &pt, const cv::Mat &T); -} \ No newline at end of file +} diff --git a/src/phg/sfm/panorama_stitcher.cpp b/src/phg/sfm/panorama_stitcher.cpp index 8d76939..bbfa227 100644 --- a/src/phg/sfm/panorama_stitcher.cpp +++ b/src/phg/sfm/panorama_stitcher.cpp @@ -4,26 +4,82 @@ #include #include -/* - * imgs - список картинок - * parent - список индексов, каждый индекс указывает, к какой картинке должна быть приклеена текущая картинка - * этот список образует дерево, корень дерева (картинка, которая ни к кому не приклеивается, приклеиваются только к ней), в данном массиве имеет значение -1 - * homography_builder - функтор, возвращающий гомографию по паре картинок - * */ +#include +#include +#include +#include + cv::Mat phg::stitchPanorama(const std::vector &imgs, const std::vector &parent, std::function &homography_builder) { - const int n_images = imgs.size(); - - // склеивание панорамы происходит через приклеивание всех картинок к корню, некоторые приклеиваются не напрямую, а через цепочку других картинок + const int n_images = (int)imgs.size(); - // вектор гомографий, для каждой картинки описывает преобразование до корня std::vector Hs(n_images); { - // здесь надо посчитать вектор Hs - // при этом можно обойтись n_images - 1 вызовами функтора homography_builder - throw std::runtime_error("not implemented yet"); + if ((int)parent.size() != n_images) { + throw std::runtime_error("stitchPanorama: parent.size() != imgs.size()"); + } + + int root = -1; + for (int i = 0; i < n_images; ++i) { + if (parent[i] == -1) { + if (root != -1) { + throw std::runtime_error("stitchPanorama: multiple roots"); + } + root = i; + } else { + if (parent[i] < 0 || parent[i] >= n_images) { + throw std::runtime_error("stitchPanorama: invalid parent index"); + } + } + } + if (root == -1) { + throw std::runtime_error("stitchPanorama: no root image (parent==-1)"); + } + + std::vector> children((size_t)n_images); + children.assign((size_t)n_images, {}); + for (int i = 0; i < n_images; ++i) { + const int p = parent[i]; + if (p >= 0) { + children[(size_t)p].push_back(i); + } + } + + Hs[root] = cv::Mat::eye(3, 3, CV_64FC1); + + std::vector visited((size_t)n_images, 0); + visited[(size_t)root] = 1; + + std::vector stack; + stack.push_back(root); + + while (!stack.empty()) { + const int cur = stack.back(); + stack.pop_back(); + + for (int child : children[(size_t)cur]) { + cv::Mat H_child_to_parent = homography_builder(imgs[child], imgs[cur]); + if (H_child_to_parent.empty() || H_child_to_parent.rows != 3 || H_child_to_parent.cols != 3) { + throw std::runtime_error("stitchPanorama: homography_builder returned invalid matrix"); + } + if (H_child_to_parent.type() != CV_64FC1) { + H_child_to_parent.convertTo(H_child_to_parent, CV_64F); + } + + Hs[child] = Hs[cur] * H_child_to_parent; + + visited[(size_t)child] = 1; + stack.push_back(child); + } + } + + for (int i = 0; i < n_images; ++i) { + if (!visited[(size_t)i]) { + throw std::runtime_error("stitchPanorama: parent array does not form a connected tree"); + } + } } bbox2 bbox; @@ -43,23 +99,6 @@ cv::Mat phg::stitchPanorama(const std::vector &imgs, 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(); }); @@ -69,7 +108,6 @@ cv::Mat phg::stitchPanorama(const std::vector &imgs, 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]); diff --git a/src/phg/sfm/panorama_stitcher.h b/src/phg/sfm/panorama_stitcher.h index e8567b3..7bf5cc8 100644 --- a/src/phg/sfm/panorama_stitcher.h +++ b/src/phg/sfm/panorama_stitcher.h @@ -2,6 +2,9 @@ #include +#include +#include + namespace phg { cv::Mat stitchPanorama( @@ -10,4 +13,4 @@ namespace phg { std::function &homography_builder ); -} \ No newline at end of file +} diff --git a/tests/test_matching.cpp b/tests/test_matching.cpp index adaac65..c9e927e 100644 --- a/tests/test_matching.cpp +++ b/tests/test_matching.cpp @@ -20,7 +20,7 @@ // TODO enable both toggles for testing custom detector & matcher #define ENABLE_MY_DESCRIPTOR 0 -#define ENABLE_MY_MATCHING 0 +#define ENABLE_MY_MATCHING 1 #define ENABLE_GPU_BRUTEFORCE_MATCHER 0 // TODO disable for local testing but do not commit