PhotogrammetryCourse · ivan0sokin · Mar 18, 2026 · Mar 18, 2026 · Mar 18, 2026 · Mar 18, 2026
diff --git a/.gitignore b/.gitignore
@@ -2,3 +2,5 @@
 .idea
 build
 cmake-build*
+.cache
+data/debug
diff --git a/src/phg/matching/cl/bruteforce_matcher.cl b/src/phg/matching/cl/bruteforce_matcher.cl
@@ -22,6 +22,7 @@ __kernel void bruteforce_matcher(__global const float* train,
 
     // храним KEYPOINTS_PER_WG=4 дескриптора-query:
     __local float query_local[KEYPOINTS_PER_WG * NDIM];
+    __local float dist2_for_reduction[NDIM];
     // храним два лучших сопоставления для каждого дескриптора-query:
     __local uint  res_train_idx_local[KEYPOINTS_PER_WG * 2];
     __local float res_distance2_local[KEYPOINTS_PER_WG * 2]; // храним квадраты чтобы не считать корень до самого последнего момента
@@ -33,6 +34,11 @@ __kernel void bruteforce_matcher(__global const float* train,
     // грузим 4 дескриптора-query (для каждого из четырех дескрипторов каждый поток грузит значение своей размерности dim_id)
     // TODO: т.е. надо прогрузить в query_local все KEYPOINTS_PER_WG=4 дескриптора из query (начиная с индекса query_id0) (а если часть из них выходит за пределы n_query_desc - грузить нули)
 
+    for (int query_local_i = 0; query_local_i < KEYPOINTS_PER_WG; query_local_i++) {
+        int query_id = query_id0 + query_local_i;
+        query_local[query_local_i * NDIM + dim_id] = query_id < n_query_desc ? query[query_id * NDIM + dim_id] : 0.f;
+    }
+
     barrier(CLK_LOCAL_MEM_FENCE); // дожидаемся прогрузки наших дескрипторов-запросов
 
     for (int train_idx = 0; train_idx < n_train_desc; ++train_idx) {
@@ -43,13 +49,15 @@ __kernel void bruteforce_matcher(__global const float* train,
             // до дескриптора-train в глобальной памяти (#train_idx)
 
             // TODO посчитать квадрат расстояния по нашей размерности (dim_id) и сохранить его в нашу ячейку в dist2_for_reduction
+            float d = query_local[query_local_i * NDIM + dim_id] - train_value_dim;
+            dist2_for_reduction[dim_id] = d * d;
 
             barrier(CLK_LOCAL_MEM_FENCE);
             // TODO суммируем редукцией все что есть в dist2_for_reduction
             int step = NDIM / 2;
             while (step > 0) {
                 if (dim_id < step) {
-                    // TODO
+                    dist2_for_reduction[dim_id] += dist2_for_reduction[dim_id + step];
                 }
                 barrier(CLK_LOCAL_MEM_FENCE);
                 step /= 2;
@@ -63,28 +71,34 @@ __kernel void bruteforce_matcher(__global const float* train,
                 #define SECOND_BEST_INDEX 1
 
                 // пытаемся улучшить самое лучшее сопоставление для локального дескриптора
-                if (dist2 <= res_distance2_local[query_local_i * 2 + BEST_INDEX]) {
+                if (dist2 < res_distance2_local[query_local_i * 2 + BEST_INDEX]) {
                     // не забываем что прошлое лучшее сопоставление теперь стало вторым по лучшевизне (на данный момент)
                     res_distance2_local[query_local_i * 2 + SECOND_BEST_INDEX] = res_distance2_local[query_local_i * 2 + BEST_INDEX];
                     res_train_idx_local[query_local_i * 2 + SECOND_BEST_INDEX] = res_train_idx_local[query_local_i * 2 + BEST_INDEX];
                     // TODO заменяем нашим (dist2, train_idx) самое лучшее сопоставление для локального дескриптора
-                } else if (dist2 <= res_distance2_local[query_local_i * 2 + SECOND_BEST_INDEX]) { // может мы улучшили хотя бы второе по лучшевизне сопоставление?
+                    res_distance2_local[query_local_i * 2 + BEST_INDEX] = dist2;
+                    res_train_idx_local[query_local_i * 2 + BEST_INDEX] = train_idx;
+                } else if (dist2 < res_distance2_local[query_local_i * 2 + SECOND_BEST_INDEX]) { // может мы улучшили хотя бы второе по лучшевизне сопоставление?
                     // TODO заменяем второе по лучшевизне сопоставление для локального дескриптора
+                    res_distance2_local[query_local_i * 2 + SECOND_BEST_INDEX] = dist2;
+                    res_train_idx_local[query_local_i * 2 + SECOND_BEST_INDEX] = train_idx;
                 }
             }
         }
     }
 
+    barrier(CLK_LOCAL_MEM_FENCE);
+
     // итак, мы нашли два лучших сопоставления для наших KEYPOINTS_PER_WG дескрипторов, надо сохрнить эти результаты в глобальную память
     if (dim_id < KEYPOINTS_PER_WG * 2) { // полагаемся на то что нам надо прогрузить KEYPOINTS_PER_WG*2==4*2<dim_id<=NDIM==128
         int query_local_i = dim_id / 2;
         int k = dim_id % 2;
 
         int query_id = query_id0 + query_local_i;
         if (query_id < n_query_desc) {
-            res_train_idx[query_id * 2 + k] = // TODO
-            res_query_idx[query_id * 2 + k] = // TODO хм, не масло масленное ли?
-            res_distance [query_id * 2 + k] = // TODO не забудьте извлечь корень
+            res_train_idx[query_id * 2 + k] = res_train_idx_local[query_local_i * 2 + k];
+            res_query_idx[query_id * 2 + k] = query_id;
+            res_distance [query_id * 2 + k] = sqrt(res_distance2_local[query_local_i * 2 + k]);
         }
     }
 }
diff --git a/src/phg/matching/descriptor_matcher.cpp b/src/phg/matching/descriptor_matcher.cpp
@@ -2,13 +2,19 @@
 
 #include <opencv2/flann/miniflann.hpp>
 #include "flann_factory.h"
+#include <unordered_set>
 
 void phg::DescriptorMatcher::filterMatchesRatioTest(const std::vector<std::vector<cv::DMatch>> &matches,
                                                     std::vector<cv::DMatch> &filtered_matches)
 {
     filtered_matches.clear();
+    const float threshold2 = 0.705f * 0.705f;
 
-    throw std::runtime_error("not implemented yet");
+    for (const auto& knn : matches) {
+        if (knn.size() >= 2 && knn[0].distance < threshold2 * knn[1].distance) {
+            filtered_matches.push_back(knn[0]);
+        }
+    }
 }
 
 
@@ -35,42 +41,65 @@ void phg::DescriptorMatcher::filterMatchesClusters(const std::vector<cv::DMatch>
         points_query.at<cv::Point2f>(i) = keypoints_query[matches[i].queryIdx].pt;
         points_train.at<cv::Point2f>(i) = keypoints_train[matches[i].trainIdx].pt;
     }
-//
-//    // размерность всего 2, так что точное KD-дерево
-//    std::shared_ptr<cv::flann::IndexParams> index_params = flannKdTreeIndexParams(TODO);
-//    std::shared_ptr<cv::flann::SearchParams> search_params = flannKsTreeSearchParams(TODO);
-//
-//    std::shared_ptr<cv::flann::Index> index_query = flannKdTreeIndex(points_query, index_params);
-//    std::shared_ptr<cv::flann::Index> 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<double> max_dists2_query(n_matches);
-//        std::vector<double> max_dists2_train(n_matches);
-//        for (int i = 0; i < n_matches; ++i) {
-//            max_dists2_query[i] = distances2_query.at<float>(i, total_neighbours - 1);
-//            max_dists2_train[i] = distances2_train.at<float>(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;
-//    }
-//
+
+    // размерность всего 2, так что точное KD-дерево
+    std::shared_ptr<cv::flann::IndexParams> index_params = flannKdTreeIndexParams(1);
+    std::shared_ptr<cv::flann::SearchParams> search_params = flannKsTreeSearchParams(32);
+
+    std::shared_ptr<cv::flann::Index> index_query = flannKdTreeIndex(points_query, index_params);
+    std::shared_ptr<cv::flann::Index> 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<double> max_dists2_query(n_matches);
+        std::vector<double> max_dists2_train(n_matches);
+        for (int i = 0; i < n_matches; ++i) {
+            max_dists2_query[i] = distances2_query.at<float>(i, total_neighbours - 1);
+            max_dists2_train[i] = distances2_train.at<float>(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
+
+    std::unordered_set<int> query_neighbors;
+    for (size_t matchIdx = 0; matchIdx < n_matches; matchIdx++) {
+        for (size_t neighborIdx = 0; neighborIdx < total_neighbours; neighborIdx++) {
+            if (distances2_query.at<float>(matchIdx, neighborIdx) <= radius2_query) {
+                query_neighbors.insert(indices_query.at<int>(matchIdx, neighborIdx));
+            }
+        }
+
+        size_t count = 0;
+        for (size_t neighborIdx = 0; neighborIdx < total_neighbours; neighborIdx++) {
+            if (distances2_train.at<float>(matchIdx, neighborIdx) <= radius2_train &&
+                query_neighbors.find(indices_train.at<int>(matchIdx, neighborIdx)) != query_neighbors.end()) {
+                count++;
+            }
+        }
+
+        if (count >= consistent_matches) {
+            filtered_matches.emplace_back(matches[matchIdx]);
+        }
+
+        query_neighbors.clear();
+    }
 }
diff --git a/src/phg/matching/flann_matcher.cpp b/src/phg/matching/flann_matcher.cpp
@@ -1,13 +1,15 @@
 #include <iostream>
 #include "flann_matcher.h"
 #include "flann_factory.h"
+#include "opencv2/core/hal/interface.h"
+#include "opencv2/core/types.hpp"
 
 
 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 +19,25 @@ void phg::FlannMatcher::train(const cv::Mat &train_desc)
 
 void phg::FlannMatcher::knnMatch(const cv::Mat &query_desc, std::vector<std::vector<cv::DMatch>> &matches, int k) const
 {
-    throw std::runtime_error("not implemented yet");
+    cv::Mat indices(query_desc.rows, k, CV_32SC1);
+    cv::Mat distances2(query_desc.rows, k, CV_32FC1);
+
+    flann_index->knnSearch(
+        query_desc,
+        indices,
+        distances2,
+        k,
+        *search_params
+    );
+
+    matches.resize(query_desc.rows);
+    for (size_t rowIdx = 0; rowIdx < query_desc.rows; rowIdx++) {
+        for (size_t idx = 0; idx < k; idx++) {
+            matches[rowIdx].emplace_back(
+                rowIdx,
+                indices.at<int>(rowIdx, idx),
+                distances2.at<float>(rowIdx, idx)
+            );
+        }
+    }
 }
diff --git a/src/phg/sfm/homography.cpp b/src/phg/sfm/homography.cpp
@@ -2,6 +2,7 @@
 
 #include <opencv2/calib3d/calib3d.hpp>
 #include <iostream>
+#include <opencv2/core/types.hpp>
 
 namespace {
 
@@ -84,8 +85,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({0.0, 0.0, 0.0, -x0 * w1, -y0 * w1, -w0 * w1, x0 * y1, y0 * y1, -w0 * y1});
+           A.push_back({x0 * w1, y0 * w1, w0 * w1, 0.0, 0.0, 0.0, -x0 * x1, -y0 * x1, w0 * x1});
         }
 
         int res = gauss(A, H);
@@ -168,57 +169,59 @@ 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<int> 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 double p = 0.99;
+       const double w = 0.5;
+       const int n_trials = std::round(std::log(1.0 - p) / std::log(1 - std::pow(w, 4.0)));
+
+       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<int> 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 +241,11 @@ cv::Mat phg::findHomographyCV(const std::vector<cv::Point2f> &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");
+    cv::Mat out = T * cv::Mat(cv::Point3d(pt.x, pt.y, 1.0));
+    return {
+        out.at<double>(0) / out.at<double>(2),
+        out.at<double>(1) / out.at<double>(2)
+    };
 }
 
 cv::Point2d phg::transformPointCV(const cv::Point2d &pt, const cv::Mat &T) {
-Original file line number
+Diff line change
@@ Expand Up / @@ -2,3 +2,5 @@ @@
     .idea
     build
     cmake-build*
+    .cache
+    data/debug