From 5a562323dc692ac29d9353860f151f0c2b0d6ed7 Mon Sep 17 00:00:00 2001 From: mozhaa Date: Thu, 19 Mar 2026 11:29:50 +0300 Subject: [PATCH 1/5] Squashed commit of the following: commit d97fcc2a3aba0fd154c4311a70094a38645444c5 Author: mozhaa Date: Wed Mar 18 11:59:58 2026 +0300 fix: use different ratio_threshold again commit 04861d71339a4c44ba6c7d57fe1e362e81c769ee Author: mozhaa Date: Wed Mar 18 10:24:45 2026 +0300 fix: use different ratio threshold commit fa076f2be8f4b8886a82bf6772bed50440c94c6e Author: mozhaa Date: Wed Mar 18 09:44:41 2026 +0300 chore: enable my descriptor testing commit f314a4b80b35b39ae2dc3fe79db32c6d16b89bbe Author: mozhaa Date: Wed Mar 18 09:42:35 2026 +0300 feat: hw2 done commit 47e0b24bbf34888e093a8e0369ecd566f282c1b7 Author: mozhaa Date: Wed Mar 18 08:46:12 2026 +0300 Squashed commit of the following: commit de6673485a978ee6f3748e92f934cfe51e80b485 Author: mozhaa Date: Mon Feb 23 16:20:06 2026 +0300 feat: better answer in the last question commit 540e59d715c929697b60ccdb0e5d4b9001eec24c Author: mozhaa Date: Mon Feb 23 16:15:11 2026 +0300 feat: implemented all TODOs, enabled tests (they pass) commit 9d8dda19639f2eb36e06b1cbeea9c569ed4834db Author: Boris Date: Wed Mar 11 11:18:38 2026 +0300 Update README for bonus points criteria commit a56074e3a3440ad13e597ac78ff02cff89af845b Author: Boris Date: Tue Mar 10 22:48:06 2026 +0300 opencl matching can be slower on server --- src/phg/matching/descriptor_matcher.cpp | 111 +++++++----- src/phg/matching/flann_matcher.cpp | 18 +- src/phg/sfm/homography.cpp | 112 +++++++------ src/phg/sfm/panorama_stitcher.cpp | 20 ++- src/phg/sift/sift.cpp | 214 ++++++++++++++---------- tests/test_matching.cpp | 8 +- tests/test_sift.cpp | 2 +- 7 files changed, 291 insertions(+), 194 deletions(-) diff --git a/src/phg/matching/descriptor_matcher.cpp b/src/phg/matching/descriptor_matcher.cpp index f4bcd871..b4bb55d2 100644 --- a/src/phg/matching/descriptor_matcher.cpp +++ b/src/phg/matching/descriptor_matcher.cpp @@ -7,11 +7,16 @@ void phg::DescriptorMatcher::filterMatchesRatioTest(const std::vector &filtered_matches) { filtered_matches.clear(); - - throw std::runtime_error("not implemented yet"); + const float ratio_threshold = 0.4f; + for (const auto &knn : matches) { + if (knn.size() >= 2) { + if (knn[0].distance < ratio_threshold * knn[1].distance) { + filtered_matches.push_back(knn[0]); + } + } + } } - void phg::DescriptorMatcher::filterMatchesClusters(const std::vector &matches, const std::vector keypoints_query, const std::vector keypoints_train, @@ -35,42 +40,66 @@ void phg::DescriptorMatcher::filterMatchesClusters(const std::vector 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 + + // размерность всего 2, так что точное KD-дерево + std::shared_ptr index_params = flannKdTreeIndexParams(4); + std::shared_ptr search_params = flannKsTreeSearchParams(32); + + 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 общих элементов + for (int i = 0; i < n_matches; ++i) { + std::vector neighbors_query, neighbors_train; + for (int j = 0; j < total_neighbours; ++j) { + if (distances2_query.at(i, j) <= radius2_query) { + neighbors_query.push_back(indices_query.at(i, j)); + } + } + for (int j = 0; j < total_neighbours; ++j) { + if (distances2_train.at(i, j) <= radius2_train) { + neighbors_train.push_back(indices_train.at(i, j)); + } + } + + std::sort(neighbors_query.begin(), neighbors_query.end()); + std::sort(neighbors_train.begin(), neighbors_train.end()); + std::vector common; + std::set_intersection(neighbors_query.begin(), neighbors_query.end(), + neighbors_train.begin(), neighbors_train.end(), + std::back_inserter(common)); + + if (common.size() >= consistent_matches) { + filtered_matches.push_back(matches[i]); + } + } } diff --git a/src/phg/matching/flann_matcher.cpp b/src/phg/matching/flann_matcher.cpp index 9e9f5180..951de429 100644 --- a/src/phg/matching/flann_matcher.cpp +++ b/src/phg/matching/flann_matcher.cpp @@ -6,8 +6,8 @@ phg::FlannMatcher::FlannMatcher() { // параметры для приближенного поиска -// index_params = flannKdTreeIndexParams(TODO); -// search_params = flannKsTreeSearchParams(TODO); + index_params = flannKdTreeIndexParams(4); + search_params = flannKsTreeSearchParams(32); } void phg::FlannMatcher::train(const cv::Mat &train_desc) @@ -17,5 +17,17 @@ void phg::FlannMatcher::train(const cv::Mat &train_desc) void phg::FlannMatcher::knnMatch(const cv::Mat &query_desc, std::vector> &matches, int k) const { - throw std::runtime_error("not implemented yet"); + cv::Mat indices(query_desc.rows, k, CV_32S); + cv::Mat dists(query_desc.rows, k, CV_32F); + flann_index->knnSearch(query_desc, indices, dists, k, *search_params); + + matches.resize(query_desc.rows); + for (int i = 0; i < query_desc.rows; ++i) { + matches[i].resize(k); + for (int j = 0; j < k; ++j) { + int trainIdx = indices.at(i, j); + float distance = dists.at(i, j); + matches[i][j] = cv::DMatch(i, trainIdx, 0, distance); + } + } } diff --git a/src/phg/sfm/homography.cpp b/src/phg/sfm/homography.cpp index 5cbc780c..0c43a182 100644 --- a/src/phg/sfm/homography.cpp +++ b/src/phg/sfm/homography.cpp @@ -84,8 +84,8 @@ namespace { double w1 = ws1[i]; // 8 elements of matrix + free term as needed by gauss routine -// A.push_back({TODO}); -// A.push_back({TODO}); + A.push_back({x0, y0, 1, 0, 0, 0, -x0 * x1, -y0 * x1, x1}); + A.push_back({0, 0, 0, x0, y0, 1, -x0 * y1, -y0 * y1, y1}); } int res = gauss(A, H); @@ -168,57 +168,57 @@ namespace { // * (простое описание для понимания) // * [3] http://ikrisoft.blogspot.com/2015/01/ransac-with-contrario-approach.html -// const int n_matches = points_lhs.size(); -// -// // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters -// const int n_trials = TODO; -// -// const int n_samples = TODO; -// uint64_t seed = 1; -// const double reprojection_error_threshold_px = 2; -// -// int best_support = 0; -// cv::Mat best_H; -// -// std::vector sample; -// for (int i_trial = 0; i_trial < n_trials; ++i_trial) { -// randomSample(sample, n_matches, n_samples, &seed); -// -// cv::Mat H = estimateHomography4Points(points_lhs[sample[0]], points_lhs[sample[1]], points_lhs[sample[2]], points_lhs[sample[3]], -// points_rhs[sample[0]], points_rhs[sample[1]], points_rhs[sample[2]], points_rhs[sample[3]]); -// -// int support = 0; -// for (int i_point = 0; i_point < n_matches; ++i_point) { -// try { -// cv::Point2d proj = phg::transformPoint(points_lhs[i_point], H); -// if (cv::norm(proj - cv::Point2d(points_rhs[i_point])) < reprojection_error_threshold_px) { -// ++support; -// } -// } catch (const std::exception &e) -// { -// std::cerr << e.what() << std::endl; -// } -// } -// -// if (support > best_support) { -// best_support = support; -// best_H = H; -// -// std::cout << "estimateHomographyRANSAC : support: " << best_support << "/" << n_matches << std::endl; -// -// if (best_support == n_matches) { -// break; -// } -// } -// } -// -// std::cout << "estimateHomographyRANSAC : best support: " << best_support << "/" << n_matches << std::endl; -// -// if (best_support == 0) { -// throw std::runtime_error("estimateHomographyRANSAC : failed to estimate homography"); -// } -// -// return best_H; + const int n_matches = points_lhs.size(); + + // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters + const int n_trials = 1000; + + const int n_samples = 4; + 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; } } @@ -238,7 +238,11 @@ cv::Mat phg::findHomographyCV(const std::vector &points_lhs, const // таким преобразованием внутри занимается функции cv::perspectiveTransform и cv::warpPerspective cv::Point2d phg::transformPoint(const cv::Point2d &pt, const cv::Mat &T) { - throw std::runtime_error("not implemented yet"); + double x = pt.x, y = pt.y; + double w = T.at(2, 0) * x + T.at(2, 1) * y + T.at(2, 2); + double xp = (T.at(0, 0) * x + T.at(0, 1) * y + T.at(0, 2)) / w; + double yp = (T.at(1, 0) * x + T.at(1, 1) * y + T.at(1, 2)) / w; + return cv::Point2d(xp, yp); } cv::Point2d phg::transformPointCV(const cv::Point2d &pt, const cv::Mat &T) { diff --git a/src/phg/sfm/panorama_stitcher.cpp b/src/phg/sfm/panorama_stitcher.cpp index 8d76939b..7164bd78 100644 --- a/src/phg/sfm/panorama_stitcher.cpp +++ b/src/phg/sfm/panorama_stitcher.cpp @@ -23,7 +23,25 @@ cv::Mat phg::stitchPanorama(const std::vector &imgs, { // здесь надо посчитать вектор Hs // при этом можно обойтись n_images - 1 вызовами функтора homography_builder - throw std::runtime_error("not implemented yet"); + std::vector computed(n_images, false); + for (int i = 0; i < n_images; ++i) { + if (parent[i] == -1) { + Hs[i] = cv::Mat::eye(3, 3, CV_64F); + computed[i] = true; + } + } + bool changed = true; + while (changed) { + changed = false; + for (int i = 0; i < n_images; ++i) { + if (!computed[i] && parent[i] != -1 && computed[parent[i]]) { + cv::Mat H_i_to_parent = homography_builder(imgs[i], imgs[parent[i]]); + Hs[i] = Hs[parent[i]] * H_i_to_parent; + computed[i] = true; + changed = true; + } + } + } } bbox2 bbox; diff --git a/src/phg/sift/sift.cpp b/src/phg/sift/sift.cpp index 72047711..0dcdc829 100755 --- a/src/phg/sift/sift.cpp +++ b/src/phg/sift/sift.cpp @@ -108,16 +108,19 @@ std::vector phg::buildOctaves(const cv::Mat& img, const phg:: // можно подумать, как сделать эффективнее - для построения 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); + // корень из двух нужной степени, чтобы при i==s получали удвоение базового блюра + double sigma_layer = sigma0 * std::pow(2.0, (double)i / s); + // вычтем sigma0 чтобы размыть ровно до нужной суммарной сигмы + sigma_layer = std::sqrt(sigma_layer * sigma_layer - sigma0 * sigma0); + 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(даунскейлим текущий слой в два раза, без интерполяции, просто сабсепмлинг); + // даунскейлим текущий слой в два раза, без интерполяции, просто сабсеrпмлинг + // base = downsample2x(oct.layers[s]); + cv::resize(oct.layers[s], base, cv::Size(0, 0), 0.5, 0.5, cv::INTER_NEAREST); // можно использовать и downsample2x_avg(oct.layers[s]), это позволяет потом заапскейлить слои обратно до оригинального разрешения без сдвига // но потребуется везде изменить формулу для пересчета ключевых точек: pt_upscaled = (pt_downscaled + 0.5) * 2^o - 0.5 @@ -137,8 +140,10 @@ std::vector phg::buildDoG(const std::vector phg::findScaleSpaceExtrema(const std::vector= val) is_max = false; + if (v <= val) is_min = false; + } + } if (!is_max && !is_min) continue; - // TODO проверить локальный максимум на предыдущем скейле + // проверить локальный максимум на предыдущем скейле + for (int dy = -1; dy <= 1; ++dy) { + const float* row = (dy == -1) ? pp : (dy == 0 ? p : pn); + for (int dx = -1; dx <= 1; ++dx) { + float v = row[x + dx]; + if (v >= val) is_max = false; + if (v <= val) is_min = false; + } + } if (!is_max && !is_min) continue; - // TODO проверить локальный максимум на следующем скейле + // проверить локальный максимум на следующем скейле + for (int dy = -1; dy <= 1; ++dy) { + const float* row = (dy == -1) ? np : (dy == 0 ? n : nn); + for (int dx = -1; dx <= 1; ++dx) { + float v = row[x + dx]; + if (v >= val) is_max = false; + if (v <= val) is_min = false; + } + } if (!is_max && !is_min) continue; @@ -238,13 +268,12 @@ std::vector phg::findScaleSpaceExtrema(const std::vector(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; + dxx = cL.at(yi, xi + 1) + cL.at(yi, xi - 1) - 2.f * resp_center; + dyy = cL.at(yi + 1, xi) + cL.at(yi - 1, xi) - 2.f * resp_center; + dss = nL.at(yi, xi) + pL.at(yi, xi) - 2.f * resp_center; + 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; + dxs = ( (nL.at(yi, xi + 1) - nL.at(yi, xi - 1)) - (pL.at(yi, xi + 1) - pL.at(yi, xi - 1)) ) * 0.25f; + 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); @@ -273,21 +302,21 @@ std::vector phg::findScaleSpaceExtrema(const std::vector= (r + 1) * (r + 1) / r) + break; } // скейлим координаты точек обратно до родных размеров картинки @@ -379,39 +408,39 @@ std::vector phg::computeOrientations(const std::vector(py, px + 1) - img.at(py, px - 1); -// float gy = img.at(py + 1, px) - img.at(py - 1, px); -// -// float mag = TODO; -// float angle = std::atan2(TODO); // [-pi, pi] -// -// float angle_deg = angle * 180.f / (float) CV_PI; -// if (angle_deg < 0.f) angle_deg += 360.f; -// -// // гауссово взвешивание голоса точки с затуханием к краям -// float weight = std::exp(-(TODO) / (2.f * sigma_win * sigma_win)); -// if (!params.enable_orientation_gaussian_weighting) { -// weight = 1.f; -// } -// -// // голосуем в гистограмме направлений. находим два ближайших бина и гладко распределяем голос между ними -// // в таком случае, голос попавший близко к границе между бинами, проголосует поровну за оба бина -// float bin = TODO; -// if (bin >= n_bins) bin -= n_bins; -// int bin0 = (int) bin; -// int bin1 = (bin0 + 1) % n_bins; -// -// float frac = bin - bin0; -// if (!params.enable_orientation_bin_interpolation) { -// frac = 0.f; -// } -// -// histogram[bin0] += TODO; -// histogram[bin1] += TODO; + int px = xi + dx; + int py = yi + dy; + + // градиент + float gx = img.at(py, px + 1) - img.at(py, px - 1); + float gy = img.at(py + 1, px) - img.at(py - 1, px); + + float mag = std::sqrt(gx * gx + gy * gy); + float angle = std::atan2(gy, gx); // [-pi, pi] + + float angle_deg = angle * 180.f / (float) CV_PI; + if (angle_deg < 0.f) angle_deg += 360.f; + + // гауссово взвешивание голоса точки с затуханием к краям + float weight = std::exp(-(dx*dx + dy*dy) / (2.f * sigma_win * sigma_win)); + if (!params.enable_orientation_gaussian_weighting) { + weight = 1.f; + } + + // голосуем в гистограмме направлений. находим два ближайших бина и гладко распределяем голос между ними + // в таком случае, голос попавший близко к границе между бинами, проголосует поровну за оба бина + float bin = angle_deg * n_bins / 360.f; + 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.f - frac); + histogram[bin1] += weight * mag * frac; } } @@ -450,20 +479,20 @@ std::vector phg::computeOrientations(const std::vector a = (left + right - 2 * center) / 2 // f(1) - f(-1) = 2b -> b = (right - left) / 2 -// float offset = TODO; -// if (!params.enable_orientation_subpixel_localization) { -// offset = 0.f; -// } -// -// float bin_real = i + offset; -// if (bin_real < 0.f) bin_real += n_bins; -// if (bin_real >= n_bins) bin_real -= n_bins; -// -// float angle = bin_real * 360.f / n_bins; -// -// cv::KeyPoint new_kp = kp; -// new_kp.angle = angle; -// oriented_kpts.push_back(new_kp); + float offset = 0.5f * (left - right) / (left + right - 2.f * center); + if (!params.enable_orientation_subpixel_localization) { + offset = 0.f; + } + + float bin_real = i + offset; + if (bin_real < 0.f) bin_real += n_bins; + if (bin_real >= n_bins) bin_real -= n_bins; + + float angle = bin_real * 360.f / n_bins; + + cv::KeyPoint new_kp = kp; + new_kp.angle = angle; + oriented_kpts.push_back(new_kp); } } } @@ -574,11 +603,11 @@ std::pair> phg::computeDescriptors(const std: bin_o -= n_orient_bins; // семплы вблизи края патча взвешиваем с меньшим весом -// float weight = std::exp(-(TODO) / (2.f * sigma_desc * sigma_desc)); -// if (!params.enable_descriptor_gaussian_weighting) { -// weight = 1.f; -// } -// float weighted_mag = mag * weight; + float weight = std::exp(-(rot_x * rot_x + rot_y * rot_y) / (2.f * sigma_desc * sigma_desc)); + if (!params.enable_descriptor_gaussian_weighting) { + weight = 1.f; + } + float weighted_mag = mag * weight; if (params.enable_descriptor_bin_interpolation) { // размажем вклад weighted_mag по пространственным бинам и по бинам гистограммок трилинейной интерполяцией @@ -609,8 +638,8 @@ std::pair> phg::computeDescriptors(const std: io += n_orient_bins; float wo = (dio == 0) ? (1.f - fo) : fo; -// int idx = TODO; -// desc[idx] += TODO; + int idx = (iy * n_spatial_bins + ix) * n_orient_bins + io; + desc[idx] += weighted_mag * wx * wy * wo; } } } @@ -620,9 +649,8 @@ std::pair> phg::computeDescriptors(const std: int io_nearest = (int)std::round(bin_o) % n_orient_bins; if (ix_nearest >= 0 && ix_nearest < n_spatial_bins && iy_nearest >= 0 && iy_nearest < n_spatial_bins) { - // TODO uncomment -// int idx = (iy_nearest * n_spatial_bins + ix_nearest) * n_orient_bins + io_nearest; -// desc[idx] += weighted_mag; + int idx = (iy_nearest * n_spatial_bins + ix_nearest) * n_orient_bins + io_nearest; + desc[idx] += weighted_mag; } } } @@ -731,7 +759,13 @@ void phg::SIFT::detectAndCompute(const cv::Mat& img, const cv::Mat& mask, std::v std::tie(desc, kpts) = computeDescriptors(kpts, octaves, p, verbose_level); // TODO всегда ли мы получаем ровно столько точек сколько запросили в параметре nfeatures? в каких случаях это не так и в какую сторону? - // как подкрутить алгоритм, чтобы всегда выдавать ровно запрошенное количество точек (когда это в принципе возможно) но не сильно просесть в производительности? + // + // не всегда, может быть меньше (но больше нет из-за selectTopKeypoints), в случае когда на картинке просто много фичей. + + // как подкрутить алгоритм, чтобы всегда выдавать ровно запрошенное количество точек (когда это в принципе возможно) но не сильно просесть в производительности? + // + // бинарный поиск по threshold? + } void phg::SIFT::saveImg(const std::string& name, const cv::Mat& img) const diff --git a/tests/test_matching.cpp b/tests/test_matching.cpp index 4de5b716..5c7a61b5 100644 --- a/tests/test_matching.cpp +++ b/tests/test_matching.cpp @@ -19,8 +19,8 @@ // TODO enable both toggles for testing custom detector & matcher -#define ENABLE_MY_DESCRIPTOR 0 -#define ENABLE_MY_MATCHING 0 +#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 @@ -560,7 +560,7 @@ TEST (MATCHING, SimpleMatching) { EXPECT_LT(time_my, 1.5 * time_cv); EXPECT_LT(time_my, 0.1 * time_bruteforce); -#if ENABLE_GPU_BRUTEFORCE_MATCHER +#if ENABLE_GPU_BRUTEFORCE_MATCHER && !SERVER_TESTING EXPECT_LT(time_bruteforce_gpu, time_bruteforce); #endif @@ -839,4 +839,4 @@ TEST (STITCHING, Orthophoto) { std::cout << "n stable ortho kpts: : " << score << std::endl; EXPECT_GT(score, 7500); #endif -} \ No newline at end of file +} diff --git a/tests/test_sift.cpp b/tests/test_sift.cpp index cf3bd7df..7433f9bb 100755 --- a/tests/test_sift.cpp +++ b/tests/test_sift.cpp @@ -28,7 +28,7 @@ // TODO ENABLE ME // TODO ENABLE ME // TODO ENABLE ME -#define ENABLE_MY_SIFT_TESTING 0 +#define ENABLE_MY_SIFT_TESTING 1 #define DENY_CREATE_REF_DATA 1 From 891715639469fe491e0ae1395b752d3a437f7953 Mon Sep 17 00:00:00 2001 From: mozhaa Date: Thu, 19 Mar 2026 14:10:36 +0300 Subject: [PATCH 2/5] done --- src/phg/sfm/ematrix.cpp | 211 ++++++++++++++++++--------------- src/phg/sfm/fmatrix.cpp | 216 +++++++++++++++++++--------------- src/phg/sfm/resection.cpp | 199 ++++++++++++++++++------------- src/phg/sfm/sfm_utils.cpp | 12 +- src/phg/sfm/triangulation.cpp | 26 +++- tests/test_sfm.cpp | 2 +- 6 files changed, 391 insertions(+), 275 deletions(-) diff --git a/src/phg/sfm/ematrix.cpp b/src/phg/sfm/ematrix.cpp index 3bc052b0..24a3c0e9 100644 --- a/src/phg/sfm/ematrix.cpp +++ b/src/phg/sfm/ematrix.cpp @@ -18,8 +18,26 @@ 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; + } + + 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 +46,9 @@ 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; + matrix3d E = calib1.K().t() * F * calib0.K(); + ensureSpectralProperty(E); + return E; } namespace { @@ -61,21 +76,22 @@ namespace { 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; + // скомпенсировать калибровки камер + 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; } } @@ -88,80 +104,81 @@ namespace { // первичное разложение существенной матрицы (а из него, взаимное расположение камер) для последующего уточнения методом нелинейной оптимизации 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; + + mat W(3, 3); + W << 0, -1, 0, 1, 0, 0, 0, 0, 1; + + mat R0 = U * W * V.transpose(); + mat R1 = U * W.transpose() * 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(); + + // 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 50127189..093277c7 100644 --- a/src/phg/sfm/fmatrix.cpp +++ b/src/phg/sfm/fmatrix.cpp @@ -25,49 +25,84 @@ namespace { // (см. Hartley & Zisserman p.279) 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; + int a_rows = count; + int a_cols = 9; + + 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.0; + } + + Eigen::JacobiSVD svda(A, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::VectorXd null_space = svda.matrixV().col(8); + + 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); + + Eigen::MatrixXd Uf = svdf.matrixU(); + Eigen::VectorXd sf = svdf.singularValues(); + Eigen::MatrixXd Vf = svdf.matrixV(); + + sf[2] = 0.0; + Eigen::MatrixXd Sf = Eigen::MatrixXd::Zero(3, 3); + Sf(0, 0) = sf[0]; + Sf(1, 1) = sf[1]; + F = Uf * Sf * Vf.transpose(); + + cv::Matx33d Fcv; + copy(F, Fcv); + + return Fcv; } // Нужно создать матрицу преобразования, которая сдвинет переданное множество точек так, что центр масс перейдет в ноль, а 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"); + cv::Vec2d centroid(0, 0); + for (const auto &pt : m) { + centroid += pt; + } + centroid /= (double)m.size(); + + double avg_dist = 0; + for (const auto &pt : m) { + avg_dist += cv::norm(pt - centroid); + } + avg_dist /= (double)m.size(); + + double scale = std::sqrt(2.0) / avg_dist; + + cv::Matx33d T = cv::Matx33d::zeros(); + T(0, 0) = scale; + T(1, 1) = scale; + T(0, 2) = -scale * centroid[0]; + T(1, 2) = -scale * centroid[1]; + T(2, 2) = 1; + return T; } cv::Vec2d transformPoint(const cv::Vec2d &pt, const cv::Matx33d &T) @@ -104,61 +139,58 @@ namespace { 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; + // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters + // будет отличаться от случая с гомографией + const int n_trials = 20000; + + const int n_samples = 8; + 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_norm = estimateFMatrixDLT(ms0, ms1, n_samples); + cv::Matx33d F = TN1.t() * F_norm * TN0; + + int support = 0; + for (int i = 0; i < n_matches; ++i) { + if (phg::epipolarTest(m0[i], m1[i], F, 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/resection.cpp b/src/phg/sfm/resection.cpp index d2cf6433..4e191cd6 100644 --- a/src/phg/sfm/resection.cpp +++ b/src/phg/sfm/resection.cpp @@ -49,95 +49,128 @@ 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(2 * count, 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; + + A(2 * i, 0) = X; + A(2 * i, 1) = Y; + A(2 * i, 2) = Z; + A(2 * i, 3) = W; + A(2 * i, 4) = 0; + A(2 * i, 5) = 0; + A(2 * i, 6) = 0; + A(2 * i, 7) = 0; + A(2 * i, 8) = -x * X; + A(2 * i, 9) = -x * Y; + A(2 * i, 10) = -x * Z; + A(2 * i, 11) = -x * W; + + A(2 * i + 1, 0) = 0; + A(2 * i + 1, 1) = 0; + A(2 * i + 1, 2) = 0; + A(2 * i + 1, 3) = 0; + A(2 * i + 1, 4) = X; + A(2 * i + 1, 5) = Y; + A(2 * i + 1, 6) = Z; + A(2 * i + 1, 7) = W; + A(2 * i + 1, 8) = -y * X; + A(2 * i + 1, 9) = -y * Y; + A(2 * i + 1, 10) = -y * Z; + A(2 * i + 1, 11) = -y * W; + } + + Eigen::JacobiSVD svd(A, Eigen::ComputeFullV); + vec p = svd.matrixV().col(11); + + matrix34d result; + for (int i = 0; i < 12; ++i) { + result(i / 4, i % 4) = p(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 = 5000; + + const double threshold_px = 3; + + const int n_samples = 6; + 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] = X[sample[i]]; + ms1[i] = calib.unproject(x[sample[i]]); + } + + cv::Matx34d P = estimateCameraMatrixDLT(ms0, ms1, n_samples); + + int support = 0; + for (int i = 0; i < n_points; ++i) { + cv::Vec4d Xhom(X[i][0], X[i][1], X[i][2], 1.0); + cv::Vec3d proj_norm = P * Xhom; + if (proj_norm[2] == 0) { + continue; + } + cv::Vec3d px_hom = calib.project(proj_norm); + cv::Vec2d px(px_hom[0] / px_hom[2], px_hom[1] / px_hom[2]); + 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; } diff --git a/src/phg/sfm/sfm_utils.cpp b/src/phg/sfm/sfm_utils.cpp index d2d2e294..52920c7e 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"); + cv::Vec3d p0(pt0[0], pt0[1], 1.0); + cv::Vec3d p1(pt1[0], pt1[1], 1.0); + cv::Vec3d l = F * p0; + + double numerator = std::abs(p1.dot(l)); + double denominator = std::sqrt(l[0] * l[0] + l[1] * l[1]); + if (denominator == 0) { + return false; + } + double distance = numerator / denominator; + return distance < t; } diff --git a/src/phg/sfm/triangulation.cpp b/src/phg/sfm/triangulation.cpp index 8dd11e69..9f895c30 100644 --- a/src/phg/sfm/triangulation.cpp +++ b/src/phg/sfm/triangulation.cpp @@ -12,5 +12,29 @@ cv::Vec4d phg::triangulatePoint(const cv::Matx34d *Ps, const cv::Vec3d *ms, int { // составление однородной системы + SVD // без подвохов - throw std::runtime_error("not implemented yet"); + Eigen::MatrixXd A(2 * count, 4); + + for (int i = 0; i < count; ++i) { + const cv::Matx34d &P = Ps[i]; + cv::Vec3d m = ms[i]; + double x = m[0], y = m[1], w = m[2]; + + cv::Matx P1 = P.row(0); + cv::Matx P2 = P.row(1); + cv::Matx P3 = P.row(2); + + A(2 * i, 0) = x * P3(0) - w * P1(0); + A(2 * i, 1) = x * P3(1) - w * P1(1); + A(2 * i, 2) = x * P3(2) - w * P1(2); + A(2 * i, 3) = x * P3(3) - w * P1(3); + + A(2 * i + 1, 0) = y * P3(0) - w * P2(0); + A(2 * i + 1, 1) = y * P3(1) - w * P2(1); + A(2 * i + 1, 2) = y * P3(2) - w * P2(2); + A(2 * i + 1, 3) = y * P3(3) - w * P2(3); + } + + Eigen::JacobiSVD svd(A, Eigen::ComputeFullV); + Eigen::VectorXd X = svd.matrixV().col(3); + return cv::Vec4d(X(0), X(1), X(2), X(3)); } diff --git a/tests/test_sfm.cpp b/tests/test_sfm.cpp index 4229b86b..48cc1588 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 { From 3d0da59e5c9e7cab701fbee396f56fccff4bba7d Mon Sep 17 00:00:00 2001 From: mozhaa Date: Thu, 19 Mar 2026 14:36:34 +0300 Subject: [PATCH 3/5] fix: ensure positive X(3) in triangulatePoint --- src/phg/sfm/triangulation.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/phg/sfm/triangulation.cpp b/src/phg/sfm/triangulation.cpp index 9f895c30..8aa08259 100644 --- a/src/phg/sfm/triangulation.cpp +++ b/src/phg/sfm/triangulation.cpp @@ -36,5 +36,8 @@ cv::Vec4d phg::triangulatePoint(const cv::Matx34d *Ps, const cv::Vec3d *ms, int Eigen::JacobiSVD svd(A, Eigen::ComputeFullV); Eigen::VectorXd X = svd.matrixV().col(3); + if (X(3) < 0) { + X = -X; + } return cv::Vec4d(X(0), X(1), X(2), X(3)); } From 0772632223c240f99b87810b90e3fffadcb9ac37 Mon Sep 17 00:00:00 2001 From: mozhaa Date: Thu, 19 Mar 2026 14:46:58 +0300 Subject: [PATCH 4/5] chore: increase n_trials --- src/phg/sfm/fmatrix.cpp | 2 +- src/phg/sfm/resection.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/phg/sfm/fmatrix.cpp b/src/phg/sfm/fmatrix.cpp index 093277c7..85b50260 100644 --- a/src/phg/sfm/fmatrix.cpp +++ b/src/phg/sfm/fmatrix.cpp @@ -141,7 +141,7 @@ namespace { } // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters // будет отличаться от случая с гомографией - const int n_trials = 20000; + const int n_trials = 50000; const int n_samples = 8; uint64_t seed = 1; diff --git a/src/phg/sfm/resection.cpp b/src/phg/sfm/resection.cpp index 4e191cd6..d3bd9ef2 100644 --- a/src/phg/sfm/resection.cpp +++ b/src/phg/sfm/resection.cpp @@ -115,7 +115,7 @@ namespace { // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters // будет отличаться от случая с гомографией - const int n_trials = 5000; + const int n_trials = 20000; const double threshold_px = 3; From 2e808ce29e301da901d93e9dc690e28f45032b08 Mon Sep 17 00:00:00 2001 From: mozhaa Date: Thu, 19 Mar 2026 15:15:30 +0300 Subject: [PATCH 5/5] chore: increase n_trials more --- src/phg/sfm/fmatrix.cpp | 2 +- src/phg/sfm/resection.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/phg/sfm/fmatrix.cpp b/src/phg/sfm/fmatrix.cpp index 85b50260..c940d09b 100644 --- a/src/phg/sfm/fmatrix.cpp +++ b/src/phg/sfm/fmatrix.cpp @@ -141,7 +141,7 @@ namespace { } // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters // будет отличаться от случая с гомографией - const int n_trials = 50000; + const int n_trials = 200000; const int n_samples = 8; uint64_t seed = 1; diff --git a/src/phg/sfm/resection.cpp b/src/phg/sfm/resection.cpp index d3bd9ef2..5147eedc 100644 --- a/src/phg/sfm/resection.cpp +++ b/src/phg/sfm/resection.cpp @@ -115,7 +115,7 @@ namespace { // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters // будет отличаться от случая с гомографией - const int n_trials = 20000; + const int n_trials = 50000; const double threshold_px = 3;