Task02 Егор Машин ИТМО

src/phg/matching/descriptor_matcher.cpp

@@ -7,8 +7,21 @@ void phg::DescriptorMatcher::filterMatchesRatioTest(const std::vector<std::vecto
                                                     std::vector<cv::DMatch> &filtered_matches)
 {
     filtered_matches.clear();
-
-    throw std::runtime_error("not implemented yet");
+
+    const float ratio_threshold = 0.75f;
+
+    for (const auto& match_pair : matches) {
+        if (match_pair.size() < 2) continue;
+
+        float dist1 = match_pair[0].distance;
+        float dist2 = match_pair[1].distance;
+
+        if (dist2 == 0) continue;
+
+        if (dist1 < ratio_threshold * dist2) {
+            filtered_matches.push_back(match_pair[0]);
+        }
+    }
 }
 
 
@@ -32,45 +45,69 @@ void phg::DescriptorMatcher::filterMatchesClusters(const std::vector<cv::DMatch>
     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<cv::Point2f>(i) = keypoints_query[matches[i].queryIdx].pt;
-        points_train.at<cv::Point2f>(i) = keypoints_train[matches[i].trainIdx].pt;
+    points_query.at<float>(i, 0) = keypoints_query[matches[i].queryIdx].pt.x;
+    points_query.at<float>(i, 1) = keypoints_query[matches[i].queryIdx].pt.y;
+    points_train.at<float>(i, 0) = keypoints_train[matches[i].trainIdx].pt.x;
+    points_train.at<float>(i, 1) = keypoints_train[matches[i].trainIdx].pt.y;
     }
 //
 //    // размерность всего 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::IndexParams> index_params = flannKdTreeIndexParams(2);
+    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);
+    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);
+    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);
+    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;
-//    }
+    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
+    for (int i = 0; i < n_matches; ++i) {
+        std::vector<int> valid_query_neighbors;
+        for (int j = 0; j < total_neighbours; ++j) {
+            if (distances2_query.at<float>(i, j) <= radius2_query) {
+                valid_query_neighbors.push_back(indices_query.at<int>(i, j));
+            }
+        }
+
+        int shared_count = 0;
+        for (int j = 0; j < total_neighbours; ++j) {
+            if (distances2_train.at<float>(i, j) <= radius2_train) {
+                int train_neighbor_idx = indices_train.at<int>(i, j);
+
+                if (std::find(valid_query_neighbors.begin(), valid_query_neighbors.end(), train_neighbor_idx) != valid_query_neighbors.end()) {
+                    shared_count++;
+                }
+            }
+        }
+
+        if (shared_count >= consistent_matches) {
+            filtered_matches.push_back(matches[i]);
+        }
+}
 }

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,30 @@ 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");
+    matches.clear();
+
+    if (query_desc.empty() || !flann_index) {
+        return; 
+    }
+
+    cv::Mat indices(query_desc.rows, k, CV_32S);
+    cv::Mat distances(query_desc.rows, k, CV_32F);
+
+    flann_index->knnSearch(query_desc, indices, distances, k, *search_params);
+
+    matches.resize(query_desc.rows);
+
+    for (int i = 0; i < query_desc.rows; ++i) {
+        matches[i].reserve(k);
+
+        const int* indices_ptr = indices.ptr<int>(i);
+        const float* distances_ptr = distances.ptr<float>(i);
+
+        for (int j = 0; j < k; ++j) {
+            int trainIdx = indices_ptr[j];
+            if (trainIdx >= 0) {
+                matches[i].emplace_back(i, trainIdx, std::sqrt(distances_ptr[j]));
+            }
+        }
+    }
 }

src/phg/sfm/homography.cpp

@@ -1,5 +1,5 @@
 #include "homography.h"
-
+#include <cmath>
 #include <opencv2/calib3d/calib3d.hpp>
 #include <iostream>
 
@@ -84,8 +84,9 @@ 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);
@@ -157,70 +158,101 @@ namespace {
     }
 
     cv::Mat estimateHomographyRANSAC(const std::vector<cv::Point2f> &points_lhs, const std::vector<cv::Point2f> &points_rhs)
-    {
-        if (points_lhs.size() != points_rhs.size()) {
-            throw std::runtime_error("findHomography: points_lhs.size() != points_rhs.size()");
+{
+    if (points_lhs.size() != points_rhs.size()) {
+        throw std::runtime_error("findHomography: points_lhs.size() != points_rhs.size()");
+    }
+
+    const int n_matches = points_lhs.size();
+    if (n_matches < 4) {
+        throw std::runtime_error("estimateHomographyRANSAC: not enough points");
+    }
+
+    if (n_matches == 4) {
+        return estimateHomography4Points(points_lhs[0], points_lhs[1], points_lhs[2], points_lhs[3],
+                                         points_rhs[0], points_rhs[1], points_rhs[2], points_rhs[3]);
+    }
+
+    double min_x = points_rhs[0].x, max_x = points_rhs[0].x;
+    double min_y = points_rhs[0].y, max_y = points_rhs[0].y;
+    for (const auto& p : points_rhs) {
+        min_x = std::min(min_x, (double)p.x); max_x = std::max(max_x, (double)p.x);
+        min_y = std::min(min_y, (double)p.y); max_y = std::max(max_y, (double)p.y);
+    }
+    double target_area = (max_x - min_x) * (max_y - min_y);
+    if (target_area < 1.0) target_area = 1e6;
+
+    const int n_trials = 5000;
+    const int n_samples = 4;
+    uint64_t seed = 1;
+
+    double best_log_nfa = std::numeric_limits<double>::max();
+    cv::Mat best_H;
+    int best_support = 0;
+
+    double log_N_minus_4 = std::log(std::max(1, n_matches - 4));
+    double lgamma_n_plus_1 = std::lgamma(n_matches + 1.0);
+    double lgamma_4_plus_1 = std::lgamma(n_samples + 1.0);
+
+    std::vector<int> sample;
+    std::vector<double> errors(n_matches);
+
+    for (int i_trial = 0; i_trial < n_trials; ++i_trial) {
+        randomSample(sample, n_matches, n_samples, &seed);
+
+        cv::Mat H;
+        try {
+            H = estimateHomography4Points(points_lhs[sample[0]], points_lhs[sample[1]], points_lhs[sample[2]], points_lhs[sample[3]],
+                                          points_rhs[sample[0]], points_rhs[sample[1]], points_rhs[sample[2]], points_rhs[sample[3]]);
+        } catch (...) {
+            continue;
         }
 
-        // 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<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;
+        for (int i = 0; i < n_matches; ++i) {
+            try {
+                cv::Point2d proj = phg::transformPoint(points_lhs[i], H);
+                cv::Point2d diff = proj - cv::Point2d(points_rhs[i]);
+                errors[i] = diff.x * diff.x + diff.y * diff.y;
+            } catch (...) {
+                errors[i] = std::numeric_limits<double>::max();
+            }
+        }
+
+        std::vector<double> sorted_errors = errors;
+        std::sort(sorted_errors.begin(), sorted_errors.end());
+
+        for (int k = 5; k <= n_matches; ++k) {
+            double e2 = sorted_errors[k - 1];
+
+            if (e2 >= std::numeric_limits<double>::max() / 2.0) break;
+
+            double alpha = (CV_PI * e2) / target_area;
+            if (alpha >= 1.0) continue;
+            alpha = std::max(alpha, 1e-12);
+
+            double lgamma_k_plus_1 = std::lgamma(k + 1.0);
+            double log_binom_n_k = lgamma_n_plus_1 - lgamma_k_plus_1 - std::lgamma(n_matches - k + 1.0);
+            double log_binom_k_4 = lgamma_k_plus_1 - lgamma_4_plus_1 - std::lgamma(k - n_samples + 1.0);
+
+            double log_nfa = log_N_minus_4 + log_binom_n_k + log_binom_k_4 + (k - n_samples) * std::log(alpha);
+
+            if (log_nfa < best_log_nfa) {
+                best_log_nfa = log_nfa;
+                best_H = H;
+                best_support = k;
+            }
+        }
+    }
+
+    if (best_log_nfa > 0) {
+        throw std::runtime_error("estimateHomographyRANSAC : failed to estimate homography");
     }
 
+    std::cout << "estimateHomographyRANSAC : support: " << best_support << "/" << n_matches << std::endl;
+
+    return best_H;
+}
+
 }
 
 cv::Mat phg::findHomography(const std::vector<cv::Point2f> &points_lhs, const std::vector<cv::Point2f> &points_rhs)
@@ -238,7 +270,17 @@ 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");
+    double x = pt.x, y = pt.y;
+    double w = T.at<double>(2,0) * x + T.at<double>(2,1) * y + T.at<double>(2,2);
+
+    if (std::abs(w) < 1e-8) {
+        throw std::runtime_error("transformPoint: division by zero");
+    }
+
+    return cv::Point2d(
+        (T.at<double>(0,0) * x + T.at<double>(0,1) * y + T.at<double>(0,2)) / w,
+        (T.at<double>(1,0) * x + T.at<double>(1,1) * y + T.at<double>(1,2)) / w
+    );
 }
 
 cv::Point2d phg::transformPointCV(const cv::Point2d &pt, const cv::Mat &T) {

src/phg/sfm/panorama_stitcher.cpp

@@ -21,9 +21,28 @@ cv::Mat phg::stitchPanorama(const std::vector<cv::Mat> &imgs,
     // вектор гомографий, для каждой картинки описывает преобразование до корня
     std::vector<cv::Mat> Hs(n_images);
     {
-        // здесь надо посчитать вектор Hs
-        // при этом можно обойтись n_images - 1 вызовами функтора homography_builder
-        throw std::runtime_error("not implemented yet");
+        std::vector<bool> computed(n_images, false);
+        std::function<void(int)> compute_H = [&](int i) {
+            if (computed[i]) return;
+
+            if (parent[i] == -1) {
+                Hs[i] = cv::Mat::eye(3, 3, CV_64F);
+                computed[i] = true;
+                return;
+            }
+
+            compute_H(parent[i]);
+
+            cv::Mat H_local = homography_builder(imgs[i], imgs[parent[i]]);
+
+            Hs[i] = Hs[parent[i]] * H_local;
+
+            computed[i] = true;
+        };
+
+        for (int i = 0; i < n_images; ++i) {
+            compute_H(i);
+        }
     }
 
     bbox2<double, cv::Point2d> bbox;