Task02 Евгений Новокшанов ИТМО ПОВС

src/phg/matching/descriptor_matcher.cpp

@@ -1,14 +1,22 @@
 #include "descriptor_matcher.h"
 
+#include <algorithm>
 #include <opencv2/flann/miniflann.hpp>
 #include "flann_factory.h"
 
 void phg::DescriptorMatcher::filterMatchesRatioTest(const std::vector<std::vector<cv::DMatch>> &matches,
                                                     std::vector<cv::DMatch> &filtered_matches)
 {
     filtered_matches.clear();
-
-    throw std::runtime_error("not implemented yet");
+    const float ratio = 0.7f;
+    for (const auto &m : matches) {
+        if (m.size() < 2) {
+            continue;
+        }
+        if (m[0].distance < ratio * m[1].distance) {
+            filtered_matches.push_back(m[0]);
+        }
+    }
 }
 
 
@@ -73,4 +81,62 @@ void phg::DescriptorMatcher::filterMatchesClusters(const std::vector<cv::DMatch>
 //
 //    метч остается, если левое и правое множества первых total_neighbors соседей в радиусах поиска(radius2_query, radius2_train) имеют как минимум consistent_matches общих элементов
 //    // TODO заполнить filtered_matches
+    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);
+
+    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, 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;
+    }
+
+    for (int i = 0; i < n_matches; ++i) {
+        std::vector<int> neigh_q;
+        std::vector<int> neigh_t;
+
+        neigh_q.reserve(total_neighbours);
+        neigh_t.reserve(total_neighbours);
+
+        for (size_t j = 0; j < total_neighbours; ++j) {
+            if (distances2_query.at<float>(i, (int)j) <= radius2_query) {
+                neigh_q.push_back(indices_query.at<int>(i, (int)j));
+            }
+            if (distances2_train.at<float>(i, (int)j) <= radius2_train) {
+                neigh_t.push_back(indices_train.at<int>(i, (int)j));
+            }
+        }
+
+        int common = 0;
+        for (int vq : neigh_q) {
+            if (std::find(neigh_t.begin(), neigh_t.end(), vq) != neigh_t.end()) {
+                ++common;
+            }
+        }
+
+        if (common >= (int)consistent_matches) {
+            filtered_matches.push_back(matches[i]);
+        }
+    }
 }

src/phg/matching/flann_matcher.cpp

@@ -1,4 +1,6 @@
 #include <iostream>
+#include <algorithm>
+#include <cmath>
 #include "flann_matcher.h"
 #include "flann_factory.h"
 
@@ -8,6 +10,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 +21,32 @@ 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");
+    if (!flann_index) {
+        throw std::runtime_error("FlannMatcher:: knnMatch : matcher is not trained");
+    }
+    if (k <= 0) {
+        throw std::runtime_error("FlannMatcher:: knnMatch : k must be > 0");
+    }
+    if (query_desc.empty()) {
+        matches.clear();
+        return;
+    }
+
+    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 (int i = 0; i < query_desc.rows; ++i) {
+        matches[i].clear();
+        matches[i].reserve(k);
+        for (int j = 0; j < k; ++j) {
+            cv::DMatch m;
+            m.queryIdx = i;
+            m.trainIdx = indices.at<int>(i, j);
+            m.imgIdx = 0;
+            m.distance = std::sqrt(std::max(0.f, distances2.at<float>(i, j)));
+            matches[i].push_back(m);
+        }
+    }
 }

src/phg/sfm/homography.cpp

@@ -2,6 +2,9 @@
 
 #include <opencv2/calib3d/calib3d.hpp>
 #include <iostream>
+#include <algorithm>
+#include <cmath>
+#include <limits>
 
 namespace {
 
@@ -86,6 +89,8 @@ namespace {
             // 8 elements of matrix + free term as needed by gauss routine
 //            A.push_back({TODO});
 //            A.push_back({TODO});
+            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);
@@ -219,6 +224,83 @@ namespace {
 //        }
 //
 //        return best_H;
+        const int n_matches = points_lhs.size();
+        if (n_matches < 4) {
+            throw std::runtime_error("estimateHomographyRANSAC : too few points");
+        }
+
+        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<int> sample;
+        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;
+            }
+
+            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");
+        }
+
+        std::vector<cv::Point2f> in_lhs;
+        std::vector<cv::Point2f> in_rhs;
+        in_lhs.reserve(n_matches);
+        in_rhs.reserve(n_matches);
+        for (int i_point = 0; i_point < n_matches; ++i_point) {
+            cv::Point2d proj = phg::transformPoint(points_lhs[i_point], best_H);
+            if (cv::norm(proj - cv::Point2d(points_rhs[i_point])) < reprojection_error_threshold_px) {
+                in_lhs.push_back(points_lhs[i_point]);
+                in_rhs.push_back(points_rhs[i_point]);
+            }
+        }
+
+        if ((int)in_lhs.size() >= 4) {
+            cv::Mat refined_H = cv::findHomography(in_lhs, in_rhs, 0);
+            if (!refined_H.empty()) {
+                return refined_H;
+            }
+        }
+
+        return best_H;
     }
 
 }
@@ -238,7 +320,29 @@ 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");
+    if (T.rows != 3 || T.cols != 3) {
+        throw std::runtime_error("transformPoint : invalid matrix size");
+    }
+
+    cv::Mat T64;
+    if (T.type() == CV_64F) {
+        T64 = T;
+    } else {
+        T.convertTo(T64, CV_64F);
+    }
+
+    const double x = pt.x;
+    const double y = pt.y;
+
+    const double tx = T64.at<double>(0, 0) * x + T64.at<double>(0, 1) * y + T64.at<double>(0, 2);
+    const double ty = T64.at<double>(1, 0) * x + T64.at<double>(1, 1) * y + T64.at<double>(1, 2);
+    const double tw = T64.at<double>(2, 0) * x + T64.at<double>(2, 1) * y + T64.at<double>(2, 2);
+
+    if (std::abs(tw) < 1e-12) {
+        throw std::runtime_error("transformPoint : invalid homogeneous coordinate");
+    }
+
+    return {tx / tw, ty / tw};
 }
 
 cv::Point2d phg::transformPointCV(const cv::Point2d &pt, const cv::Mat &T) {

src/phg/sfm/panorama_stitcher.cpp

@@ -23,7 +23,39 @@ cv::Mat phg::stitchPanorama(const std::vector<cv::Mat> &imgs,
     {
         // здесь надо посчитать вектор 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()");
+        }
+
+        std::vector<int> state(n_images, 0);
+
+        std::function<void(int)> build = [&](int i) {
+            if (state[i] == 2) {
+                return;
+            }
+            if (state[i] == 1) {
+                throw std::runtime_error("stitchPanorama: cycle in parent graph");
+            }
+            state[i] = 1;
+
+            if (parent[i] == -1) {
+                Hs[i] = cv::Mat::eye(3, 3, CV_64FC1);
+            } else {
+                int p = parent[i];
+                if (p < 0 || p >= n_images) {
+                    throw std::runtime_error("stitchPanorama: invalid parent id");
+                }
+                build(p);
+                cv::Mat H_to_parent = homography_builder(imgs[i], imgs[p]);
+                Hs[i] = Hs[p] * H_to_parent;
+            }
+
+            state[i] = 2;
+        };
+
+        for (int i = 0; i < n_images; ++i) {
+            build(i);
+        }
     }
 
     bbox2<double, cv::Point2d> bbox;